/* USER CODE BEGIN Includes */ #include <stdio.h> #include "uartRingBufDMA.h" #include…
- User
/* USER CODE BEGIN Includes */ #include <stdio.h> #include "uartRingBufDMA.h" #include "stdbool.h" #include "stdlib.h" #include "wheelslib.h" #include "string.h" #include "pid.h"
extern uint16_t Head, Tail; extern uint8_t RxBuf[RxBuf_SIZE]; extern uint8_t MainBuf[MainBuf_SIZE];
#define MAX_BUTTONS 8 #define Enc_A_Pin GPIO_PIN_0 #define Enc_B_Pin GPIO_PIN_1 #define Enc_C_Pin GPIO_PIN_8 #define Enc_D_Pin GPIO_PIN_9
volatile int32_t encA_count = 0; volatile int32_t encB_count = 0; volatile int32_t encC_count = 0; volatile int32_t encD_count = 0;
typedef struct { volatile int32_t *counter; uint16_t pin; } EncSensor;
EncSensor sensors[4] = { {&encA_count, Enc_A_Pin}, {&encB_count, Enc_B_Pin}, {&encC_count, Enc_C_Pin}, {&encD_count, Enc_D_Pin} };
uint8_t pwmIndex = 0; char buffer[100]; uint32_t pwmData_CH1[10] = {0}; uint32_t pwmData_CH2[10] = {0}; uint32_t pwmData_CH3[10] = {0}; uint32_t pwmData_CH4[10] = {0};
// Структуры данных для очереди typedef enum { PACKET_TYPE_BUTTON = 0xF0, PACKET_TYPE_JOYSTICK = 0xF1 } PacketType;
typedef struct { PacketType type; union { struct { uint8_t id; uint8_t state; } button; struct { uint8_t x; uint8_t y; uint8_t power; } joystick; } data; } PacketMessage;
// Дескриптор очереди пакетов osMessageQueueId_t packetQueueHandle; #define PACKET_QUEUE_SIZE 16
/**************************************/ // Для отладки volatile uint32_t total_bytes_received = 0; volatile uint32_t valid_packets_received = 0; volatile uint32_t button_packets_received = 0; volatile uint32_t joystick_packets_received = 0; volatile uint32_t invalid_packets_detected = 0; volatile uint32_t dropped_packets = 0; volatile uint8_t last_joystick_data[3] = {0}; volatile uint8_t last_button_data[2] = {0};
int16_t joystickPower = 0; char joystickDir = 'F'; /* USER CODE END Includes */
/* USER CODE BEGIN PV */ // флаг для управления режимом работы typedef enum { CONTROL_MODE_DIRECT, CONTROL_MODE_PID } ControlMode;
ControlMode currentControlMode = CONTROL_MODE_DIRECT;
// Структура для хранения желаемых скоростей (setpoints) typedef struct { double speedFL; double speedFR; double speedBL; double speedBR; bool isActive; } TargetSpeeds;
TargetSpeeds targetSpeeds = {0, 0, 0, 0, false};
// PID структуры для каждого колеса PID_TypeDef pidFL, pidFR, pidBL, pidBR;
// Заданные скорости для каждого колеса (setpoints) double setpointFL = 0, setpointFR = 0, setpointBL = 0, setpointBR = 0; double currentSpeedFL = 0, currentSpeedFR = 0, currentSpeedBL = 0, currentSpeedBR = 0; double pidOutputFL = 0, pidOutputFR = 0, pidOutputBL = 0, pidOutputBR = 0;
// Константы PID - делаем более консервативными #define KP 1.0f // Вместо 0.2f #define KI 0.05f // Вместо 0.01f #define KD 0.01f // Можно оставить #define MOTOR_DEADZONE_MIN 1 #define MOTOR_DEADZONE_MAX 5 #define PID_UPDATE_PERIOD 10
bool pidDebugEnabled = true; int8_t ticks_per_rev = 20; // Количество тактов кулера в один оборот. float rpm = 0.0;
// Для расчета скорости - добавляем более детальную диагностику volatile int32_t lastEncA = 0, lastEncB = 0, lastEncC = 0, lastEncD = 0; float ticksToSpeed = 10.0f;
// Добавляем диагностические переменные volatile uint32_t pidComputeCount = 0; volatile uint32_t pidLastComputeTime = 0; volatile bool pidInitialized = false;
MotorsState motors; /* USER CODE END PV */
/* USER CODE BEGIN PFP */ int _write(int file, char ptr, int len) { int i; HAL_UART_Transmit(&huart6, (uint8_t) ptr, len, 50); for (i = 0; i < len; i++) { ITM_SendChar(*ptr++); } return len; }
void process_uart_data(void); /* USER CODE END PFP */
/* USER CODE BEGIN 0 */ // Определение состояний FSM для обработки данных из UART typedef enum { STATE_WAIT_HEADER, STATE_COLLECT_BUTTON, STATE_COLLECT_JOYSTICK } RxState;
void UART_ProcessByte(uint8_t byte) { static RxState state = STATE_WAIT_HEADER; static PacketMessage packet; static uint8_t byteCounter = 0;
total_bytes_received++; switch (state) { case STATE_WAIT_HEADER: if (byte == PACKET_TYPE_BUTTON) { packet.type = PACKET_TYPE_BUTTON; state = STATE_COLLECT_BUTTON; byteCounter = 0; } else if (byte == PACKET_TYPE_JOYSTICK) { packet.type = PACKET_TYPE_JOYSTICK; state = STATE_COLLECT_JOYSTICK; byteCounter = 0; } else { invalid_packets_detected++; } break; case STATE_COLLECT_BUTTON: if (byteCounter == 0) { packet.data.button.id = byte; byteCounter++; } else if (byteCounter == 1) { packet.data.button.state = byte; if (osMessageQueuePut(packetQueueHandle, &packet, 0, 0) == osOK) { button_packets_received++; valid_packets_received++; last_button_data[0] = packet.data.button.id; last_button_data[1] = packet.data.button.state; } else { dropped_packets++; } state = STATE_WAIT_HEADER; } break; case STATE_COLLECT_JOYSTICK: if (byteCounter == 0) { packet.data.joystick.x = byte; byteCounter++; } else if (byteCounter == 1) { packet.data.joystick.y = byte; byteCounter++; } else if (byteCounter == 2) { packet.data.joystick.power = byte; if (osMessageQueuePut(packetQueueHandle, &packet, 0, 0) == osOK) { joystick_packets_received++; valid_packets_received++; last_joystick_data[0] = packet.data.joystick.x; last_joystick_data[1] = packet.data.joystick.y; last_joystick_data[2] = packet.data.joystick.power; } else { dropped_packets++; } state = STATE_WAIT_HEADER; } break; }}
void InitializePID(void) { printf("=== INITIALIZING PID CONTROLLERS ===\r\n");
// Инициализация PID-регуляторов для каждого колеса printf("Initializing PID for FL wheel...\r\n"); PID(&pidFL, ¤tSpeedFL, &pidOutputFL, &setpointFL, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT); PID(&pidFR, ¤tSpeedFR, &pidOutputFR, &setpointFR, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT); PID(&pidBL, ¤tSpeedBL, &pidOutputBL, &setpointBL, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT); PID(&pidBR, ¤tSpeedBR, &pidOutputBR, &setpointBR, KP, KI, KD, _PID_P_ON_E, _PID_CD_DIRECT); // ИСПРАВЛЕНИЕ: Устанавливаем правильные ограничения выхода printf("Setting output limits to ±15...\r\n"); // Исправляем лог PID_SetOutputLimits(&pidFL, -15, 15); // Уменьшаем с ±80 до ±15 PID_SetOutputLimits(&pidFR, -15, 15); PID_SetOutputLimits(&pidBL, -15, 15); PID_SetOutputLimits(&pidBR, -15, 15); // Включаем автоматический режим PID_SetMode(&pidFL, _PID_MODE_AUTOMATIC); PID_SetMode(&pidFR, _PID_MODE_AUTOMATIC); PID_SetMode(&pidBL, _PID_MODE_AUTOMATIC); PID_SetMode(&pidBR, _PID_MODE_AUTOMATIC); // Устанавливаем время семплирования PID_SetSampleTime(&pidFL, PID_UPDATE_PERIOD); PID_SetSampleTime(&pidFR, PID_UPDATE_PERIOD); PID_SetSampleTime(&pidBL, PID_UPDATE_PERIOD); PID_SetSampleTime(&pidBR, PID_UPDATE_PERIOD); pidInitialized = true; printf("=== PID CONTROLLERS INITIALIZED SUCCESSFULLY ===\r\n"); printf("PID Parameters: Kp=%.2f, Ki=%.2f, Kd=%.2f\r\n", KP, KI, KD); printf("Output limits: ±15, PWM range: %d-%d\r\n", MOTOR_DEADZONE_MIN, MOTOR_DEADZONE_MAX);}
uint32_t CompensateDeadzone(double pidOutput) { double absOutput = fabs(pidOutput); if (absOutput < 1.0) return 0; if (absOutput > 15.0) absOutput = 15.0;
// Линейное масштабирование без сложных расчетов return (uint32_t)(absOutput * (35.0 / 15.0));}
// Добавляем функцию для детального логирования PID void LogPIDState(const char* wheelName, PID_TypeDef* pid, double setpoint, double input, double output) { printf("SETPOINT: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", setpointFL, setpointFR, setpointBL, setpointBR); printf("CURRENT: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR); printf("PID_OUT: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR); } /* USER CODE END 0 */
/* USER CODE BEGIN 2 */ printf("Hi, from machine! \r\n");
// Инициализация переменных перед PID setpointFL = setpointFR = setpointBL = setpointBR = 0.0; currentSpeedFL = currentSpeedFR = currentSpeedBL = currentSpeedBR = 0.0; pidOutputFL = pidOutputFR = pidOutputBL = pidOutputBR = 0.0;
InitializePID(); /* USER CODE END 2 */
/* USER CODE BEGIN 4 / void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { for (int i = 0; i < 4; i++) { if (GPIO_Pin == sensors[i].pin) { ((sensors[i].counter))++; // Добавляем логирование энкодеров при отладке if (pidDebugEnabled && (*(sensors[i].counter)) % 100 == 0) { printf("ENC_%d: %ld\r\n", i, (sensors[i].counter)); } break; } } } / USER CODE END 4 */
void Start_Motor_Task(void argument) { / USER CODE BEGIN Start_Motor_Task */ #define PACKET_TIMEOUT 50 #define JOYSTICK_UPDATE_RATE 20 #define PWM_UPDATE_INTERVAL 50
// Инициализация GPIO и PWM HAL_GPIO_WritePin(Mot_A2_GPIO_Port, Mot_A2_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOD, Mot_D2_Pin | Mot_C2_Pin | Mot_B2_Pin, GPIO_PIN_RESET);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);
// uint32_t now = 0; Ringbuf_Init(); Wheels_Init();
// Инициализация очереди для пакетов packetQueueHandle = osMessageQueueNew(PACKET_QUEUE_SIZE, sizeof(PacketMessage), NULL); if (packetQueueHandle == NULL) { while (1) { printf("Queue init failed!\r\n"); osDelay(1000); } }
// typedef struct { // uint8_t buttonId; // uint8_t stableState; // uint32_t lastStableEventTime; // } ButtonState;
// ButtonState buttons[MAX_BUTTONS] = {0}; // for (int i = 0; i < MAX_BUTTONS; i++) { // buttons[i].buttonId = i; // }
uint32_t lastPwmUpdateTime = 0; bool pwmRunning = false; static uint32_t lastPwmValues[4] = {0};
// Инициализация массивов PWM memset(pwmData_CH1, 0, sizeof(pwmData_CH1)); memset(pwmData_CH2, 0, sizeof(pwmData_CH2)); memset(pwmData_CH3, 0, sizeof(pwmData_CH3)); memset(pwmData_CH4, 0, sizeof(pwmData_CH4));
// Запуск PWM с DMA HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_1, (uint32_t*) pwmData_CH1, 10); HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_2, (uint32_t*) pwmData_CH2, 10); HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_3, (uint32_t*) pwmData_CH3, 10); HAL_TIM_PWM_Start_DMA(&htim1, TIM_CHANNEL_4, (uint32_t*) pwmData_CH4, 10);
pwmRunning = true; Wheels_Stop();
// ВАЖНО: Начинаем с прямого режима currentControlMode = CONTROL_MODE_DIRECT; printf("Started in DIRECT MODE\r\n"); /* Infinite loop */ for (;;) { // Обработка UART данных if (Tail != Head) { osMutexAcquire(myMutexHandle, osWaitForever); // now = HAL_GetTick();
uint16_t bytesToProcess = (Head >= Tail) ? (Head - Tail) : (MainBuf_SIZE - Tail + Head); uint16_t maxBytes = 16; if (bytesToProcess > maxBytes) bytesToProcess = maxBytes; uint8_t tempBuffer[16]; uint16_t tempTail = Tail; for (uint16_t i = 0; i < bytesToProcess; i++) { tempBuffer[i] = MainBuf[tempTail]; tempTail = (tempTail + 1) % MainBuf_SIZE; } Tail = tempTail; osMutexRelease(myMutexHandle); for (uint16_t i = 0; i < bytesToProcess; i++) { UART_ProcessByte(tempBuffer[i]); } } // Обновление PWM if (pwmRunning && (HAL_GetTick() - lastPwmUpdateTime) >= PWM_UPDATE_INTERVAL) { lastPwmUpdateTime = HAL_GetTick(); uint32_t newValues[4] = {0}; bool motorsActive = false; // УЛУЧШЕННАЯ логика выбора режима if (currentControlMode == CONTROL_MODE_PID) { if (targetSpeeds.isActive) { motorsActive = true; newValues[0] = CompensateDeadzone(pidOutputFL); newValues[1] = CompensateDeadzone(pidOutputFR); newValues[2] = CompensateDeadzone(pidOutputBL); newValues[3] = CompensateDeadzone(pidOutputBR); HAL_GPIO_WritePin(Mot_A2_GPIO_Port, Mot_A2_Pin, (pidOutputFL < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOD, Mot_B2_Pin, (pidOutputFR < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOD, Mot_C2_Pin, (pidOutputBL < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOD, Mot_D2_Pin, (pidOutputBR < 0) ? GPIO_PIN_SET : GPIO_PIN_RESET); } } else { MotorsState currentState = Wheels_Get_State(); if (currentState.isActive) { motorsActive = true; newValues[0] = currentState.powerFL; newValues[1] = currentState.powerFR; newValues[2] = currentState.powerBL; newValues[3] = currentState.powerBR; } } // Проверяем изменились ли значения bool valuesChanged = false; for (int i = 0; i < 4; i++) { if (lastPwmValues[i] != newValues[i]) { valuesChanged = true; lastPwmValues[i] = newValues[i]; } } // Обновляем PWM значения for (int i = 0; i < 10; i++) { pwmData_CH1[i] = newValues[0]; pwmData_CH2[i] = newValues[1]; pwmData_CH3[i] = newValues[2]; pwmData_CH4[i] = newValues[3]; } // УПРОЩЕННОЕ логирование для избежания переполнения буфера printf if (valuesChanged) { if (currentControlMode == CONTROL_MODE_PID) { printf("[PID] PWM: %lu,%lu,%lu,%lu\r\n", newValues[0], newValues[1], newValues[2], newValues[3]); } else { printf("[DIR] P=%d PWM: %lu\r\n", joystickPower, newValues[0]); } } } osDelay(1); } /* USER CODE END Start_Motor_Task */}
void Start_PID_Task(void argument) { / USER CODE BEGIN Start_PID_Task */ uint32_t lastUpdateTime = 0; uint32_t lastLogTime = 0; const uint32_t LOG_INTERVAL = 15000; // Увеличил интервал логирования
printf("=== PID TASK STARTED ===\r\n"); printf("Update period: %d ms\r\n", PID_UPDATE_PERIOD); /* Infinite loop */for(;;) { uint32_t currentTime = HAL_GetTick();
// Работаем только в режиме PID и по истечении периода обновления if (currentControlMode == CONTROL_MODE_PID && currentTime - lastUpdateTime >= PID_UPDATE_PERIOD) { // Чтение текущих значений энкодеров с защитой от прерываний int32_t currentEncA, currentEncB, currentEncC, currentEncD; osMutexAcquire(myMutexHandle, osWaitForever); currentEncA = encA_count; currentEncB = encB_count; currentEncC = encC_count; currentEncD = encD_count; osMutexRelease(myMutexHandle); // Вычисление изменений int32_t deltaEncA = currentEncA - lastEncA; int32_t deltaEncB = currentEncB - lastEncB; int32_t deltaEncC = currentEncC - lastEncC; int32_t deltaEncD = currentEncD - lastEncD; // Обновление предыдущих значений lastEncA = currentEncA; lastEncB = currentEncB; lastEncC = currentEncC; lastEncD = currentEncD; if (targetSpeeds.isActive && pidDebugEnabled) { static uint32_t debugCounter = 0; if (++debugCounter % 100 == 0) { // Каждые 5 секунд при 50ms периоде printf("PID_DEBUG: ENC_DELTA: A=%ld B=%ld C=%ld D=%ld\r\n", deltaEncA, deltaEncB, deltaEncC, deltaEncD); printf("PID_DEBUG: SPEEDS: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR); printf("PID_DEBUG: OUTPUTS: FL=%.2f FR=%.2f BL=%.2f BR=%.2f\r\n", pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR); } } // Расчет скоростей// double timeFactor = 1000.0f / PID_UPDATE_PERIOD; // double ticksToSpeedFactor = 0.2f; currentSpeedFL = (double)deltaEncA * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0); currentSpeedFR = (double)deltaEncB * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0); currentSpeedBL = (double)deltaEncC * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0); currentSpeedBR = (double)deltaEncD * 60.0 / (double)(ticks_per_rev * PID_UPDATE_PERIOD / 1000.0);
// Установка setpoint'ов if (targetSpeeds.isActive) { static double rampSpeed = 0; rampSpeed += 0.2f; if(rampSpeed > 1.0f) rampSpeed = 1.0f; setpointFL = targetSpeeds.speedFL * rampSpeed; setpointFR = targetSpeeds.speedFR * rampSpeed; setpointBL = targetSpeeds.speedBL * rampSpeed; setpointBR = targetSpeeds.speedBR * rampSpeed; } else { setpointFL = setpointFR = setpointBL = setpointBR = 0; } // Проверка необходимости вычислений bool shouldCompute = targetSpeeds.isActive || (fabs(currentSpeedFL) > 0.1 || fabs(currentSpeedFR) > 0.1 || fabs(currentSpeedBL) > 0.1 || fabs(currentSpeedBR) > 0.1); if (shouldCompute) { bool computeFL = PID_Compute(&pidFL); bool computeFR = PID_Compute(&pidFR); bool computeBL = PID_Compute(&pidBL); bool computeBR = PID_Compute(&pidBR); printf("PID_COMPUTE: [%d,%d,%d,%d] Ticks: %ld,%ld,%ld,%ld\r\n", computeFL, computeFR, computeBL, computeBR, deltaEncA, deltaEncB, deltaEncC, deltaEncD); printf("PID_VALUES: SP=[%.1f,%.1f,%.1f,%.1f] IN=[%.1f,%.1f,%.1f,%.1f] OUT=[%.1f,%.1f,%.1f,%.1f]\r\n", setpointFL, setpointFR, setpointBL, setpointBR, currentSpeedFL, currentSpeedFR, currentSpeedBL, currentSpeedBR, pidOutputFL, pidOutputFR, pidOutputBL, pidOutputBR); // ЛОГИРУЕМ ТОЛЬКО РАЗ В 5 СЕКУНД И ТОЛЬКО ЕСЛИ ЕСТЬ АКТИВНОСТЬ static uint32_t lastQuickLog = 0; if (targetSpeeds.isActive && (currentTime - lastQuickLog) > 5000) { printf("PID: SP=%.1f CUR=%.1f OUT=%.1f\r\n", setpointFL, currentSpeedFL, pidOutputFL); lastQuickLog = currentTime; } } else { // Пропускаем цикл вычислений osDelay(1); continue; } lastUpdateTime = currentTime; pidLastComputeTime = currentTime; } // Периодическое логирование состояния if (pidDebugEnabled && currentTime - lastLogTime >= LOG_INTERVAL) { lastLogTime = currentTime; printf("\n=== PID STATUS REPORT ===\r\n"); printf("Control Mode: %s\r\n", (currentControlMode == CONTROL_MODE_PID) ? "PID" : "DIRECT"); printf("Target Active: %s\r\n", targetSpeeds.isActive ? "YES" : "NO"); printf("Last Compute: %lu ms ago\r\n", currentTime - pidLastComputeTime); printf("Encoder Values: A=%ld B=%ld C=%ld D=%ld\r\n", encA_count, encB_count, encC_count, encD_count); printf("=========================\r\n"); } osDelay(1);} /* USER CODE END Start_PID_Task */ }
void Start_Pkt_Proc_Task(void argument) { / USER CODE BEGIN Start_Pkt_Proc_Task / osDelay(100); PacketMessage msg; // uint32_t lastJoystickUpdateTime = 0; // uint32_t joystickTimeout = 500; // Таймаут в мс - если не получали команд от джойстика за это время, останавливаем моторы / Infinite loop */ for (;;) { // uint32_t currentTime = HAL_GetTick(); // Проверка таймаута джойстика - если давно не получали команд, останавливаем моторы // if ((currentTime - lastJoystickUpdateTime) > joystickTimeout && joystickPower != 0) { // // Останавливаем моторы, если последняя команда была не остановка // Wheels_Stop(); // joystickPower = 0; // joystickDir = 'F'; // printf("[AUTO STOP] Joystick timeout - motors stopped\r\n"); // } if (osMessageQueueGet(packetQueueHandle, &msg, NULL, 10) == osOK) { switch (msg.type) { case PACKET_TYPE_BUTTON: if (msg.data.button.id < MAX_BUTTONS) { uint8_t buttonId = msg.data.button.id; uint8_t newState = (msg.data.button.state == 0x7F) ? 1 : 0;
if (buttonId == 4 && newState == 1) { // STOP printf("=== EMERGENCY STOP PRESSED ===\r\n"); Wheels_Stop(); joystickPower = 0; joystickDir = 'F'; targetSpeeds.isActive = false; targetSpeeds.speedFL = 0; targetSpeeds.speedFR = 0; targetSpeeds.speedBL = 0; targetSpeeds.speedBR = 0; printf("All motors stopped, PID targets cleared\r\n"); } else if (buttonId == 5 && newState == 1) { // Переключение режимов printf("=== MODE SWITCH PRESSED ===\r\n"); if (currentControlMode == CONTROL_MODE_DIRECT) { // Сначала останавливаем моторы Wheels_Stop(); osDelay(100); // Увеличил задержку currentControlMode = CONTROL_MODE_PID; printf("SWITCHED TO: PID MODE\r\n"); } else { // Сначала останавливаем PID targetSpeeds.isActive = false; targetSpeeds.speedFL = 0; targetSpeeds.speedFR = 0; targetSpeeds.speedBL = 0; targetSpeeds.speedBR = 0; osDelay(100); // Увеличил задержку currentControlMode = CONTROL_MODE_DIRECT; printf("SWITCHED TO: DIRECT MODE\r\n"); } } else if (buttonId == 6 && newState == 1) { pidDebugEnabled = !pidDebugEnabled; printf("PID Debug: %s\r\n", pidDebugEnabled ? "ENABLED" : "DISABLED"); } } break; case PACKET_TYPE_JOYSTICK: { int8_t x = (int8_t)(msg.data.joystick.x - 127); int8_t y = (int8_t)(msg.data.joystick.y - 127); uint8_t power = msg.data.joystick.power; // ТОЛЬКО ОСНОВНАЯ ИНФОРМАЦИЯ printf("JS: x=%d y=%d p=%d\r\n", x, y, power);// lastJoystickUpdateTime = currentTime; joystickPower = power; joystickDir = (y >= 0) ? 'F' : 'R';
if (power <= 100) { if (power == 0) { Wheels_Stop(); targetSpeeds.isActive = false; targetSpeeds.speedFL = 0; targetSpeeds.speedFR = 0; targetSpeeds.speedBL = 0; targetSpeeds.speedBR = 0; } else { if (currentControlMode == CONTROL_MODE_PID) { if (power > 30) { // Уменьшаем максимальную мощность с 40 до 30 power = 30; } targetSpeeds.isActive = true; // ИСПРАВЛЕНИЕ: Еще больше уменьшаем speedScale float speedScale = 3.0f; // Было 10.0f - уменьшаем до 3.0f float baseSpeed = (power / 100.0f) * speedScale; float normY = y / 127.0f; float normX = x / 127.0f; float forwardSpeed = normY * baseSpeed; float turnSpeed = normX * baseSpeed * 0.5f; // Увеличиваем коэффициент поворота targetSpeeds.speedFL = forwardSpeed + turnSpeed; targetSpeeds.speedFR = forwardSpeed - turnSpeed; targetSpeeds.speedBL = forwardSpeed + turnSpeed; targetSpeeds.speedBR = forwardSpeed - turnSpeed; printf("PID_TGT: %.1f,%.1f,%.1f,%.1f\r\n", targetSpeeds.speedFL, targetSpeeds.speedFR, targetSpeeds.speedBL, targetSpeeds.speedBR); } else { Wheels_Process_Joystick(x, y, power); } } } } break; } } osDelay(1); } /* USER CODE END Start_Pkt_Proc_Task */}
Assistant
Отлично, код получен! Теперь задавай свой вопрос, и я постараюсь помочь.
Что я вижу в коде:
-
Система управления моторами с поддержкой двух режимов:
CONTROL_MODE_DIRECT(прямое управление через джойстик)CONTROL_MODE_PID(регулирование скорости через PID-контроллеры)
-
Обработка UART-данных (пакеты от джойстика и кнопок)
-
PID-регуляторы для каждого колеса с настройками:
KP = 1.0,KI = 0.05,KD = 0.01- Ограничение выхода:
±15(с масштабированием до0–35для PWM)
-
Энкодеры для измерения скорости вращения колес
-
Задачи FreeRTOS:
Start_Motor_Task— управление моторамиStart_PID_Task— вычисление PIDStart_Pkt_Proc_Task— обработка пакетов от джойстика
Возможные вопросы, которые могут возникнуть:
- Как работает переключение между режимами
DIRECTиPID? - Почему PID-регуляторы нестабильны или не работают?
- Как правильно настроить коэффициенты PID?
- Почему моторы не реагируют на команды?
- Как улучшить обработку данных от энкодеров?
Задавай свой вопрос, и я помогу разобраться! 🚀
-
Other models, same or similar prompt
Responses were produced in different, uncontrolled contexts (system prompts, settings, prior turns). This is not a benchmark.