From 704332412ae5979486822b5e156412acacfe60b3 Mon Sep 17 00:00:00 2001 From: velichko Date: Tue, 4 Nov 2025 02:50:56 +0200 Subject: [PATCH 1/8] Issue #17: creating files and source code --- KPI_Rover/Motors/motors_driver.c | 32 ++++++++++++++++++ KPI_Rover/Motors/motors_driver.h | 21 ++++++++++++ KPI_Rover/Motors/motors_manager.c | 54 +++++++++++++++++++++++++++++++ KPI_Rover/Motors/motors_manager.h | 13 ++++++++ 4 files changed, 120 insertions(+) create mode 100644 KPI_Rover/Motors/motors_driver.c create mode 100644 KPI_Rover/Motors/motors_driver.h create mode 100644 KPI_Rover/Motors/motors_manager.c create mode 100644 KPI_Rover/Motors/motors_manager.h diff --git a/KPI_Rover/Motors/motors_driver.c b/KPI_Rover/Motors/motors_driver.c new file mode 100644 index 0000000..9659c68 --- /dev/null +++ b/KPI_Rover/Motors/motors_driver.c @@ -0,0 +1,32 @@ +#include "motors_driver.h" +#include "ulog.h" + +void DriverMotors_Init(Motor_HandleTypeDef* motor){ + HAL_TIM_PWM_Start(motor->htim_pwm, motor->pwm_channel); + __HAL_TIM_MOE_ENABLE(motor->htim_pwm); + DriverMotors_Stop(motor); +} + +void DriverMotors_Forward(Motor_HandleTypeDef* motor){ + HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, GPIO_PIN_RESET); +} + +void DriverMotors_Backward(Motor_HandleTypeDef* motor){ + HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, GPIO_PIN_SET); +} + +void DriverMotors_Stop(Motor_HandleTypeDef* motor){ + HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, GPIO_PIN_RESET); +} + +void DriverMotors_SetSpeed(Motor_HandleTypeDef* motor, uint8_t speed_percent) { + if (speed_percent > 100) speed_percent = 100; + uint32_t period = __HAL_TIM_GET_AUTORELOAD(motor->htim_pwm); + uint32_t compare = (uint32_t)(( (uint64_t)(period + 1) * speed_percent ) / 100); + __HAL_TIM_SET_COMPARE(motor->htim_pwm, motor->pwm_channel, compare); + ULOG_INFO("SetSpeed %u%% -> compare=%lu period=%lu", speed_percent, compare, period); +} + diff --git a/KPI_Rover/Motors/motors_driver.h b/KPI_Rover/Motors/motors_driver.h new file mode 100644 index 0000000..4d630d5 --- /dev/null +++ b/KPI_Rover/Motors/motors_driver.h @@ -0,0 +1,21 @@ +#ifndef MOTORS_DRIVER_H +#define MOTORS_DRIVER_H + +#include "stm32f4xx_hal.h" + +typedef struct { + GPIO_TypeDef* IN1_port; + uint16_t IN1_pin; + GPIO_TypeDef* IN2_port; + uint16_t IN2_pin; + TIM_HandleTypeDef* htim_pwm; + uint32_t pwm_channel; +} Motor_HandleTypeDef; + +void DriverMotors_Init(Motor_HandleTypeDef* motor); +void DriverMotors_Forward(Motor_HandleTypeDef* motor); +void DriverMotors_Backward(Motor_HandleTypeDef* motor); +void DriverMotors_Stop(Motor_HandleTypeDef* motor); +void DriverMotors_SetSpeed(Motor_HandleTypeDef* motor, uint8_t speed_percent); + +#endif diff --git a/KPI_Rover/Motors/motors_manager.c b/KPI_Rover/Motors/motors_manager.c new file mode 100644 index 0000000..1e160af --- /dev/null +++ b/KPI_Rover/Motors/motors_manager.c @@ -0,0 +1,54 @@ +#include "motors_manager.h" +#include "cmsis_os.h" +#include "ulog.h" + +extern TIM_HandleTypeDef htim1; + +static Motor_HandleTypeDef motor1; + +void MotorManager_Init(void) { + + motor1.IN1_port = GPIOC; + motor1.IN1_pin = GPIO_PIN_4; + motor1.IN2_port = GPIOC; + motor1.IN2_pin = GPIO_PIN_5; + motor1.htim_pwm = &htim1; + motor1.pwm_channel = TIM_CHANNEL_1; + + DriverMotors_Init(&motor1); +} + +void MotorManager_Task(void* argument) { + MotorManager_Init(); + while(1) { + + MotorManager_MoveForward(100); + ULOG_INFO("MoveForward"); + osDelay(4000); + + MotorManager_Stop(); + osDelay(1000); + + MotorManager_MoveBackward(55); + ULOG_INFO("MoveBackward"); + osDelay(3000); + + MotorManager_Stop(); + osDelay(1000); + + } +} + +void MotorManager_MoveForward(uint8_t speed) { + DriverMotors_Forward(&motor1); + DriverMotors_SetSpeed(&motor1, speed); +} + +void MotorManager_MoveBackward(uint8_t speed) { + DriverMotors_Backward(&motor1); + DriverMotors_SetSpeed(&motor1, speed); +} + +void MotorManager_Stop(void) { + DriverMotors_Stop(&motor1); +} diff --git a/KPI_Rover/Motors/motors_manager.h b/KPI_Rover/Motors/motors_manager.h new file mode 100644 index 0000000..012faec --- /dev/null +++ b/KPI_Rover/Motors/motors_manager.h @@ -0,0 +1,13 @@ +#ifndef MOTORS_MANAGER_H +#define MOTORS_MANAGER_H + +#include "motors_driver.h" + + +void MotorManager_Init(void); +void MotorManager_Task(void* argument); +void MotorManager_MoveForward(uint8_t speed); +void MotorManager_MoveBackward(uint8_t speed); +void MotorManager_Stop(void); + +#endif From 15ee6236d6145241d2b4fd6f2a5f9ff0b03d7e40 Mon Sep 17 00:00:00 2001 From: velichko Date: Tue, 4 Nov 2025 12:59:20 +0200 Subject: [PATCH 2/8] Issue #17: added a state machine and soft start of motors --- Core/Src/main.c | 52 ++++++++++++++++-- KPI_Rover/Motors/motors_driver.c | 91 +++++++++++++++++++++++++++++++ KPI_Rover/Motors/motors_driver.h | 23 ++++++++ KPI_Rover/Motors/motors_manager.c | 60 +++++++++++++++++++- 4 files changed, 218 insertions(+), 8 deletions(-) diff --git a/Core/Src/main.c b/Core/Src/main.c index 824509b..ca657d9 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -455,9 +455,9 @@ static void MX_TIM1_Init(void) /* USER CODE END TIM1_Init 1 */ htim1.Instance = TIM1; - htim1.Init.Prescaler = 0; + htim1.Init.Prescaler = 167; htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 65535; + htim1.Init.Period = 999; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; @@ -718,6 +718,48 @@ static void MX_USART3_UART_Init(void) } +/** + * @brief TIM12 Initialization Function + * @param None + * @retval None + */ +static void MX_TIM12_Init(void) +{ + + /* USER CODE BEGIN TIM12_Init 0 */ + + /* USER CODE END TIM12_Init 0 */ + + TIM_OC_InitTypeDef sConfigOC = {0}; + + /* USER CODE BEGIN TIM12_Init 1 */ + + /* USER CODE END TIM12_Init 1 */ + htim12.Instance = TIM12; + htim12.Init.Prescaler = 0; + htim12.Init.CounterMode = TIM_COUNTERMODE_UP; + htim12.Init.Period = 65535; + htim12.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim12.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_PWM_Init(&htim12) != HAL_OK) + { + Error_Handler(); + } + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = 0; + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + if (HAL_TIM_PWM_ConfigChannel(&htim12, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM12_Init 2 */ + + /* USER CODE END TIM12_Init 2 */ + HAL_TIM_MspPostInit(&htim12); + +} + /** * @brief GPIO Initialization Function * @param None @@ -760,12 +802,12 @@ static void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); - /*Configure GPIO pin : OTG_FS_PowerSwitchOn_Pin */ - GPIO_InitStruct.Pin = OTG_FS_PowerSwitchOn_Pin; + /*Configure GPIO pins : OTG_FS_PowerSwitchOn_Pin PC4 PC5 */ + GPIO_InitStruct.Pin = OTG_FS_PowerSwitchOn_Pin|GPIO_PIN_4|GPIO_PIN_5; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(OTG_FS_PowerSwitchOn_GPIO_Port, &GPIO_InitStruct); + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); /*Configure GPIO pin : PDM_OUT_Pin */ GPIO_InitStruct.Pin = PDM_OUT_Pin; diff --git a/KPI_Rover/Motors/motors_driver.c b/KPI_Rover/Motors/motors_driver.c index 9659c68..3b97a50 100644 --- a/KPI_Rover/Motors/motors_driver.c +++ b/KPI_Rover/Motors/motors_driver.c @@ -30,3 +30,94 @@ void DriverMotors_SetSpeed(Motor_HandleTypeDef* motor, uint8_t speed_percent) { ULOG_INFO("SetSpeed %u%% -> compare=%lu period=%lu", speed_percent, compare, period); } +void DriverMotors_TimerTask(Motor_HandleTypeDef* motor) { + uint32_t now = HAL_GetTick(); + uint32_t elapsed = now - motor->state_entry_time_ms; + + switch (motor->state) { + + case MOTOR_STATE_STOP: + + break; + + case MOTOR_STATE_FORWARD: + switch (motor->substate) { + case MOTOR_SUBSTATE_ACCEL: + if (motor->current_speed < motor->target_speed) { + motor->current_speed += 15; + DriverMotors_SetSpeed(motor, motor->current_speed); + } else { + motor->substate = MOTOR_SUBSTATE_CONSTANT; + } + break; + + case MOTOR_SUBSTATE_CONSTANT: + + break; + + case MOTOR_SUBSTATE_DECEL: + if (motor->current_speed > 0) { + if (motor->current_speed > 15) + motor->current_speed -= 15; + else + motor->current_speed = 0; + + DriverMotors_SetSpeed(motor, motor->current_speed); + } else { + DriverMotors_SetSpeed(motor, 0); + DriverMotors_Stop(motor); + motor->state = MOTOR_STATE_STOP; + motor->substate = MOTOR_SUBSTATE_NONE; + ULOG_INFO("Motor stopped completely"); + } + break; + + + default: + break; + } + break; + + case MOTOR_STATE_BACKWARD: + switch (motor->substate) { + case MOTOR_SUBSTATE_ACCEL: + if (motor->current_speed < motor->target_speed) { + motor->current_speed += 15; + DriverMotors_SetSpeed(motor, motor->current_speed); + } else { + motor->substate = MOTOR_SUBSTATE_CONSTANT; + } + break; + + case MOTOR_SUBSTATE_CONSTANT: + + break; + + case MOTOR_SUBSTATE_DECEL: + if (motor->current_speed > 0) { + if (motor->current_speed > 15) + motor->current_speed -= 15; + else + motor->current_speed = 0; + + DriverMotors_SetSpeed(motor, motor->current_speed); + } else { + DriverMotors_SetSpeed(motor, 0); + DriverMotors_Stop(motor); + motor->state = MOTOR_STATE_STOP; + motor->substate = MOTOR_SUBSTATE_NONE; + ULOG_INFO("Motor stopped completely"); + } + break; + + + default: + break; + } + break; + + default: + break; + } +} + diff --git a/KPI_Rover/Motors/motors_driver.h b/KPI_Rover/Motors/motors_driver.h index 4d630d5..1d8089d 100644 --- a/KPI_Rover/Motors/motors_driver.h +++ b/KPI_Rover/Motors/motors_driver.h @@ -3,6 +3,22 @@ #include "stm32f4xx_hal.h" +typedef enum { + MOTOR_STATE_STOP = 0, + MOTOR_STATE_FORWARD, + MOTOR_STATE_BACKWARD, + MOTOR_STATE_BRAKE +} MotorState_t; + + +typedef enum { + MOTOR_SUBSTATE_NONE = 0, + MOTOR_SUBSTATE_ACCEL, + MOTOR_SUBSTATE_CONSTANT, + MOTOR_SUBSTATE_DECEL +} MotorSubState_t; + + typedef struct { GPIO_TypeDef* IN1_port; uint16_t IN1_pin; @@ -10,6 +26,12 @@ typedef struct { uint16_t IN2_pin; TIM_HandleTypeDef* htim_pwm; uint32_t pwm_channel; + + MotorState_t state; + MotorSubState_t substate; + uint32_t state_entry_time_ms; + uint8_t target_speed; + uint8_t current_speed; } Motor_HandleTypeDef; void DriverMotors_Init(Motor_HandleTypeDef* motor); @@ -17,5 +39,6 @@ void DriverMotors_Forward(Motor_HandleTypeDef* motor); void DriverMotors_Backward(Motor_HandleTypeDef* motor); void DriverMotors_Stop(Motor_HandleTypeDef* motor); void DriverMotors_SetSpeed(Motor_HandleTypeDef* motor, uint8_t speed_percent); +void DriverMotors_TimerTask(Motor_HandleTypeDef* motor); #endif diff --git a/KPI_Rover/Motors/motors_manager.c b/KPI_Rover/Motors/motors_manager.c index 1e160af..f8a5532 100644 --- a/KPI_Rover/Motors/motors_manager.c +++ b/KPI_Rover/Motors/motors_manager.c @@ -4,8 +4,14 @@ extern TIM_HandleTypeDef htim1; +#define MOTOR_TIMER_PERIOD_MS 50u + +static osTimerId_t motor_timer_handle; static Motor_HandleTypeDef motor1; +static void motor_timer_callback(void *argument); + + void MotorManager_Init(void) { motor1.IN1_port = GPIOC; @@ -16,6 +22,23 @@ void MotorManager_Init(void) { motor1.pwm_channel = TIM_CHANNEL_1; DriverMotors_Init(&motor1); + + const osTimerAttr_t timer_attrs = { + .name = "Motor_Timer" + }; + + motor_timer_handle = osTimerNew(motor_timer_callback, osTimerPeriodic, NULL, &timer_attrs); + if (!motor_timer_handle) { + ULOG_ERROR("Failed to create Motor timer"); + osThreadExit(); + } + + osDelay(10); + + if (osTimerStart(motor_timer_handle, MOTOR_TIMER_PERIOD_MS) != osOK) { + ULOG_ERROR("Failed to start Motor timer"); + osThreadExit(); + } } void MotorManager_Task(void* argument) { @@ -33,22 +56,53 @@ void MotorManager_Task(void* argument) { ULOG_INFO("MoveBackward"); osDelay(3000); + MotorManager_MoveForward(80); + ULOG_INFO("MoveForward"); + osDelay(3000); + MotorManager_Stop(); osDelay(1000); } } +static void motor_timer_callback(void *argument) { + DriverMotors_TimerTask(&motor1); +} + + void MotorManager_MoveForward(uint8_t speed) { + if (motor1.substate == MOTOR_SUBSTATE_DECEL) { + ULOG_INFO("Can't move forward while decelerating"); + return; + } DriverMotors_Forward(&motor1); - DriverMotors_SetSpeed(&motor1, speed); + motor1.target_speed = speed; + motor1.current_speed = 0; + motor1.state = MOTOR_STATE_FORWARD; + motor1.substate = MOTOR_SUBSTATE_ACCEL; + motor1.state_entry_time_ms = HAL_GetTick(); } void MotorManager_MoveBackward(uint8_t speed) { + if (motor1.substate == MOTOR_SUBSTATE_DECEL) { + ULOG_INFO("Can't move backward while decelerating"); + return; + } DriverMotors_Backward(&motor1); - DriverMotors_SetSpeed(&motor1, speed); + motor1.target_speed = speed; + motor1.current_speed = 0; + motor1.state = MOTOR_STATE_BACKWARD; + motor1.substate = MOTOR_SUBSTATE_ACCEL; + motor1.state_entry_time_ms = HAL_GetTick(); } void MotorManager_Stop(void) { - DriverMotors_Stop(&motor1); + if (motor1.state == MOTOR_STATE_FORWARD || motor1.state == MOTOR_STATE_BACKWARD) { + motor1.substate = MOTOR_SUBSTATE_DECEL; + } else { + DriverMotors_Stop(&motor1); + motor1.state = MOTOR_STATE_STOP; + motor1.substate = MOTOR_SUBSTATE_NONE; + } } From 532aa3e8c08f3c704f99f22bd438d30a2b64e0fd Mon Sep 17 00:00:00 2001 From: velichko Date: Tue, 4 Nov 2025 15:50:03 +0200 Subject: [PATCH 3/8] Issue #17: added control of multiple motors --- KPI_Rover/Motors/motors_manager.c | 259 +++++++++++++++++++++++------- KPI_Rover/Motors/motors_manager.h | 4 + 2 files changed, 201 insertions(+), 62 deletions(-) diff --git a/KPI_Rover/Motors/motors_manager.c b/KPI_Rover/Motors/motors_manager.c index f8a5532..26a5d88 100644 --- a/KPI_Rover/Motors/motors_manager.c +++ b/KPI_Rover/Motors/motors_manager.c @@ -5,104 +5,239 @@ extern TIM_HandleTypeDef htim1; #define MOTOR_TIMER_PERIOD_MS 50u +#define MOTOR_MANAGER_MAX_MOTORS 4 static osTimerId_t motor_timer_handle; -static Motor_HandleTypeDef motor1; - +static Motor_HandleTypeDef motors[MOTOR_MANAGER_MAX_MOTORS]; +static uint8_t motors_count = 0; static void motor_timer_callback(void *argument); +static Motor_HandleTypeDef* get_motor_by_id(uint8_t id) { + if (id >= motors_count) return NULL; + return &motors[id]; +} + +static void register_motor(const Motor_HandleTypeDef* src, uint8_t id) { + if (id >= MOTOR_MANAGER_MAX_MOTORS) { + ULOG_ERROR("register_motor: id %u out of range", id); + return; + } + motors[id] = *src; + if (id >= motors_count) motors_count = id + 1; +} + + + void MotorManager_Init(void) { - motor1.IN1_port = GPIOC; - motor1.IN1_pin = GPIO_PIN_4; - motor1.IN2_port = GPIOC; - motor1.IN2_pin = GPIO_PIN_5; - motor1.htim_pwm = &htim1; - motor1.pwm_channel = TIM_CHANNEL_1; + Motor_HandleTypeDef m0; + m0.IN1_port = GPIOC; + m0.IN1_pin = GPIO_PIN_4; + m0.IN2_port = GPIOC; + m0.IN2_pin = GPIO_PIN_5; + m0.htim_pwm = &htim1; + m0.pwm_channel = TIM_CHANNEL_1; + m0.state = MOTOR_STATE_STOP; + m0.substate = MOTOR_SUBSTATE_NONE; + m0.state_entry_time_ms = HAL_GetTick(); + m0.target_speed = 0; + m0.current_speed = 0; + + register_motor(&m0, 0); + DriverMotors_Init(&motors[0]); + + + Motor_HandleTypeDef m1; + m1.IN1_port = GPIOE; + m1.IN1_pin = GPIO_PIN_7; + m1.IN2_port = GPIOE; + m1.IN2_pin = GPIO_PIN_8; + m1.htim_pwm = &htim1; + m1.pwm_channel = TIM_CHANNEL_2; + m1.state = MOTOR_STATE_STOP; + m1.substate = MOTOR_SUBSTATE_NONE; + m1.state_entry_time_ms = HAL_GetTick(); + m1.target_speed = 0; + m1.current_speed = 0; + + register_motor(&m1, 1); + DriverMotors_Init(&motors[1]); + - DriverMotors_Init(&motor1); const osTimerAttr_t timer_attrs = { - .name = "Motor_Timer" - }; + .name = "Motor_Timer" + }; - motor_timer_handle = osTimerNew(motor_timer_callback, osTimerPeriodic, NULL, &timer_attrs); - if (!motor_timer_handle) { - ULOG_ERROR("Failed to create Motor timer"); - osThreadExit(); - } + motor_timer_handle = osTimerNew(motor_timer_callback, osTimerPeriodic, NULL, &timer_attrs); + if (!motor_timer_handle) { + ULOG_ERROR("Failed to create Motor timer"); + osThreadExit(); + } - osDelay(10); + osDelay(10); - if (osTimerStart(motor_timer_handle, MOTOR_TIMER_PERIOD_MS) != osOK) { - ULOG_ERROR("Failed to start Motor timer"); - osThreadExit(); - } + if (osTimerStart(motor_timer_handle, MOTOR_TIMER_PERIOD_MS) != osOK) { + ULOG_ERROR("Failed to start Motor timer"); + osThreadExit(); + } } void MotorManager_Task(void* argument) { - MotorManager_Init(); + MotorManager_Init(); while(1) { - MotorManager_MoveForward(100); - ULOG_INFO("MoveForward"); - osDelay(4000); + MotorManager_MoveForward(100); + ULOG_INFO("MoveForward (all registered)"); + osDelay(4000); - MotorManager_Stop(); - osDelay(1000); + MotorManager_Stop(); + osDelay(1000); - MotorManager_MoveBackward(55); - ULOG_INFO("MoveBackward"); - osDelay(3000); + MotorManager_MoveBackward(55); + ULOG_INFO("MoveBackward (all registered)"); + osDelay(3000); - MotorManager_MoveForward(80); - ULOG_INFO("MoveForward"); - osDelay(3000); + MotorManager_MoveForwardMotor(0,100); + ULOG_INFO("MoveForward (one registered)"); + osDelay(2000); - MotorManager_Stop(); - osDelay(1000); + MotorManager_MoveForwardMotor(1,100); + ULOG_INFO("MoveForward (one registered)"); + osDelay(2000); + + MotorManager_Stop(); + osDelay(1000); } } static void motor_timer_callback(void *argument) { - DriverMotors_TimerTask(&motor1); + for (uint8_t i = 0; i < motors_count; ++i) { + DriverMotors_TimerTask(&motors[i]); + } } - void MotorManager_MoveForward(uint8_t speed) { - if (motor1.substate == MOTOR_SUBSTATE_DECEL) { - ULOG_INFO("Can't move forward while decelerating"); - return; - } - DriverMotors_Forward(&motor1); - motor1.target_speed = speed; - motor1.current_speed = 0; - motor1.state = MOTOR_STATE_FORWARD; - motor1.substate = MOTOR_SUBSTATE_ACCEL; - motor1.state_entry_time_ms = HAL_GetTick(); + for (uint8_t i = 0; i < motors_count; ++i) { + if (motors[i].substate == MOTOR_SUBSTATE_DECEL) { + ULOG_INFO("Motor %u: Can't move forward while decelerating (skipped)", i); + continue; + } + DriverMotors_Forward(&motors[i]); + motors[i].target_speed = speed; + motors[i].current_speed = 0; + motors[i].state = MOTOR_STATE_FORWARD; + motors[i].substate = MOTOR_SUBSTATE_ACCEL; + motors[i].state_entry_time_ms = HAL_GetTick(); + } } void MotorManager_MoveBackward(uint8_t speed) { - if (motor1.substate == MOTOR_SUBSTATE_DECEL) { - ULOG_INFO("Can't move backward while decelerating"); - return; - } - DriverMotors_Backward(&motor1); - motor1.target_speed = speed; - motor1.current_speed = 0; - motor1.state = MOTOR_STATE_BACKWARD; - motor1.substate = MOTOR_SUBSTATE_ACCEL; - motor1.state_entry_time_ms = HAL_GetTick(); + for (uint8_t i = 0; i < motors_count; ++i) { + if (motors[i].substate == MOTOR_SUBSTATE_DECEL) { + ULOG_INFO("Motor %u: Can't move backward while decelerating (skipped)", i); + continue; + } + DriverMotors_Backward(&motors[i]); + motors[i].target_speed = speed; + motors[i].current_speed = 0; + motors[i].state = MOTOR_STATE_BACKWARD; + motors[i].substate = MOTOR_SUBSTATE_ACCEL; + motors[i].state_entry_time_ms = HAL_GetTick(); + } } void MotorManager_Stop(void) { - if (motor1.state == MOTOR_STATE_FORWARD || motor1.state == MOTOR_STATE_BACKWARD) { - motor1.substate = MOTOR_SUBSTATE_DECEL; + for (uint8_t i = 0; i < motors_count; ++i) { + if (motors[i].state == MOTOR_STATE_FORWARD || motors[i].state == MOTOR_STATE_BACKWARD) { + motors[i].substate = MOTOR_SUBSTATE_DECEL; + } else { + DriverMotors_Stop(&motors[i]); + motors[i].state = MOTOR_STATE_STOP; + motors[i].substate = MOTOR_SUBSTATE_NONE; + } + } +} + + +void MotorManager_MoveForwardMotor(uint8_t id, uint8_t speed) { + Motor_HandleTypeDef* m = get_motor_by_id(id); + if (!m) { + ULOG_ERROR("MoveForwardMotor: invalid id %u", id); + return; + } + + for (uint8_t i = 0; i < motors_count; ++i) { + if (i != id) { + Motor_HandleTypeDef* other = &motors[i]; + if (other->state == MOTOR_STATE_FORWARD || other->state == MOTOR_STATE_BACKWARD) { + other->substate = MOTOR_SUBSTATE_DECEL; + } + } + } + + if (m->substate == MOTOR_SUBSTATE_DECEL) { + ULOG_INFO("Motor %u: Can't move forward while decelerating", id); + return; + } + + DriverMotors_Forward(m); + m->target_speed = speed; + m->current_speed = 0; + m->state = MOTOR_STATE_FORWARD; + m->substate = MOTOR_SUBSTATE_ACCEL; + m->state_entry_time_ms = HAL_GetTick(); + + ULOG_INFO("Motor %u: Move forward %u%%", id, speed); +} + +void MotorManager_MoveBackwardMotor(uint8_t id, uint8_t speed) { + Motor_HandleTypeDef* m = get_motor_by_id(id); + if (!m) { + ULOG_ERROR("MoveBackwardMotor: invalid id %u", id); + return; + } + + for (uint8_t i = 0; i < motors_count; ++i) { + if (i != id) { + Motor_HandleTypeDef* other = &motors[i]; + if (other->state == MOTOR_STATE_FORWARD || other->state == MOTOR_STATE_BACKWARD) { + other->substate = MOTOR_SUBSTATE_DECEL; + } + } + } + + if (m->substate == MOTOR_SUBSTATE_DECEL) { + ULOG_INFO("Motor %u: Can't move backward while decelerating", id); + return; + } + + DriverMotors_Backward(m); + m->target_speed = speed; + m->current_speed = 0; + m->state = MOTOR_STATE_BACKWARD; + m->substate = MOTOR_SUBSTATE_ACCEL; + m->state_entry_time_ms = HAL_GetTick(); + + ULOG_INFO("Motor %u: Move backward %u%%", id, speed); +} + +void MotorManager_StopMotor(uint8_t id) { + Motor_HandleTypeDef* m = get_motor_by_id(id); + if (!m) { + ULOG_ERROR("StopMotor: invalid id %u", id); + return; + } + + if (m->state == MOTOR_STATE_FORWARD || m->state == MOTOR_STATE_BACKWARD) { + m->substate = MOTOR_SUBSTATE_DECEL; } else { - DriverMotors_Stop(&motor1); - motor1.state = MOTOR_STATE_STOP; - motor1.substate = MOTOR_SUBSTATE_NONE; + DriverMotors_Stop(m); + m->state = MOTOR_STATE_STOP; + m->substate = MOTOR_SUBSTATE_NONE; } + + ULOG_INFO("Motor %u: Stop", id); } diff --git a/KPI_Rover/Motors/motors_manager.h b/KPI_Rover/Motors/motors_manager.h index 012faec..6c830ba 100644 --- a/KPI_Rover/Motors/motors_manager.h +++ b/KPI_Rover/Motors/motors_manager.h @@ -10,4 +10,8 @@ void MotorManager_MoveForward(uint8_t speed); void MotorManager_MoveBackward(uint8_t speed); void MotorManager_Stop(void); +void MotorManager_MoveForwardMotor(uint8_t id, uint8_t speed); +void MotorManager_MoveBackwardMotor(uint8_t id, uint8_t speed); +void MotorManager_StopMotor(uint8_t id); + #endif From 728ac092ebc8e5f0e42f8977e5df67639a63aa38 Mon Sep 17 00:00:00 2001 From: velichko Date: Tue, 18 Nov 2025 14:00:06 +0200 Subject: [PATCH 4/8] Issue #17: fixed architecture --- KPI_Rover/Motors/motors_driver.c | 129 +++---------- KPI_Rover/Motors/motors_driver.h | 48 ++--- KPI_Rover/Motors/motors_manager.c | 243 ------------------------ KPI_Rover/Motors/motors_manager.h | 17 -- KPI_Rover/Motors/ul_motors_controller.c | 225 ++++++++++++++++++++++ KPI_Rover/Motors/ul_motors_controller.h | 47 +++++ KPI_Rover/Motors/ul_pid.c | 169 ++++++++++++++++ KPI_Rover/Motors/ul_pid.h | 47 +++++ KPI_Rover/Motors/ul_rls.c | 76 ++++++++ KPI_Rover/Motors/ul_rls.h | 19 ++ 10 files changed, 622 insertions(+), 398 deletions(-) delete mode 100644 KPI_Rover/Motors/motors_manager.c delete mode 100644 KPI_Rover/Motors/motors_manager.h create mode 100644 KPI_Rover/Motors/ul_motors_controller.c create mode 100644 KPI_Rover/Motors/ul_motors_controller.h create mode 100644 KPI_Rover/Motors/ul_pid.c create mode 100644 KPI_Rover/Motors/ul_pid.h create mode 100644 KPI_Rover/Motors/ul_rls.c create mode 100644 KPI_Rover/Motors/ul_rls.h diff --git a/KPI_Rover/Motors/motors_driver.c b/KPI_Rover/Motors/motors_driver.c index 3b97a50..3163baf 100644 --- a/KPI_Rover/Motors/motors_driver.c +++ b/KPI_Rover/Motors/motors_driver.c @@ -1,123 +1,42 @@ #include "motors_driver.h" #include "ulog.h" -void DriverMotors_Init(Motor_HandleTypeDef* motor){ +void DriverMotor_Init(drvMotor_t* motor){ HAL_TIM_PWM_Start(motor->htim_pwm, motor->pwm_channel); __HAL_TIM_MOE_ENABLE(motor->htim_pwm); - DriverMotors_Stop(motor); -} -void DriverMotors_Forward(Motor_HandleTypeDef* motor){ - HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, GPIO_PIN_SET); - HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, GPIO_PIN_RESET); -} + DriverMotor_setPwm(motor, 0); + DriverMotor_setDirection(motor, true); + motor->enabled = false; -void DriverMotors_Backward(Motor_HandleTypeDef* motor){ - HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, GPIO_PIN_RESET); - HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, GPIO_PIN_SET); + ULOG_INFO("drvMotor_init: PWM on ch=%lu", motor->pwm_channel); } -void DriverMotors_Stop(Motor_HandleTypeDef* motor){ - HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, GPIO_PIN_RESET); - HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, GPIO_PIN_RESET); +void DriverMotor_Enable(drvMotor_t* motor){ + motor->enabled = true; + ULOG_INFO("Motor enabled (ch=%lu)", motor->pwm_channel); } -void DriverMotors_SetSpeed(Motor_HandleTypeDef* motor, uint8_t speed_percent) { - if (speed_percent > 100) speed_percent = 100; - uint32_t period = __HAL_TIM_GET_AUTORELOAD(motor->htim_pwm); - uint32_t compare = (uint32_t)(( (uint64_t)(period + 1) * speed_percent ) / 100); - __HAL_TIM_SET_COMPARE(motor->htim_pwm, motor->pwm_channel, compare); - ULOG_INFO("SetSpeed %u%% -> compare=%lu period=%lu", speed_percent, compare, period); +void DriverMotor_Disable(drvMotor_t* motor){ + DriverMotor_setPwm(motor, 0); + motor->enabled = false; + ULOG_INFO("Motor disabled (ch=%lu)", motor->pwm_channel); } -void DriverMotors_TimerTask(Motor_HandleTypeDef* motor) { - uint32_t now = HAL_GetTick(); - uint32_t elapsed = now - motor->state_entry_time_ms; - - switch (motor->state) { - - case MOTOR_STATE_STOP: - - break; - - case MOTOR_STATE_FORWARD: - switch (motor->substate) { - case MOTOR_SUBSTATE_ACCEL: - if (motor->current_speed < motor->target_speed) { - motor->current_speed += 15; - DriverMotors_SetSpeed(motor, motor->current_speed); - } else { - motor->substate = MOTOR_SUBSTATE_CONSTANT; - } - break; - - case MOTOR_SUBSTATE_CONSTANT: - - break; - - case MOTOR_SUBSTATE_DECEL: - if (motor->current_speed > 0) { - if (motor->current_speed > 15) - motor->current_speed -= 15; - else - motor->current_speed = 0; - - DriverMotors_SetSpeed(motor, motor->current_speed); - } else { - DriverMotors_SetSpeed(motor, 0); - DriverMotors_Stop(motor); - motor->state = MOTOR_STATE_STOP; - motor->substate = MOTOR_SUBSTATE_NONE; - ULOG_INFO("Motor stopped completely"); - } - break; - - - default: - break; - } - break; - - case MOTOR_STATE_BACKWARD: - switch (motor->substate) { - case MOTOR_SUBSTATE_ACCEL: - if (motor->current_speed < motor->target_speed) { - motor->current_speed += 15; - DriverMotors_SetSpeed(motor, motor->current_speed); - } else { - motor->substate = MOTOR_SUBSTATE_CONSTANT; - } - break; - - case MOTOR_SUBSTATE_CONSTANT: - - break; - - case MOTOR_SUBSTATE_DECEL: - if (motor->current_speed > 0) { - if (motor->current_speed > 15) - motor->current_speed -= 15; - else - motor->current_speed = 0; - - DriverMotors_SetSpeed(motor, motor->current_speed); - } else { - DriverMotors_SetSpeed(motor, 0); - DriverMotors_Stop(motor); - motor->state = MOTOR_STATE_STOP; - motor->substate = MOTOR_SUBSTATE_NONE; - ULOG_INFO("Motor stopped completely"); - } - break; +void DriverMotor_setDirection(drvMotor_t* motor, bool forward){ + HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, forward ? GPIO_PIN_SET : GPIO_PIN_RESET); + HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, forward ? GPIO_PIN_RESET : GPIO_PIN_SET); + ULOG_DEBUG("Motor dir=%s", forward ? "FWD" : "BWD"); +} - default: - break; - } - break; +void DriverMotor_setPwm(drvMotor_t* motor, uint16_t pwm) +{ + if (!motor->enabled) + return; - default: - break; - } + __HAL_TIM_SET_COMPARE(motor->htim_pwm, motor->pwm_channel, pwm); + //ULOG_DEBUG("Motor PWM set %u (ch=%lu)", pwm, motor->pwm_channel); } + diff --git a/KPI_Rover/Motors/motors_driver.h b/KPI_Rover/Motors/motors_driver.h index 1d8089d..55c8ea1 100644 --- a/KPI_Rover/Motors/motors_driver.h +++ b/KPI_Rover/Motors/motors_driver.h @@ -2,43 +2,25 @@ #define MOTORS_DRIVER_H #include "stm32f4xx_hal.h" - -typedef enum { - MOTOR_STATE_STOP = 0, - MOTOR_STATE_FORWARD, - MOTOR_STATE_BACKWARD, - MOTOR_STATE_BRAKE -} MotorState_t; - - -typedef enum { - MOTOR_SUBSTATE_NONE = 0, - MOTOR_SUBSTATE_ACCEL, - MOTOR_SUBSTATE_CONSTANT, - MOTOR_SUBSTATE_DECEL -} MotorSubState_t; - +#include typedef struct { GPIO_TypeDef* IN1_port; - uint16_t IN1_pin; - GPIO_TypeDef* IN2_port; - uint16_t IN2_pin; - TIM_HandleTypeDef* htim_pwm; - uint32_t pwm_channel; + uint16_t IN1_pin; + + GPIO_TypeDef* IN2_port; + uint16_t IN2_pin; + + TIM_HandleTypeDef* htim_pwm; + uint32_t pwm_channel; - MotorState_t state; - MotorSubState_t substate; - uint32_t state_entry_time_ms; - uint8_t target_speed; - uint8_t current_speed; -} Motor_HandleTypeDef; + bool enabled; +} drvMotor_t; -void DriverMotors_Init(Motor_HandleTypeDef* motor); -void DriverMotors_Forward(Motor_HandleTypeDef* motor); -void DriverMotors_Backward(Motor_HandleTypeDef* motor); -void DriverMotors_Stop(Motor_HandleTypeDef* motor); -void DriverMotors_SetSpeed(Motor_HandleTypeDef* motor, uint8_t speed_percent); -void DriverMotors_TimerTask(Motor_HandleTypeDef* motor); +void DriverMotor_Init(drvMotor_t* motor); +void DriverMotor_Enable(drvMotor_t* motor); +void DriverMotor_Disable(drvMotor_t* motor); +void DriverMotor_setDirection(drvMotor_t* motor, bool forward); +void DriverMotor_setPwm(drvMotor_t* motor, uint16_t pwm); #endif diff --git a/KPI_Rover/Motors/motors_manager.c b/KPI_Rover/Motors/motors_manager.c deleted file mode 100644 index 26a5d88..0000000 --- a/KPI_Rover/Motors/motors_manager.c +++ /dev/null @@ -1,243 +0,0 @@ -#include "motors_manager.h" -#include "cmsis_os.h" -#include "ulog.h" - -extern TIM_HandleTypeDef htim1; - -#define MOTOR_TIMER_PERIOD_MS 50u -#define MOTOR_MANAGER_MAX_MOTORS 4 - -static osTimerId_t motor_timer_handle; -static Motor_HandleTypeDef motors[MOTOR_MANAGER_MAX_MOTORS]; -static uint8_t motors_count = 0; -static void motor_timer_callback(void *argument); - - -static Motor_HandleTypeDef* get_motor_by_id(uint8_t id) { - if (id >= motors_count) return NULL; - return &motors[id]; -} - -static void register_motor(const Motor_HandleTypeDef* src, uint8_t id) { - if (id >= MOTOR_MANAGER_MAX_MOTORS) { - ULOG_ERROR("register_motor: id %u out of range", id); - return; - } - motors[id] = *src; - if (id >= motors_count) motors_count = id + 1; -} - - - -void MotorManager_Init(void) { - - Motor_HandleTypeDef m0; - m0.IN1_port = GPIOC; - m0.IN1_pin = GPIO_PIN_4; - m0.IN2_port = GPIOC; - m0.IN2_pin = GPIO_PIN_5; - m0.htim_pwm = &htim1; - m0.pwm_channel = TIM_CHANNEL_1; - m0.state = MOTOR_STATE_STOP; - m0.substate = MOTOR_SUBSTATE_NONE; - m0.state_entry_time_ms = HAL_GetTick(); - m0.target_speed = 0; - m0.current_speed = 0; - - register_motor(&m0, 0); - DriverMotors_Init(&motors[0]); - - - Motor_HandleTypeDef m1; - m1.IN1_port = GPIOE; - m1.IN1_pin = GPIO_PIN_7; - m1.IN2_port = GPIOE; - m1.IN2_pin = GPIO_PIN_8; - m1.htim_pwm = &htim1; - m1.pwm_channel = TIM_CHANNEL_2; - m1.state = MOTOR_STATE_STOP; - m1.substate = MOTOR_SUBSTATE_NONE; - m1.state_entry_time_ms = HAL_GetTick(); - m1.target_speed = 0; - m1.current_speed = 0; - - register_motor(&m1, 1); - DriverMotors_Init(&motors[1]); - - - - const osTimerAttr_t timer_attrs = { - .name = "Motor_Timer" - }; - - motor_timer_handle = osTimerNew(motor_timer_callback, osTimerPeriodic, NULL, &timer_attrs); - if (!motor_timer_handle) { - ULOG_ERROR("Failed to create Motor timer"); - osThreadExit(); - } - - osDelay(10); - - if (osTimerStart(motor_timer_handle, MOTOR_TIMER_PERIOD_MS) != osOK) { - ULOG_ERROR("Failed to start Motor timer"); - osThreadExit(); - } -} - -void MotorManager_Task(void* argument) { - MotorManager_Init(); - while(1) { - - MotorManager_MoveForward(100); - ULOG_INFO("MoveForward (all registered)"); - osDelay(4000); - - MotorManager_Stop(); - osDelay(1000); - - MotorManager_MoveBackward(55); - ULOG_INFO("MoveBackward (all registered)"); - osDelay(3000); - - MotorManager_MoveForwardMotor(0,100); - ULOG_INFO("MoveForward (one registered)"); - osDelay(2000); - - MotorManager_MoveForwardMotor(1,100); - ULOG_INFO("MoveForward (one registered)"); - osDelay(2000); - - MotorManager_Stop(); - osDelay(1000); - - } -} - -static void motor_timer_callback(void *argument) { - for (uint8_t i = 0; i < motors_count; ++i) { - DriverMotors_TimerTask(&motors[i]); - } -} - -void MotorManager_MoveForward(uint8_t speed) { - for (uint8_t i = 0; i < motors_count; ++i) { - if (motors[i].substate == MOTOR_SUBSTATE_DECEL) { - ULOG_INFO("Motor %u: Can't move forward while decelerating (skipped)", i); - continue; - } - DriverMotors_Forward(&motors[i]); - motors[i].target_speed = speed; - motors[i].current_speed = 0; - motors[i].state = MOTOR_STATE_FORWARD; - motors[i].substate = MOTOR_SUBSTATE_ACCEL; - motors[i].state_entry_time_ms = HAL_GetTick(); - } -} - -void MotorManager_MoveBackward(uint8_t speed) { - for (uint8_t i = 0; i < motors_count; ++i) { - if (motors[i].substate == MOTOR_SUBSTATE_DECEL) { - ULOG_INFO("Motor %u: Can't move backward while decelerating (skipped)", i); - continue; - } - DriverMotors_Backward(&motors[i]); - motors[i].target_speed = speed; - motors[i].current_speed = 0; - motors[i].state = MOTOR_STATE_BACKWARD; - motors[i].substate = MOTOR_SUBSTATE_ACCEL; - motors[i].state_entry_time_ms = HAL_GetTick(); - } -} - -void MotorManager_Stop(void) { - for (uint8_t i = 0; i < motors_count; ++i) { - if (motors[i].state == MOTOR_STATE_FORWARD || motors[i].state == MOTOR_STATE_BACKWARD) { - motors[i].substate = MOTOR_SUBSTATE_DECEL; - } else { - DriverMotors_Stop(&motors[i]); - motors[i].state = MOTOR_STATE_STOP; - motors[i].substate = MOTOR_SUBSTATE_NONE; - } - } -} - - -void MotorManager_MoveForwardMotor(uint8_t id, uint8_t speed) { - Motor_HandleTypeDef* m = get_motor_by_id(id); - if (!m) { - ULOG_ERROR("MoveForwardMotor: invalid id %u", id); - return; - } - - for (uint8_t i = 0; i < motors_count; ++i) { - if (i != id) { - Motor_HandleTypeDef* other = &motors[i]; - if (other->state == MOTOR_STATE_FORWARD || other->state == MOTOR_STATE_BACKWARD) { - other->substate = MOTOR_SUBSTATE_DECEL; - } - } - } - - if (m->substate == MOTOR_SUBSTATE_DECEL) { - ULOG_INFO("Motor %u: Can't move forward while decelerating", id); - return; - } - - DriverMotors_Forward(m); - m->target_speed = speed; - m->current_speed = 0; - m->state = MOTOR_STATE_FORWARD; - m->substate = MOTOR_SUBSTATE_ACCEL; - m->state_entry_time_ms = HAL_GetTick(); - - ULOG_INFO("Motor %u: Move forward %u%%", id, speed); -} - -void MotorManager_MoveBackwardMotor(uint8_t id, uint8_t speed) { - Motor_HandleTypeDef* m = get_motor_by_id(id); - if (!m) { - ULOG_ERROR("MoveBackwardMotor: invalid id %u", id); - return; - } - - for (uint8_t i = 0; i < motors_count; ++i) { - if (i != id) { - Motor_HandleTypeDef* other = &motors[i]; - if (other->state == MOTOR_STATE_FORWARD || other->state == MOTOR_STATE_BACKWARD) { - other->substate = MOTOR_SUBSTATE_DECEL; - } - } - } - - if (m->substate == MOTOR_SUBSTATE_DECEL) { - ULOG_INFO("Motor %u: Can't move backward while decelerating", id); - return; - } - - DriverMotors_Backward(m); - m->target_speed = speed; - m->current_speed = 0; - m->state = MOTOR_STATE_BACKWARD; - m->substate = MOTOR_SUBSTATE_ACCEL; - m->state_entry_time_ms = HAL_GetTick(); - - ULOG_INFO("Motor %u: Move backward %u%%", id, speed); -} - -void MotorManager_StopMotor(uint8_t id) { - Motor_HandleTypeDef* m = get_motor_by_id(id); - if (!m) { - ULOG_ERROR("StopMotor: invalid id %u", id); - return; - } - - if (m->state == MOTOR_STATE_FORWARD || m->state == MOTOR_STATE_BACKWARD) { - m->substate = MOTOR_SUBSTATE_DECEL; - } else { - DriverMotors_Stop(m); - m->state = MOTOR_STATE_STOP; - m->substate = MOTOR_SUBSTATE_NONE; - } - - ULOG_INFO("Motor %u: Stop", id); -} diff --git a/KPI_Rover/Motors/motors_manager.h b/KPI_Rover/Motors/motors_manager.h deleted file mode 100644 index 6c830ba..0000000 --- a/KPI_Rover/Motors/motors_manager.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef MOTORS_MANAGER_H -#define MOTORS_MANAGER_H - -#include "motors_driver.h" - - -void MotorManager_Init(void); -void MotorManager_Task(void* argument); -void MotorManager_MoveForward(uint8_t speed); -void MotorManager_MoveBackward(uint8_t speed); -void MotorManager_Stop(void); - -void MotorManager_MoveForwardMotor(uint8_t id, uint8_t speed); -void MotorManager_MoveBackwardMotor(uint8_t id, uint8_t speed); -void MotorManager_StopMotor(uint8_t id); - -#endif diff --git a/KPI_Rover/Motors/ul_motors_controller.c b/KPI_Rover/Motors/ul_motors_controller.c new file mode 100644 index 0000000..0e40e41 --- /dev/null +++ b/KPI_Rover/Motors/ul_motors_controller.c @@ -0,0 +1,225 @@ +#include "ul_motors_controller.h" +#include "ulog.h" +#include "cmsis_os.h" +#include + +extern TIM_HandleTypeDef htim1; +static osTimerId_t motors_timer_handle; + +#define MOTORS_CONTROL_PERIOD_MS 20 + +ulMotorsController_t g_motors_ctrl; +MotorsCtrlState_t g_motors_state = MOTORS_STATE_INIT; + + +static void init_motors_hw_mapping(ulMotorsController_t* ctrl) +{ + // Motor 0 + ctrl->motors[0].IN1_port = GPIOC; + ctrl->motors[0].IN1_pin = GPIO_PIN_4; + ctrl->motors[0].IN2_port = GPIOC; + ctrl->motors[0].IN2_pin = GPIO_PIN_5; + ctrl->motors[0].htim_pwm = &htim1; + ctrl->motors[0].pwm_channel = TIM_CHANNEL_1; + + // Motor 1 + ctrl->motors[1].IN1_port = GPIOE; + ctrl->motors[1].IN1_pin = GPIO_PIN_7; + ctrl->motors[1].IN2_port = GPIOE; + ctrl->motors[1].IN2_pin = GPIO_PIN_8; + ctrl->motors[1].htim_pwm = &htim1; + ctrl->motors[1].pwm_channel = TIM_CHANNEL_2; +} + + +void ulMotorsController_Init(ulMotorsController_t* ctrl) +{ + ULOG_INFO("ENTER ulMotorsController_Init"); + + init_motors_hw_mapping(ctrl); + + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + + float kp = 5.0f, ki = 0.015f, kd = 0.0f; + ulPID_Init(&ctrl->pids[i], kp, ki, kd); + + ulRLS_Init(&ctrl->rls[i]); + + ctrl->last_pwm[i] = 0.0f; + ctrl->adapt_counter[i] = 0; + ctrl->pwm_max[i] = 0.0f; + + ctrl->setpoint_rpm[i] = 0.0f; + ctrl->measured_rpm[i] = 0.0f; + ctrl->scale_filtered[i] = 1.0f; + + + uint32_t arr = __HAL_TIM_GET_AUTORELOAD(ctrl->motors[i].htim_pwm); + ulPID_SetOutputLimits(&ctrl->pids[i], 0.0f, (float)arr); + ctrl->pwm_max[i] = (float)arr; + + ULOG_INFO("Motor %d init: PID(Kp=%.3f Ki=%.3f Kd=%.3f), ARR=%lu, pwm_ch=%lu", + i, kp, ki, kd, arr, ctrl->motors[i].pwm_channel); + } + //g_motors_state = MOTORS_STATE_IDLE; + +} + + +void ulMotorsController_Run(ulMotorsController_t* ctrl) +{ + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + + float setpoint = ctrl->setpoint_rpm[i]; + float rpm = ctrl->measured_rpm[i]; + + bool forward = (setpoint >= 0.0f); + DriverMotor_setDirection(&ctrl->motors[i], forward); + + float pwm = ulPID_Compute(&ctrl->pids[i], setpoint, rpm); + + + if (pwm < 0.0f) pwm = 0.0f; + if (pwm > ctrl->pwm_max[i]) pwm = ctrl->pwm_max[i]; + + ulRLS_Update(&ctrl->rls[i], rpm, ctrl->last_pwm[i]); + + if (++ctrl->adapt_counter[i] >= 100) { + ctrl->adapt_counter[i] = 0; + + float setpoint = ctrl->setpoint_rpm[i]; + + if (fabsf(setpoint) > 300.0f && fabsf(rpm) > 100.0f) { + + ulPID_AutoTune_FromRLS(&ctrl->pids[i], + &ctrl->rls[i], + MOTORS_CONTROL_PERIOD_MS / 1000.0f); + + ULOG_INFO("Motor[%d] autotune: Kp=%.3f Ki=%.3f a=%.3f b=%.3f", + i, + ctrl->pids[i].kp, + ctrl->pids[i].ki, + ctrl->rls[i].theta[0], + ctrl->rls[i].theta[1]); + } + } + + DriverMotor_setPwm(&ctrl->motors[i], (uint16_t)pwm); + ctrl->last_pwm[i] = pwm; + + ULOG_DEBUG("MOTOR[%d] sp=%d rpm=%d pwm=%d dir=%s", + i, (int)setpoint, (int)rpm, (int)pwm, + forward ? "FWD" : "BWD"); + } +} + + +static void MotorsTimerCallback(void *argument) +{ + (void)argument; + + switch (g_motors_state) + { + case MOTORS_STATE_INIT: + ULOG_INFO("Motors state: INIT -> initializing HW"); + + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + DriverMotor_Init(&g_motors_ctrl.motors[i]); + } + + g_motors_state = MOTORS_STATE_IDLE; + ULOG_INFO("Motors state: INIT -> IDLE"); + break; + + case MOTORS_STATE_IDLE: + { + bool need_run = false; + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + if (fabsf(g_motors_ctrl.setpoint_rpm[i]) > 1.0f) { + need_run = true; + break; + } + } + + if (need_run) { + ULOG_INFO("Motors state: IDLE -> RUN"); + + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + DriverMotor_Enable(&g_motors_ctrl.motors[i]); + } + + g_motors_state = MOTORS_STATE_RUN; + } + break; + } + + case MOTORS_STATE_RUN: + { + ulMotorsController_Run(&g_motors_ctrl); + + bool all_zero = true; + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + if (fabsf(g_motors_ctrl.setpoint_rpm[i]) > 1.0f) { + all_zero = false; + break; + } + } + if (all_zero) { + ULOG_INFO("Motors state: RUN -> IDLE (all setpoints ~0)"); + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + DriverMotor_setPwm(&g_motors_ctrl.motors[i], 0); + g_motors_ctrl.last_pwm[i] = 0.0f; + } + g_motors_state = MOTORS_STATE_IDLE; + } + break; + } + + case MOTORS_STATE_ERROR: + default: + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + DriverMotor_setPwm(&g_motors_ctrl.motors[i], 0); + g_motors_ctrl.last_pwm[i] = 0.0f; + } + break; + } +} + + +void ulMotorsController_Task(void* argument) +{ + (void)argument; + + ulMotorsController_Init(&g_motors_ctrl); + + const osTimerAttr_t timer_attrs = { + .name = "Motors_Timer" + }; + + motors_timer_handle = osTimerNew(MotorsTimerCallback, osTimerPeriodic, NULL, &timer_attrs); + if (!motors_timer_handle) { + ULOG_ERROR("Failed to create Motors timer"); + osThreadExit(); + } + + osDelay(10); + + if (osTimerStart(motors_timer_handle, MOTORS_CONTROL_PERIOD_MS) != osOK) { + ULOG_ERROR("Failed to start Motors timer"); + osThreadExit(); + } + + + + for (;;) { + + g_motors_ctrl.setpoint_rpm[0] = 1200.0f; + g_motors_ctrl.setpoint_rpm[1] = 1200.0f; + g_motors_ctrl.measured_rpm[0] = 1150.0f; + g_motors_ctrl.measured_rpm[1] = 1150.0f; + + + + osDelay(10); + } +} diff --git a/KPI_Rover/Motors/ul_motors_controller.h b/KPI_Rover/Motors/ul_motors_controller.h new file mode 100644 index 0000000..ad44254 --- /dev/null +++ b/KPI_Rover/Motors/ul_motors_controller.h @@ -0,0 +1,47 @@ +#ifndef UL_MOTORS_CONTROLLER_H +#define UL_MOTORS_CONTROLLER_H + +#include +#include "motors_driver.h" +#include "ul_pid.h" +#include "ul_rls.h" + +#define ULMOTORS_NUM_MOTORS 2 + +typedef struct { + drvMotor_t motors[ULMOTORS_NUM_MOTORS]; + ulPID_t pids[ULMOTORS_NUM_MOTORS]; + ulRLS_t rls[ULMOTORS_NUM_MOTORS]; + + float last_pwm[ULMOTORS_NUM_MOTORS]; + uint16_t adapt_counter[ULMOTORS_NUM_MOTORS]; + + float pwm_max[ULMOTORS_NUM_MOTORS]; + + uint16_t period_ms; + + float measured_rpm[ULMOTORS_NUM_MOTORS]; + float setpoint_rpm[ULMOTORS_NUM_MOTORS]; + float scale_filtered[ULMOTORS_NUM_MOTORS]; +} ulMotorsController_t; + +typedef enum +{ + MOTORS_STATE_INIT = 0, + MOTORS_STATE_IDLE, + MOTORS_STATE_RUN, + MOTORS_STATE_ERROR +} MotorsCtrlState_t; + +extern ulMotorsController_t g_motors_ctrl; + +extern MotorsCtrlState_t g_motors_state; + + +void ulMotorsController_Init(ulMotorsController_t* ctrl); +void ulMotorsController_Run(ulMotorsController_t* ctrl); +void ulMotorsController_Task(void* argument); + +void ulMotorsController_RetunePid(ulMotorsController_t* ctrl, int i); + +#endif // UL_MOTORS_CONTROLLER_H diff --git a/KPI_Rover/Motors/ul_pid.c b/KPI_Rover/Motors/ul_pid.c new file mode 100644 index 0000000..905c5b1 --- /dev/null +++ b/KPI_Rover/Motors/ul_pid.c @@ -0,0 +1,169 @@ +#include "ul_pid.h" +#include +#include +#include "ul_rls.h" +#include "stm32f4xx_hal.h" + +#define PID_AW_GAIN 0.2f + +void ulPID_Init(ulPID_t* pid, float kp, float ki, float kd){ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + + pid->integral = 0.0f; + pid->prev_meas = 0.0f; + pid->d_filtered = 0.0f; + + pid->out_min = 0.0f; + pid->out_max = 1000.0f; + + pid->int_min = -500.0f; + pid->int_max = 500.0f; + + pid->sp_filtered = 0.0f; + pid->sp_alpha = 0.25f; + + pid->d_alpha = 0.4f; + pid->slew_rate = 20.0f; + pid->deadzone = 300.0f; + + pid->last_output = 0.0f; + + pid->last_time = HAL_GetTick(); + pid->initialized = false; +} + +void ulPID_Reset(ulPID_t* pid){ + pid->integral = 0.0f; + pid->prev_meas = 0.0f; + pid->d_filtered = 0.0f; + pid->last_output = 0.0f; + pid->sp_filtered = 0.0f; + pid->initialized = false; +} + +void ulPID_ResetI(ulPID_t* pid){ + pid->integral = 0.0f; +} + +void ulPID_SetParams(ulPID_t* pid, float kp, float ki, float kd){ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; +} + +void ulPID_SetOutputLimits(ulPID_t* pid, float out_min, float out_max){ + pid->out_min = out_min; + pid->out_max = out_max; +} + +void ulPID_SetIntegralLimits(ulPID_t* pid, float int_min, float int_max){ + pid->int_min = int_min; + pid->int_max = int_max; +} + +float ulPID_Compute(ulPID_t* pid, float setpoint, float measured){ + uint32_t now = HAL_GetTick(); + float dt = (now - pid->last_time) / 1000.0f; + pid->last_time = now; + + if (dt <= 0.0f) dt = 0.001f; + if (dt > 0.1f) dt = 0.1f; + + if (!pid->initialized) + { + pid->sp_filtered = setpoint; + pid->prev_meas = measured; + pid->d_filtered = 0.0f; + pid->last_output = 0.0f; + pid->initialized = true; + } + + pid->sp_filtered += pid->sp_alpha * (setpoint - pid->sp_filtered); + float sp = pid->sp_filtered; + float error = sp - measured; + + pid->integral += error * dt; + if (pid->integral > pid->int_max) pid->integral = pid->int_max; + if (pid->integral < pid->int_min) pid->integral = pid->int_min; + + float d_meas = (measured - pid->prev_meas) / dt; + pid->prev_meas = measured; + + float d_raw = -d_meas; + pid->d_filtered += pid->d_alpha * (d_raw - pid->d_filtered); + + float out_unsat = + pid->kp * error + + pid->ki * pid->integral + + pid->kd * pid->d_filtered; + + float out_clamped = out_unsat; + if (out_clamped > pid->out_max) out_clamped = pid->out_max; + if (out_clamped < pid->out_min) out_clamped = pid->out_min; + + if (pid->ki != 0.0f) + { + float aw = out_clamped - out_unsat; + pid->integral += (PID_AW_GAIN * aw) / pid->ki; + + if (pid->integral > pid->int_max) pid->integral = pid->int_max; + if (pid->integral < pid->int_min) pid->integral = pid->int_min; + } + + float actuator = out_clamped; + + if (fabsf(actuator) > 0.0f) + { + if (actuator > 0.0f) + actuator += pid->deadzone; + else + actuator -= pid->deadzone; + + if (actuator > pid->out_max) actuator = pid->out_max; + if (actuator < pid->out_min) actuator = pid->out_min; + } + + float diff = actuator - pid->last_output; + float max_change = pid->slew_rate * dt; + + if (diff > max_change) + actuator = pid->last_output + max_change; + else if (diff < -max_change) + actuator = pid->last_output - max_change; + + pid->last_output = actuator; + + return actuator; +} + +void ulPID_AutoTune_FromRLS(ulPID_t* pid, const ulRLS_t* rls, float Ts){ + float a = rls->theta[0]; + float b = rls->theta[1]; + + if (a > 0.999f) a = 0.999f; + if (a < 0.01f) a = 0.01f; + + float tau = -Ts / logf(a); + float K = b / (1.0f - a); + + if (K < 0.01f) K = 0.01f; + if (K > 1000.0f) K = 1000.0f; + + float lambda = 0.25f * tau; + if (lambda < 0.001f) lambda = 0.001f; + + float kp = tau / (K * lambda); + float ki = 1.0f / (K * lambda); + + if (kp < 0.0f) kp = 0.0f; + if (kp > 300.0f) kp = 300.0f; + + if (ki < 0.0f) ki = 0.0f; + if (ki > 300.0f) ki = 300.0f; + + pid->kp = kp; + pid->ki = ki; + pid->kd = 0.0f; +} diff --git a/KPI_Rover/Motors/ul_pid.h b/KPI_Rover/Motors/ul_pid.h new file mode 100644 index 0000000..43c0f23 --- /dev/null +++ b/KPI_Rover/Motors/ul_pid.h @@ -0,0 +1,47 @@ +#ifndef UL_PID_H +#define UL_PID_H + +#include +#include +#include "stm32f4xx_hal.h" + +struct ulRLS_s; +typedef struct ulRLS_s ulRLS_t; + +typedef struct { + float kp; + float ki; + float kd; + + float integral; + float prev_meas; + float d_filtered; + + float out_min; + float out_max; + + float int_min; + float int_max; + + float sp_filtered; + float sp_alpha; + + float d_alpha; + float last_output; + float slew_rate; + + float deadzone; + + uint32_t last_time; + + bool initialized; +} ulPID_t; + +void ulPID_Init(ulPID_t* pid, float kp, float ki, float kd); +void ulPID_Reset(ulPID_t* pid); +void ulPID_SetParams(ulPID_t* pid, float kp, float ki, float kd); +void ulPID_SetOutputLimits(ulPID_t* pid, float out_min, float out_max); +float ulPID_Compute(ulPID_t* pid, float setpoint, float measured); +void ulPID_AutoTune_FromRLS(ulPID_t* pid, const ulRLS_t* rls, float Ts); +void ulPID_ResetI(ulPID_t* pid); +#endif // UL_PID_H diff --git a/KPI_Rover/Motors/ul_rls.c b/KPI_Rover/Motors/ul_rls.c new file mode 100644 index 0000000..f02b280 --- /dev/null +++ b/KPI_Rover/Motors/ul_rls.c @@ -0,0 +1,76 @@ +#include "ul_rls.h" +#include + +void ulRLS_Init(ulRLS_t* rls) +{ + rls->theta[0] = 0.8f; // Початкове a + rls->theta[1] = 0.2f; // Початкове b + + // Початкова коваріаційна матриця (високе початкове значення для швидкої адаптації) + rls->P[0][0] = 500.0f; + rls->P[0][1] = 0.0f; + rls->P[1][0] = 0.0f; + rls->P[1][1] = 500.0f; + + rls->lambda = 0.995f; + + rls->last_w = 0.0f; + rls->last_u = 0.0f; + rls->initialized = 0; +} + +void ulRLS_Update(ulRLS_t* rls, float w_k, float u_k){ + + if (!rls->initialized) { + rls->last_w = w_k; + rls->last_u = u_k; + rls->initialized = 1; + return; + } + + float phi0 = rls->last_w; + float phi1 = rls->last_u; + + float y = w_k; + + float Pphi0 = rls->P[0][0]*phi0 + rls->P[0][1]*phi1; + float Pphi1 = rls->P[1][0]*phi0 + rls->P[1][1]*phi1; + + float denom = rls->lambda + phi0*Pphi0 + phi1*Pphi1; + + if (fabsf(denom) < 1e-6f) + denom = (denom >= 0.0f) ? 1e-6f : -1e-6f; + + float K0 = Pphi0 / denom; + float K1 = Pphi1 / denom; + + float a = rls->theta[0]; + float b = rls->theta[1]; + + float y_hat = a*phi0 + b*phi1; + float eps = y - y_hat; + + float new_a = a + K0 * eps; + float new_b = b + K1 * eps; + + if (new_a < 0.7f) new_a = 0.7f; + if (new_a > 1.3f) new_a = 1.3f; + if (new_b < 0.0f) new_b = 0.0f; + if (new_b > 3.0f) new_b = 3.0f; + + rls->theta[0] = new_a; + rls->theta[1] = new_b; + + float P00_new = (rls->P[0][0] - K0 * Pphi0) / rls->lambda; + float P11_new = (rls->P[1][1] - K1 * Pphi1) / rls->lambda; + float P01_new = (rls->P[0][1] - K0 * Pphi1) / rls->lambda; + + rls->P[0][0] = P00_new; + rls->P[1][1] = P11_new; + + rls->P[0][1] = P01_new; + rls->P[1][0] = P01_new; + + rls->last_w = w_k; + rls->last_u = u_k; +} diff --git a/KPI_Rover/Motors/ul_rls.h b/KPI_Rover/Motors/ul_rls.h new file mode 100644 index 0000000..b3ebf57 --- /dev/null +++ b/KPI_Rover/Motors/ul_rls.h @@ -0,0 +1,19 @@ +#ifndef UL_RLS_H +#define UL_RLS_H + +#include + +typedef struct ulRLS_s { + float theta[2]; + float P[2][2]; + float lambda; + + float last_w; + float last_u; + uint8_t initialized; +} ulRLS_t; + +void ulRLS_Init(ulRLS_t* rls); +void ulRLS_Update(ulRLS_t* rls, float w_k, float u_k_1); + +#endif // UL_RLS_H From 7a2eb96735183bff9d0572e8b10831b14ee4a8b7 Mon Sep 17 00:00:00 2001 From: velichko Date: Tue, 2 Dec 2025 01:57:51 +0200 Subject: [PATCH 5/8] Issue #17: checked pin updates --- Core/Src/main.c | 50 ++----------------- KPI_Rover/KPIRover.c | 2 +- .../Motors/{motors_driver.c => drvMotors.c} | 2 +- .../Motors/{motors_driver.h => drvMotors.h} | 0 ...tors_controller.c => ulMotorsController.c} | 28 +++++------ ...tors_controller.h => ulMotorsController.h} | 6 +-- KPI_Rover/Motors/{ul_pid.c => ulPID.c} | 4 +- KPI_Rover/Motors/{ul_pid.h => ulPID.h} | 0 KPI_Rover/Motors/{ul_rls.c => ulRLS.c} | 2 +- KPI_Rover/Motors/{ul_rls.h => ulRLS.h} | 0 10 files changed, 26 insertions(+), 68 deletions(-) rename KPI_Rover/Motors/{motors_driver.c => drvMotors.c} (97%) rename KPI_Rover/Motors/{motors_driver.h => drvMotors.h} (100%) rename KPI_Rover/Motors/{ul_motors_controller.c => ulMotorsController.c} (91%) rename KPI_Rover/Motors/{ul_motors_controller.h => ulMotorsController.h} (92%) rename KPI_Rover/Motors/{ul_pid.c => ulPID.c} (98%) rename KPI_Rover/Motors/{ul_pid.h => ulPID.h} (100%) rename KPI_Rover/Motors/{ul_rls.c => ulRLS.c} (98%) rename KPI_Rover/Motors/{ul_rls.h => ulRLS.h} (100%) diff --git a/Core/Src/main.c b/Core/Src/main.c index ca657d9..559d1fd 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -455,9 +455,9 @@ static void MX_TIM1_Init(void) /* USER CODE END TIM1_Init 1 */ htim1.Instance = TIM1; - htim1.Init.Prescaler = 167; + htim1.Init.Prescaler = 0; htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 999; + htim1.Init.Period = 65535; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; @@ -718,48 +718,6 @@ static void MX_USART3_UART_Init(void) } -/** - * @brief TIM12 Initialization Function - * @param None - * @retval None - */ -static void MX_TIM12_Init(void) -{ - - /* USER CODE BEGIN TIM12_Init 0 */ - - /* USER CODE END TIM12_Init 0 */ - - TIM_OC_InitTypeDef sConfigOC = {0}; - - /* USER CODE BEGIN TIM12_Init 1 */ - - /* USER CODE END TIM12_Init 1 */ - htim12.Instance = TIM12; - htim12.Init.Prescaler = 0; - htim12.Init.CounterMode = TIM_COUNTERMODE_UP; - htim12.Init.Period = 65535; - htim12.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - htim12.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - if (HAL_TIM_PWM_Init(&htim12) != HAL_OK) - { - Error_Handler(); - } - sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 0; - sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - if (HAL_TIM_PWM_ConfigChannel(&htim12, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN TIM12_Init 2 */ - - /* USER CODE END TIM12_Init 2 */ - HAL_TIM_MspPostInit(&htim12); - -} - /** * @brief GPIO Initialization Function * @param None @@ -803,11 +761,11 @@ static void MX_GPIO_Init(void) HAL_GPIO_Init(GPIOE, &GPIO_InitStruct); /*Configure GPIO pins : OTG_FS_PowerSwitchOn_Pin PC4 PC5 */ - GPIO_InitStruct.Pin = OTG_FS_PowerSwitchOn_Pin|GPIO_PIN_4|GPIO_PIN_5; + GPIO_InitStruct.Pin = OTG_FS_PowerSwitchOn_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + HAL_GPIO_Init(OTG_FS_PowerSwitchOn_GPIO_Port, &GPIO_InitStruct); /*Configure GPIO pin : PDM_OUT_Pin */ GPIO_InitStruct.Pin = PDM_OUT_Pin; diff --git a/KPI_Rover/KPIRover.c b/KPI_Rover/KPIRover.c index d04d367..b13936c 100644 --- a/KPI_Rover/KPIRover.c +++ b/KPI_Rover/KPIRover.c @@ -1,6 +1,6 @@ +#include #include "KPIRover.h" #include "cmsis_os.h" - #include "Database/ulDatabase.h" diff --git a/KPI_Rover/Motors/motors_driver.c b/KPI_Rover/Motors/drvMotors.c similarity index 97% rename from KPI_Rover/Motors/motors_driver.c rename to KPI_Rover/Motors/drvMotors.c index 3163baf..bd25a49 100644 --- a/KPI_Rover/Motors/motors_driver.c +++ b/KPI_Rover/Motors/drvMotors.c @@ -1,4 +1,4 @@ -#include "motors_driver.h" +#include #include "ulog.h" void DriverMotor_Init(drvMotor_t* motor){ diff --git a/KPI_Rover/Motors/motors_driver.h b/KPI_Rover/Motors/drvMotors.h similarity index 100% rename from KPI_Rover/Motors/motors_driver.h rename to KPI_Rover/Motors/drvMotors.h diff --git a/KPI_Rover/Motors/ul_motors_controller.c b/KPI_Rover/Motors/ulMotorsController.c similarity index 91% rename from KPI_Rover/Motors/ul_motors_controller.c rename to KPI_Rover/Motors/ulMotorsController.c index 0e40e41..26cf429 100644 --- a/KPI_Rover/Motors/ul_motors_controller.c +++ b/KPI_Rover/Motors/ulMotorsController.c @@ -1,9 +1,9 @@ -#include "ul_motors_controller.h" #include "ulog.h" #include "cmsis_os.h" #include +#include -extern TIM_HandleTypeDef htim1; +extern TIM_HandleTypeDef htim2; static osTimerId_t motors_timer_handle; #define MOTORS_CONTROL_PERIOD_MS 20 @@ -14,21 +14,21 @@ MotorsCtrlState_t g_motors_state = MOTORS_STATE_INIT; static void init_motors_hw_mapping(ulMotorsController_t* ctrl) { - // Motor 0 - ctrl->motors[0].IN1_port = GPIOC; - ctrl->motors[0].IN1_pin = GPIO_PIN_4; - ctrl->motors[0].IN2_port = GPIOC; - ctrl->motors[0].IN2_pin = GPIO_PIN_5; - ctrl->motors[0].htim_pwm = &htim1; - ctrl->motors[0].pwm_channel = TIM_CHANNEL_1; - // Motor 1 + ctrl->motors[0].IN1_port = GPIOE; + ctrl->motors[0].IN1_pin = GPIO_PIN_2; + ctrl->motors[0].IN2_port = GPIOE; + ctrl->motors[0].IN2_pin = GPIO_PIN_3; + ctrl->motors[0].htim_pwm = &htim2; + ctrl->motors[0].pwm_channel = TIM_CHANNEL_3; + + // Motor 2 ctrl->motors[1].IN1_port = GPIOE; - ctrl->motors[1].IN1_pin = GPIO_PIN_7; + ctrl->motors[1].IN1_pin = GPIO_PIN_5; ctrl->motors[1].IN2_port = GPIOE; - ctrl->motors[1].IN2_pin = GPIO_PIN_8; - ctrl->motors[1].htim_pwm = &htim1; - ctrl->motors[1].pwm_channel = TIM_CHANNEL_2; + ctrl->motors[1].IN2_pin = GPIO_PIN_6; + ctrl->motors[1].htim_pwm = &htim2; + ctrl->motors[1].pwm_channel = TIM_CHANNEL_4; } diff --git a/KPI_Rover/Motors/ul_motors_controller.h b/KPI_Rover/Motors/ulMotorsController.h similarity index 92% rename from KPI_Rover/Motors/ul_motors_controller.h rename to KPI_Rover/Motors/ulMotorsController.h index ad44254..9947c2f 100644 --- a/KPI_Rover/Motors/ul_motors_controller.h +++ b/KPI_Rover/Motors/ulMotorsController.h @@ -1,10 +1,10 @@ #ifndef UL_MOTORS_CONTROLLER_H #define UL_MOTORS_CONTROLLER_H +#include +#include +#include #include -#include "motors_driver.h" -#include "ul_pid.h" -#include "ul_rls.h" #define ULMOTORS_NUM_MOTORS 2 diff --git a/KPI_Rover/Motors/ul_pid.c b/KPI_Rover/Motors/ulPID.c similarity index 98% rename from KPI_Rover/Motors/ul_pid.c rename to KPI_Rover/Motors/ulPID.c index 905c5b1..575d2cd 100644 --- a/KPI_Rover/Motors/ul_pid.c +++ b/KPI_Rover/Motors/ulPID.c @@ -1,7 +1,7 @@ -#include "ul_pid.h" #include +#include +#include #include -#include "ul_rls.h" #include "stm32f4xx_hal.h" #define PID_AW_GAIN 0.2f diff --git a/KPI_Rover/Motors/ul_pid.h b/KPI_Rover/Motors/ulPID.h similarity index 100% rename from KPI_Rover/Motors/ul_pid.h rename to KPI_Rover/Motors/ulPID.h diff --git a/KPI_Rover/Motors/ul_rls.c b/KPI_Rover/Motors/ulRLS.c similarity index 98% rename from KPI_Rover/Motors/ul_rls.c rename to KPI_Rover/Motors/ulRLS.c index f02b280..488ffde 100644 --- a/KPI_Rover/Motors/ul_rls.c +++ b/KPI_Rover/Motors/ulRLS.c @@ -1,5 +1,5 @@ -#include "ul_rls.h" #include +#include void ulRLS_Init(ulRLS_t* rls) { diff --git a/KPI_Rover/Motors/ul_rls.h b/KPI_Rover/Motors/ulRLS.h similarity index 100% rename from KPI_Rover/Motors/ul_rls.h rename to KPI_Rover/Motors/ulRLS.h From 4933bb91f03a26a3f8d0210d6dd9b2dcd209756a Mon Sep 17 00:00:00 2001 From: velichko Date: Mon, 15 Dec 2025 13:45:43 +0200 Subject: [PATCH 6/8] Issue #17: added driver for PCA --- KPI_Rover/Motors/PCA9685.c | 68 +++++++++++++++++++ KPI_Rover/Motors/PCA9685.h | 5 ++ KPI_Rover/Motors/drvMotors.c | 51 ++++++++++----- KPI_Rover/Motors/drvMotors.h | 29 +++++++-- KPI_Rover/Motors/ulMotorsController.c | 94 +++++++++++++-------------- 5 files changed, 174 insertions(+), 73 deletions(-) create mode 100644 KPI_Rover/Motors/PCA9685.c create mode 100644 KPI_Rover/Motors/PCA9685.h diff --git a/KPI_Rover/Motors/PCA9685.c b/KPI_Rover/Motors/PCA9685.c new file mode 100644 index 0000000..fc5573e --- /dev/null +++ b/KPI_Rover/Motors/PCA9685.c @@ -0,0 +1,68 @@ +#include "PCA9685.h" +#include "stm32f4xx_hal.h" + +/* ЗОВНІШНІЙ I2C */ +extern I2C_HandleTypeDef hi2c1; + +/* PCA9685 I2C address (0x40 default) */ +#define PCA9685_ADDR (0x40 << 1) + +#define MODE1 0x00 +#define PRESCALE 0xFE +#define LED0_ON_L 0x06 + +static void pca9685_write(uint8_t reg, uint8_t data) +{ + HAL_I2C_Mem_Write(&hi2c1, + PCA9685_ADDR, + reg, + I2C_MEMADD_SIZE_8BIT, + &data, + 1, + HAL_MAX_DELAY); +} + +static void pca9685_write4(uint8_t reg, uint16_t on, uint16_t off) +{ + uint8_t buf[4]; + buf[0] = on & 0xFF; + buf[1] = on >> 8; + buf[2] = off & 0xFF; + buf[3] = off >> 8; + + HAL_I2C_Mem_Write(&hi2c1, + PCA9685_ADDR, + reg, + I2C_MEMADD_SIZE_8BIT, + buf, + 4, + HAL_MAX_DELAY); +} + +void PCA9685_Init(void) +{ + pca9685_write(MODE1, 0x00); + HAL_Delay(10); + + /* === Set frequency ~1 kHz === */ + uint8_t prescale = 5; // ≈1017 Hz + + pca9685_write(MODE1, 0x10); + HAL_Delay(1); + + pca9685_write(PRESCALE, prescale); + + pca9685_write(MODE1, 0xA1); + HAL_Delay(5); +} + +void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off) +{ + if (channel > 15) + return; + + if (off > 4095) + off = 4095; + + pca9685_write4(LED0_ON_L + 4 * channel, on, off); +} diff --git a/KPI_Rover/Motors/PCA9685.h b/KPI_Rover/Motors/PCA9685.h new file mode 100644 index 0000000..75bcdc1 --- /dev/null +++ b/KPI_Rover/Motors/PCA9685.h @@ -0,0 +1,5 @@ +#pragma once +#include + +void PCA9685_Init(void); +void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off); diff --git a/KPI_Rover/Motors/drvMotors.c b/KPI_Rover/Motors/drvMotors.c index bd25a49..792de43 100644 --- a/KPI_Rover/Motors/drvMotors.c +++ b/KPI_Rover/Motors/drvMotors.c @@ -1,26 +1,33 @@ #include #include "ulog.h" -void DriverMotor_Init(drvMotor_t* motor){ - HAL_TIM_PWM_Start(motor->htim_pwm, motor->pwm_channel); - __HAL_TIM_MOE_ENABLE(motor->htim_pwm); +void DriverMotor_Init(drvMotor_t* motor) +{ + if (motor->pwm_src == PWM_SRC_TIM) { + HAL_TIM_PWM_Start(motor->pwm.tim.htim, motor->pwm.tim.channel); + __HAL_TIM_MOE_ENABLE(motor->pwm.tim.htim); + } + else if (motor->pwm_src == PWM_SRC_PCA9685) { + PCA9685_SetPWM(motor->pwm.pca.channel, 0, 0); + } - DriverMotor_setPwm(motor, 0); - DriverMotor_setDirection(motor, true); - motor->enabled = false; + motor->enabled = false; + DriverMotor_setDirection(motor, true); - ULOG_INFO("drvMotor_init: PWM on ch=%lu", motor->pwm_channel); + ULOG_INFO("drvMotor_init: src=%s", + motor->pwm_src == PWM_SRC_TIM ? "TIM" : "PCA9685"); } -void DriverMotor_Enable(drvMotor_t* motor){ - motor->enabled = true; - ULOG_INFO("Motor enabled (ch=%lu)", motor->pwm_channel); +void DriverMotor_Enable(drvMotor_t* motor) +{ + motor->enabled = true; + DriverMotor_setPwm(motor, 0); } -void DriverMotor_Disable(drvMotor_t* motor){ - DriverMotor_setPwm(motor, 0); +void DriverMotor_Disable(drvMotor_t* motor) +{ motor->enabled = false; - ULOG_INFO("Motor disabled (ch=%lu)", motor->pwm_channel); + DriverMotor_setPwm(motor, 0); } void DriverMotor_setDirection(drvMotor_t* motor, bool forward){ @@ -33,10 +40,20 @@ void DriverMotor_setDirection(drvMotor_t* motor, bool forward){ void DriverMotor_setPwm(drvMotor_t* motor, uint16_t pwm) { if (!motor->enabled) - return; - - __HAL_TIM_SET_COMPARE(motor->htim_pwm, motor->pwm_channel, pwm); - //ULOG_DEBUG("Motor PWM set %u (ch=%lu)", pwm, motor->pwm_channel); + pwm = 0; + + if (motor->pwm_src == PWM_SRC_TIM) { + __HAL_TIM_SET_COMPARE( + motor->pwm.tim.htim, + motor->pwm.tim.channel, + pwm + ); + } + else if (motor->pwm_src == PWM_SRC_PCA9685) { + if (pwm > 4095) pwm = 4095; + PCA9685_SetPWM(motor->pwm.pca.channel, 0, pwm); + } } + diff --git a/KPI_Rover/Motors/drvMotors.h b/KPI_Rover/Motors/drvMotors.h index 55c8ea1..ee6d5f8 100644 --- a/KPI_Rover/Motors/drvMotors.h +++ b/KPI_Rover/Motors/drvMotors.h @@ -4,17 +4,32 @@ #include "stm32f4xx_hal.h" #include +typedef enum { + PWM_SRC_TIM, + PWM_SRC_PCA9685 +} pwm_source_t; + typedef struct { - GPIO_TypeDef* IN1_port; - uint16_t IN1_pin; + GPIO_TypeDef* IN1_port; + uint16_t IN1_pin; + + GPIO_TypeDef* IN2_port; + uint16_t IN2_pin; + + pwm_source_t pwm_src; - GPIO_TypeDef* IN2_port; - uint16_t IN2_pin; + union { + struct { + TIM_HandleTypeDef* htim; + uint32_t channel; + } tim; - TIM_HandleTypeDef* htim_pwm; - uint32_t pwm_channel; + struct { + uint8_t channel; + } pca; + } pwm; - bool enabled; + bool enabled; } drvMotor_t; void DriverMotor_Init(drvMotor_t* motor); diff --git a/KPI_Rover/Motors/ulMotorsController.c b/KPI_Rover/Motors/ulMotorsController.c index 26cf429..b686df8 100644 --- a/KPI_Rover/Motors/ulMotorsController.c +++ b/KPI_Rover/Motors/ulMotorsController.c @@ -2,6 +2,7 @@ #include "cmsis_os.h" #include #include +#include "Motors/PCA9685.h" extern TIM_HandleTypeDef htim2; static osTimerId_t motors_timer_handle; @@ -18,102 +19,96 @@ static void init_motors_hw_mapping(ulMotorsController_t* ctrl) ctrl->motors[0].IN1_port = GPIOE; ctrl->motors[0].IN1_pin = GPIO_PIN_2; ctrl->motors[0].IN2_port = GPIOE; - ctrl->motors[0].IN2_pin = GPIO_PIN_3; - ctrl->motors[0].htim_pwm = &htim2; - ctrl->motors[0].pwm_channel = TIM_CHANNEL_3; + ctrl->motors[0].IN2_pin = GPIO_PIN_4; + ctrl->motors[0].pwm_src = PWM_SRC_PCA9685; + ctrl->motors[0].pwm.pca.channel = 0; // Motor 2 ctrl->motors[1].IN1_port = GPIOE; ctrl->motors[1].IN1_pin = GPIO_PIN_5; ctrl->motors[1].IN2_port = GPIOE; ctrl->motors[1].IN2_pin = GPIO_PIN_6; - ctrl->motors[1].htim_pwm = &htim2; - ctrl->motors[1].pwm_channel = TIM_CHANNEL_4; + ctrl->motors[1].pwm_src = PWM_SRC_PCA9685; + ctrl->motors[1].pwm.pca.channel = 1; + } void ulMotorsController_Init(ulMotorsController_t* ctrl) { - ULOG_INFO("ENTER ulMotorsController_Init"); + ULOG_INFO("MotorsController init"); init_motors_hw_mapping(ctrl); for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { - float kp = 5.0f, ki = 0.015f, kd = 0.0f; - ulPID_Init(&ctrl->pids[i], kp, ki, kd); - + ulPID_Init(&ctrl->pids[i], 5.0f, 0.015f, 0.0f); ulRLS_Init(&ctrl->rls[i]); - ctrl->last_pwm[i] = 0.0f; - ctrl->adapt_counter[i] = 0; - ctrl->pwm_max[i] = 0.0f; + ctrl->last_pwm[i] = 0.0f; + ctrl->adapt_counter[i] = 0; - ctrl->setpoint_rpm[i] = 0.0f; - ctrl->measured_rpm[i] = 0.0f; - ctrl->scale_filtered[i] = 1.0f; + ctrl->setpoint_rpm[i] = 0.0f; + ctrl->measured_rpm[i] = 0.0f; + ulPID_SetOutputLimits(&ctrl->pids[i], 0.0f, 1.0f); - uint32_t arr = __HAL_TIM_GET_AUTORELOAD(ctrl->motors[i].htim_pwm); - ulPID_SetOutputLimits(&ctrl->pids[i], 0.0f, (float)arr); - ctrl->pwm_max[i] = (float)arr; + if (ctrl->motors[i].pwm_src == PWM_SRC_TIM) { + ctrl->pwm_max[i] = + (float)__HAL_TIM_GET_AUTORELOAD( + ctrl->motors[i].pwm.tim.htim); + } + else { + ctrl->pwm_max[i] = 4095.0f; + } - ULOG_INFO("Motor %d init: PID(Kp=%.3f Ki=%.3f Kd=%.3f), ARR=%lu, pwm_ch=%lu", - i, kp, ki, kd, arr, ctrl->motors[i].pwm_channel); + ULOG_INFO("Motor[%d] pwm_max=%.0f", i, ctrl->pwm_max[i]); } - //g_motors_state = MOTORS_STATE_IDLE; - } + void ulMotorsController_Run(ulMotorsController_t* ctrl) { for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { - float setpoint = ctrl->setpoint_rpm[i]; - float rpm = ctrl->measured_rpm[i]; + float sp = ctrl->setpoint_rpm[i]; + float rpm = ctrl->measured_rpm[i]; - bool forward = (setpoint >= 0.0f); + bool forward = (sp >= 0.0f); DriverMotor_setDirection(&ctrl->motors[i], forward); - float pwm = ulPID_Compute(&ctrl->pids[i], setpoint, rpm); + float pwm_norm = ulPID_Compute(&ctrl->pids[i], + fabsf(sp), + fabsf(rpm)); - - if (pwm < 0.0f) pwm = 0.0f; - if (pwm > ctrl->pwm_max[i]) pwm = ctrl->pwm_max[i]; + if (pwm_norm < 0.0f) pwm_norm = 0.0f; + if (pwm_norm > 1.0f) pwm_norm = 1.0f; ulRLS_Update(&ctrl->rls[i], rpm, ctrl->last_pwm[i]); if (++ctrl->adapt_counter[i] >= 100) { ctrl->adapt_counter[i] = 0; - float setpoint = ctrl->setpoint_rpm[i]; - - if (fabsf(setpoint) > 300.0f && fabsf(rpm) > 100.0f) { - + if (fabsf(sp) > 300.0f && fabsf(rpm) > 100.0f) { ulPID_AutoTune_FromRLS(&ctrl->pids[i], &ctrl->rls[i], MOTORS_CONTROL_PERIOD_MS / 1000.0f); - - ULOG_INFO("Motor[%d] autotune: Kp=%.3f Ki=%.3f a=%.3f b=%.3f", - i, - ctrl->pids[i].kp, - ctrl->pids[i].ki, - ctrl->rls[i].theta[0], - ctrl->rls[i].theta[1]); } } - DriverMotor_setPwm(&ctrl->motors[i], (uint16_t)pwm); - ctrl->last_pwm[i] = pwm; + uint32_t pwm_hw = + (uint32_t)(pwm_norm * ctrl->pwm_max[i]); + + DriverMotor_setPwm(&ctrl->motors[i], pwm_hw); + + ctrl->last_pwm[i] = pwm_norm; - ULOG_DEBUG("MOTOR[%d] sp=%d rpm=%d pwm=%d dir=%s", - i, (int)setpoint, (int)rpm, (int)pwm, - forward ? "FWD" : "BWD"); + ULOG_DEBUG("M%d sp=%.0f rpm=%.0f pwm=%lu", + i, sp, rpm, pwm_hw); } } - static void MotorsTimerCallback(void *argument) { (void)argument; @@ -192,6 +187,8 @@ void ulMotorsController_Task(void* argument) ulMotorsController_Init(&g_motors_ctrl); + PCA9685_Init(); + const osTimerAttr_t timer_attrs = { .name = "Motors_Timer" }; @@ -215,9 +212,8 @@ void ulMotorsController_Task(void* argument) g_motors_ctrl.setpoint_rpm[0] = 1200.0f; g_motors_ctrl.setpoint_rpm[1] = 1200.0f; - g_motors_ctrl.measured_rpm[0] = 1150.0f; - g_motors_ctrl.measured_rpm[1] = 1150.0f; - + g_motors_ctrl.measured_rpm[0] = 1100.0f; + g_motors_ctrl.measured_rpm[1] = 1200.0f; osDelay(10); From f64712b8034ce79354cb69da103da917239d4822 Mon Sep 17 00:00:00 2001 From: velichko Date: Thu, 18 Dec 2025 00:35:24 +0200 Subject: [PATCH 7/8] Issue #17: PID controller configured, RLS replaced with another autotune - Gradient Descent, added to the database parameters metadata table --- KPI_Rover/Database/ulDatabase.h | 16 +++ KPI_Rover/Encoders/ulEncoder.c | 4 +- KPI_Rover/KPIRover.c | 28 +++- KPI_Rover/Motors/PCA9685.c | 53 +++++-- KPI_Rover/Motors/drvMotors.c | 6 +- KPI_Rover/Motors/ulGD.c | 77 ++++++++++ KPI_Rover/Motors/ulGD.h | 23 +++ KPI_Rover/Motors/ulMotorsController.c | 196 +++++++++++++++++++++----- KPI_Rover/Motors/ulMotorsController.h | 6 +- KPI_Rover/Motors/ulPID.c | 124 ++++++++-------- KPI_Rover/Motors/ulPID.h | 3 +- KPI_Rover/Motors/ulRLS.c | 76 ---------- KPI_Rover/Motors/ulRLS.h | 19 --- 13 files changed, 421 insertions(+), 210 deletions(-) create mode 100644 KPI_Rover/Motors/ulGD.c create mode 100644 KPI_Rover/Motors/ulGD.h delete mode 100644 KPI_Rover/Motors/ulRLS.c delete mode 100644 KPI_Rover/Motors/ulRLS.h diff --git a/KPI_Rover/Database/ulDatabase.h b/KPI_Rover/Database/ulDatabase.h index 724e9ec..2a616c1 100644 --- a/KPI_Rover/Database/ulDatabase.h +++ b/KPI_Rover/Database/ulDatabase.h @@ -17,6 +17,22 @@ enum ulDatabase_ParamType { }; enum ulDatabase_ParamId { + MOTOR_FL_KP, + MOTOR_FL_KI, + MOTOR_FL_KD, + MOTOR_FR_KP, + MOTOR_FR_KI, + MOTOR_FR_KD, + MOTOR_RL_KP, + MOTOR_RL_KI, + MOTOR_RL_KD, + MOTOR_RR_KP, + MOTOR_RR_KI, + MOTOR_RR_KD, + MOTOR_FL_SETPOINT, + MOTOR_FR_SETPOINT, + MOTOR_RL_SETPOINT, + MOTOR_RR_SETPOINT, MOTOR_FL_RPM, MOTOR_FR_RPM, MOTOR_RL_RPM, diff --git a/KPI_Rover/Encoders/ulEncoder.c b/KPI_Rover/Encoders/ulEncoder.c index 9004ba0..7f77fb1 100644 --- a/KPI_Rover/Encoders/ulEncoder.c +++ b/KPI_Rover/Encoders/ulEncoder.c @@ -13,7 +13,7 @@ static uint16_t lastTicks_RPM[MOTORS_COUNT]; static uint16_t lastTicks_ROS[MOTORS_COUNT]; -static uint16_t encoder_period_ms = 5; +static uint16_t encoder_period_ms = 20; static float encoder_ticks_per_rev = 820.0f; static osTimerId_t encoderTimerHandle; @@ -46,7 +46,7 @@ void ulEncoder_Init(void) { drvEncoder_Init(); if (!ulDatabase_getUint16(ENCODER_CONTROL_PERIOD_MS, &encoder_period_ms) || encoder_period_ms == 0) { - encoder_period_ms = 5; + encoder_period_ms = 20; } if (!ulDatabase_getFloat(ENCODER_TICKS_PER_REVOLUTION, &encoder_ticks_per_rev) || encoder_ticks_per_rev < 1.0f) { diff --git a/KPI_Rover/KPIRover.c b/KPI_Rover/KPIRover.c index b13936c..2c955f7 100644 --- a/KPI_Rover/KPIRover.c +++ b/KPI_Rover/KPIRover.c @@ -1,19 +1,45 @@ #include +#include #include "KPIRover.h" #include "cmsis_os.h" #include "Database/ulDatabase.h" static struct ulDatabase_ParamMetadata ulDatabase_params[] = { + + {0, FLOAT, true, 0.046f}, // MOTOR_FL_KP + {0, FLOAT, true, 0.013f}, // MOTOR_FL_KI + {0, FLOAT, true, 0.0001f}, // MOTOR_FL_KD + {0, FLOAT, true, 0.046f}, // MOTOR_FR_KP + {0, FLOAT, true, 0.013f}, // MOTOR_FR_KI + {0, FLOAT, true, 0.0001f}, // MOTOR_FR_KD + {0, FLOAT, true, 0.046f}, // MOTOR_RL_KP + {0, FLOAT, true, 0.013f}, // MOTOR_RL_KI + {0, FLOAT, true, 0.0001f}, // MOTOR_RL_KD + {0, FLOAT, true, 0.046f}, // MOTOR_RR_KP + {0, FLOAT, true, 0.013f}, // MOTOR_RR_KI + {0, FLOAT, true, 0.0001f}, // MOTOR_RR_KD + {0, INT32, false, 0}, // MOTOR_FL_SETPOINT, + {0, INT32, false, 0}, // MOTOR_FR_SETPOINT, + {0, INT32, false, 0}, // MOTOR_RL_SETPOINT, + {0, INT32, false, 0}, // MOTOR_RR_SETPOINT, {0, INT32, false, 0}, // MOTOR_FL_RPM, {0, INT32, false, 0}, // MOTOR_FR_RPM, {0, INT32, false, 0}, // MOTOR_RL_RPM, {0, INT32, false, 0}, // MOTOR_RR_RPM, - {0, UINT16, true, 5}, // ENCODER_CONTROL_PERIOD_MS, + {0, UINT16, true, 20}, // ENCODER_CONTROL_PERIOD_MS, {0, FLOAT, true, 820.0f} // ENCODER_TICKS_PER_REVOLUTION, }; void KPIRover_Init(void) { ulDatabase_init(ulDatabase_params, sizeof(ulDatabase_params) / sizeof(struct ulDatabase_ParamMetadata)); ulEncoder_Init(); + + static const osThreadAttr_t MotorsCtrlTask_attributes = { + .name = "MotorsCtrlTask", + .priority = (osPriority_t) osPriorityNormal, + .stack_size = 1024 * 4 + }; + (void) osThreadNew(ulMotorsController_Task, NULL, &MotorsCtrlTask_attributes); + } diff --git a/KPI_Rover/Motors/PCA9685.c b/KPI_Rover/Motors/PCA9685.c index fc5573e..69608f3 100644 --- a/KPI_Rover/Motors/PCA9685.c +++ b/KPI_Rover/Motors/PCA9685.c @@ -1,16 +1,21 @@ #include "PCA9685.h" #include "stm32f4xx_hal.h" -/* ЗОВНІШНІЙ I2C */ +/* I2C Handle defined in main.c */ extern I2C_HandleTypeDef hi2c1; -/* PCA9685 I2C address (0x40 default) */ +/* PCA9685 I2C address */ +/* 0x40 is the 7-bit address. HAL requires it shifted left by 1 (0x80) */ #define PCA9685_ADDR (0x40 << 1) +/* Register Addresses */ #define MODE1 0x00 #define PRESCALE 0xFE #define LED0_ON_L 0x06 +/** + * Helper: Write a single byte to a register. + */ static void pca9685_write(uint8_t reg, uint8_t data) { HAL_I2C_Mem_Write(&hi2c1, @@ -22,13 +27,18 @@ static void pca9685_write(uint8_t reg, uint8_t data) HAL_MAX_DELAY); } +/** + * Helper: Write 4 bytes consecutively (Burst Write). + * Used to update all PWM registers (ON_L, ON_H, OFF_L, OFF_H) in one go. + */ static void pca9685_write4(uint8_t reg, uint16_t on, uint16_t off) { uint8_t buf[4]; - buf[0] = on & 0xFF; - buf[1] = on >> 8; - buf[2] = off & 0xFF; - buf[3] = off >> 8; + /* Split 12-bit values into Low and High bytes */ + buf[0] = on & 0xFF; // ON_L + buf[1] = on >> 8; // ON_H + buf[2] = off & 0xFF; // OFF_L + buf[3] = off >> 8; // OFF_H HAL_I2C_Mem_Write(&hi2c1, PCA9685_ADDR, @@ -39,30 +49,53 @@ static void pca9685_write4(uint8_t reg, uint16_t on, uint16_t off) HAL_MAX_DELAY); } +/** + * Initialize the PCA9685 chip. + * Setup frequency and enable auto-increment mode. + */ void PCA9685_Init(void) { + /* 1. Reset device (Wake up) */ pca9685_write(MODE1, 0x00); HAL_Delay(10); - /* === Set frequency ~1 kHz === */ - uint8_t prescale = 5; // ≈1017 Hz + /* 2. Setup PWM Frequency ~1000Hz */ + /* Formula: prescale = 25MHz / (4096 * update_rate) - 1 */ + /* For 1000Hz: 25000000 / (4096 * 1000) - 1 ~= 5 */ + uint8_t prescale = 5; - pca9685_write(MODE1, 0x10); + /* To change pre-scaler, we must be in SLEEP mode */ + pca9685_write(MODE1, 0x10); // Sleep mode HAL_Delay(1); - pca9685_write(PRESCALE, prescale); + pca9685_write(PRESCALE, prescale); // Write pre-scaler + /* 3. Wake up and set Auto-Increment */ + /* 0xA1: 1010 0001 + * Bit 5 (0x20): Auto-Increment enabled (allows writing 4 bytes at once) + * Bit 0 (0x01): ALLCALL enabled + */ pca9685_write(MODE1, 0xA1); HAL_Delay(5); } +/** + * Set PWM duty cycle for a specific channel. + * @param channel: 0 to 15 + * @param on: Step number to turn ON (usually 0) + * @param off: Step number to turn OFF (0 to 4095) - This acts as Duty Cycle + */ void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off) { + /* Safety check */ if (channel > 15) return; + /* 12-bit limit check (4095 is max) */ if (off > 4095) off = 4095; + /* Calculate register address for this channel */ + /* Each channel has 4 registers, starting at LED0_ON_L (0x06) */ pca9685_write4(LED0_ON_L + 4 * channel, on, off); } diff --git a/KPI_Rover/Motors/drvMotors.c b/KPI_Rover/Motors/drvMotors.c index 792de43..46a6271 100644 --- a/KPI_Rover/Motors/drvMotors.c +++ b/KPI_Rover/Motors/drvMotors.c @@ -1,4 +1,5 @@ #include +#include #include "ulog.h" void DriverMotor_Init(drvMotor_t* motor) @@ -13,9 +14,6 @@ void DriverMotor_Init(drvMotor_t* motor) motor->enabled = false; DriverMotor_setDirection(motor, true); - - ULOG_INFO("drvMotor_init: src=%s", - motor->pwm_src == PWM_SRC_TIM ? "TIM" : "PCA9685"); } void DriverMotor_Enable(drvMotor_t* motor) @@ -33,8 +31,6 @@ void DriverMotor_Disable(drvMotor_t* motor) void DriverMotor_setDirection(drvMotor_t* motor, bool forward){ HAL_GPIO_WritePin(motor->IN1_port, motor->IN1_pin, forward ? GPIO_PIN_SET : GPIO_PIN_RESET); HAL_GPIO_WritePin(motor->IN2_port, motor->IN2_pin, forward ? GPIO_PIN_RESET : GPIO_PIN_SET); - - ULOG_DEBUG("Motor dir=%s", forward ? "FWD" : "BWD"); } void DriverMotor_setPwm(drvMotor_t* motor, uint16_t pwm) diff --git a/KPI_Rover/Motors/ulGD.c b/KPI_Rover/Motors/ulGD.c new file mode 100644 index 0000000..fd376a6 --- /dev/null +++ b/KPI_Rover/Motors/ulGD.c @@ -0,0 +1,77 @@ +#include "ulGD.h" +#include +#include + +void ulGD_Init(ulGD_t* gd) +{ + /* Tuning Speed */ + gd->alpha_p = 0.005f; // Kp learning rate + gd->alpha_i = 0.0005f; // Ki learning rate + + /* Safety Limits */ + gd->kp_min = 0.040f; + gd->kp_max = 0.080f; + + gd->ki_min = 0.010f; + gd->ki_max = 0.020f; + + /* Stability Settings */ + gd->max_delta = 0.0001f; // Max step size per cycle + gd->deadzone = 0.02f; // Ignore small errors + + /* Anti-Drift (Leakage) */ + gd->kp_anchor = 0.046f; // Target Kp (Base value) + gd->ki_anchor = 0.013f; // Target Ki (Base value) + gd->leakage_rate = 0.01f;// Return-to-base force +} + +void ulGD_Update(ulGD_t* gd, float error, float error_integral, float pwm_norm, float* kp, float* ki) +{ + /* Validation */ + if (isnan(error) || isnan(error_integral)) return; + if (isnan(*kp) || isnan(*ki)) return; + + /* Skip if error is negligible */ + bool inside_deadzone = fabsf(error) < gd->deadzone; + + /* Skip if motor is saturated (0% or 100%) */ + if (pwm_norm < 0.05f || pwm_norm > 0.95f) return; + + float dKp = 0.0f; + float dKi = 0.0f; + + /* Calculate Gradients */ + if (!inside_deadzone) { + // P-term gradient + dKp = gd->alpha_p * error * fabsf(error); + + // I-term gradient (with integral windup protection) + float learning_integral = error_integral; + if (learning_integral > 50.0f) learning_integral = 50.0f; + if (learning_integral < -50.0f) learning_integral = -50.0f; + + dKi = gd->alpha_i * error * learning_integral; + + // Clamp step size + if (dKp > gd->max_delta) dKp = gd->max_delta; + if (dKp < -gd->max_delta) dKp = -gd->max_delta; + + if (dKi > gd->max_delta) dKi = gd->max_delta; + if (dKi < -gd->max_delta) dKi = -gd->max_delta; + } + + /* Calculate Leakage (Pull towards anchor) */ + float leak_p = (*kp - gd->kp_anchor) * gd->leakage_rate; + float leak_i = (*ki - gd->ki_anchor) * gd->leakage_rate; + + /* Update Gains */ + *kp = *kp + dKp - leak_p; + *ki = *ki + dKi - leak_i; + + /* Apply Hard Limits */ + if (*kp < gd->kp_min) *kp = gd->kp_min; + if (*kp > gd->kp_max) *kp = gd->kp_max; + + if (*ki < gd->ki_min) *ki = gd->ki_min; + if (*ki > gd->ki_max) *ki = gd->ki_max; +} diff --git a/KPI_Rover/Motors/ulGD.h b/KPI_Rover/Motors/ulGD.h new file mode 100644 index 0000000..bdd9ede --- /dev/null +++ b/KPI_Rover/Motors/ulGD.h @@ -0,0 +1,23 @@ +#ifndef MOTORS_ULGD_H_ +#define MOTORS_ULGD_H_ + +#include +#include + +typedef struct { + float alpha_p; + float alpha_i; + float kp_min, kp_max; + float ki_min, ki_max; + float max_delta; + float deadzone; + float kp_anchor; + float ki_anchor; + float leakage_rate; + +} ulGD_t; + +void ulGD_Init(ulGD_t* gd); +void ulGD_Update(ulGD_t* gd, float error, float error_integral, float pwm_norm, float* kp, float* ki); + +#endif /* MOTORS_ULGD_H_ */ diff --git a/KPI_Rover/Motors/ulMotorsController.c b/KPI_Rover/Motors/ulMotorsController.c index b686df8..0408dd3 100644 --- a/KPI_Rover/Motors/ulMotorsController.c +++ b/KPI_Rover/Motors/ulMotorsController.c @@ -3,19 +3,70 @@ #include #include #include "Motors/PCA9685.h" +#include +#include +#include +#include "usbd_cdc_if.h" -extern TIM_HandleTypeDef htim2; static osTimerId_t motors_timer_handle; #define MOTORS_CONTROL_PERIOD_MS 20 +const float dt_sec = MOTORS_CONTROL_PERIOD_MS / 1000.0f; + ulMotorsController_t g_motors_ctrl; MotorsCtrlState_t g_motors_state = MOTORS_STATE_INIT; +static const uint16_t key_rpm[ULMOTORS_NUM_MOTORS] = { + MOTOR_FL_RPM, + MOTOR_FR_RPM, + MOTOR_RL_RPM, + MOTOR_RR_RPM +}; + +static const uint16_t key_kp[ULMOTORS_NUM_MOTORS] = { + MOTOR_FL_KP, + MOTOR_FR_KP, + MOTOR_RL_KP, + MOTOR_RR_KP +}; + +static const uint16_t key_ki[ULMOTORS_NUM_MOTORS] = { + MOTOR_FL_KI, + MOTOR_FR_KI, + MOTOR_RL_KI, + MOTOR_RR_KI +}; + +static const uint16_t key_kd[ULMOTORS_NUM_MOTORS] = { + MOTOR_FL_KD, + MOTOR_FR_KD, + MOTOR_RL_KD, + MOTOR_RR_KD +}; + +static const uint16_t key_setpoint[ULMOTORS_NUM_MOTORS] = { + MOTOR_FL_SETPOINT, + MOTOR_FR_SETPOINT, + MOTOR_RL_SETPOINT, + MOTOR_RR_SETPOINT +}; + +static void ulMotorsController_UpdateFeedback(ulMotorsController_t* ctrl) +{ + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + int32_t rpm_db = 0; + + if (ulDatabase_getInt32(key_rpm[i], &rpm_db)) { + ctrl->measured_rpm[i] = (float)rpm_db; + } + } +} + static void init_motors_hw_mapping(ulMotorsController_t* ctrl) { - // Motor 1 + // MOTOR_1_FL ctrl->motors[0].IN1_port = GPIOE; ctrl->motors[0].IN1_pin = GPIO_PIN_2; ctrl->motors[0].IN2_port = GPIOE; @@ -23,7 +74,7 @@ static void init_motors_hw_mapping(ulMotorsController_t* ctrl) ctrl->motors[0].pwm_src = PWM_SRC_PCA9685; ctrl->motors[0].pwm.pca.channel = 0; - // Motor 2 + // MOTOR_2_FR ctrl->motors[1].IN1_port = GPIOE; ctrl->motors[1].IN1_pin = GPIO_PIN_5; ctrl->motors[1].IN2_port = GPIOE; @@ -31,6 +82,22 @@ static void init_motors_hw_mapping(ulMotorsController_t* ctrl) ctrl->motors[1].pwm_src = PWM_SRC_PCA9685; ctrl->motors[1].pwm.pca.channel = 1; + // MOTOR_3_RL + ctrl->motors[2].IN1_port = GPIOD; + ctrl->motors[2].IN1_pin = GPIO_PIN_0; + ctrl->motors[2].IN2_port = GPIOD; + ctrl->motors[2].IN2_pin = GPIO_PIN_1; + ctrl->motors[2].pwm_src = PWM_SRC_PCA9685; + ctrl->motors[2].pwm.pca.channel = 2; + + // MOTOR_4_RR + ctrl->motors[3].IN1_port = GPIOD; + ctrl->motors[3].IN1_pin = GPIO_PIN_2; + ctrl->motors[3].IN2_port = GPIOD; + ctrl->motors[3].IN2_pin = GPIO_PIN_3; + ctrl->motors[3].pwm_src = PWM_SRC_PCA9685; + ctrl->motors[3].pwm.pca.channel = 3; + } @@ -42,16 +109,40 @@ void ulMotorsController_Init(ulMotorsController_t* ctrl) for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { - ulPID_Init(&ctrl->pids[i], 5.0f, 0.015f, 0.0f); - ulRLS_Init(&ctrl->rls[i]); + float kp = 0.0f; + float ki = 0.0f; + float kd = 0.0f; - ctrl->last_pwm[i] = 0.0f; - ctrl->adapt_counter[i] = 0; + if (!ulDatabase_getFloat(key_kp[i], &kp)) { + ULOG_ERROR("Failed to load Kp for motor %d! Using default.", i); + kp = 0.046f; + } + + if (!ulDatabase_getFloat(key_ki[i], &ki)) { + ULOG_ERROR("Failed to load Ki for motor %d!", i); + ki = 0.013f; + } + + if (!ulDatabase_getFloat(key_kd[i], &kd)) { + kd = 0.0001f; + } + ulPID_Init(&ctrl->pids[i], kp, ki, kd); + + ctrl->pids[i].deadzone = 0.02f; + ctrl->pids[i].slew_rate = 25.0f; + ctrl->pids[i].d_alpha = 0.2f; + ctrl->pids[i].sp_alpha = 0.1f; + + ulGD_Init(&ctrl->gd[i]); + + ulPID_SetOutputLimits(&ctrl->pids[i], 0.0f, 1.0f); + ulPID_SetIntegralLimits(&ctrl->pids[i], -50.0f, 50.0f); + + ctrl->last_pwm[i] = 0.0f; ctrl->setpoint_rpm[i] = 0.0f; ctrl->measured_rpm[i] = 0.0f; - ulPID_SetOutputLimits(&ctrl->pids[i], 0.0f, 1.0f); if (ctrl->motors[i].pwm_src == PWM_SRC_TIM) { ctrl->pwm_max[i] = @@ -67,48 +158,92 @@ void ulMotorsController_Init(ulMotorsController_t* ctrl) } +static void ulMotorsController_UpdateFromDB(ulMotorsController_t* ctrl) +{ + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + int32_t sp_db = 0; + + if (ulDatabase_getInt32(key_setpoint[i], &sp_db)) { + ctrl->setpoint_rpm[i] = (float)sp_db; + } + } +} + void ulMotorsController_Run(ulMotorsController_t* ctrl) { + static char teleplot_buf[512]; + teleplot_buf[0] = '\0'; + const float MAX_RPM_CONST = 250.0f; + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + if (isnan(ctrl->pids[i].kp) || ctrl->pids[i].kp < 0.001f) ctrl->pids[i].kp = 0.046f; + if (isnan(ctrl->pids[i].ki) || ctrl->pids[i].ki < 0.0f) ctrl->pids[i].ki = 0.013f; + float sp = ctrl->setpoint_rpm[i]; float rpm = ctrl->measured_rpm[i]; + if (isnan(rpm)) rpm = 0.0f; + bool forward = (sp >= 0.0f); DriverMotor_setDirection(&ctrl->motors[i], forward); - float pwm_norm = ulPID_Compute(&ctrl->pids[i], - fabsf(sp), - fabsf(rpm)); + float pwm_norm = ulPID_Compute(&ctrl->pids[i], fabsf(sp), fabsf(rpm), dt_sec); + + if (isnan(pwm_norm)) { + pwm_norm = 0.0f; + } if (pwm_norm < 0.0f) pwm_norm = 0.0f; if (pwm_norm > 1.0f) pwm_norm = 1.0f; - ulRLS_Update(&ctrl->rls[i], rpm, ctrl->last_pwm[i]); - - if (++ctrl->adapt_counter[i] >= 100) { - ctrl->adapt_counter[i] = 0; + float max_val = ctrl->pwm_max[i]; + if (max_val < 1.0f) max_val = 4095.0f; - if (fabsf(sp) > 300.0f && fabsf(rpm) > 100.0f) { - ulPID_AutoTune_FromRLS(&ctrl->pids[i], - &ctrl->rls[i], - MOTORS_CONTROL_PERIOD_MS / 1000.0f); - } - } + uint32_t pwm_hw = (uint32_t)(pwm_norm * max_val); - uint32_t pwm_hw = - (uint32_t)(pwm_norm * ctrl->pwm_max[i]); + if (pwm_hw > (uint32_t)max_val) pwm_hw = (uint32_t)max_val; DriverMotor_setPwm(&ctrl->motors[i], pwm_hw); - ctrl->last_pwm[i] = pwm_norm; - ULOG_DEBUG("M%d sp=%.0f rpm=%.0f pwm=%lu", - i, sp, rpm, pwm_hw); + +// ADAPTIVE PI (GRADIENT DESCENT) + + float error = sp - rpm; + float error_norm = error / MAX_RPM_CONST; + + ulGD_Update( + &ctrl->gd[i], + error_norm, + ctrl->pids[i].integral, + pwm_norm, + &ctrl->pids[i].kp, + &ctrl->pids[i].ki + ); + + +// TELEPLOT + char tmp[64]; + snprintf(tmp, sizeof(tmp), ">M%d_SP:%d\n>M%d_RPM:%d\n>M%d_PWM:%lu\n>M%d_P:%.4f\n>M%d_I:%.4f\n", + i, (int)sp, + i, (int)rpm, + i, pwm_hw, + i, ctrl->pids[i].kp, + i, ctrl->pids[i].ki); + + if (strlen(teleplot_buf) + strlen(tmp) < sizeof(teleplot_buf) - 1) { + strcat(teleplot_buf, tmp); + } + } + + if (strlen(teleplot_buf) > 0) { + CDC_Transmit_FS((uint8_t*)teleplot_buf, strlen(teleplot_buf)); } } + static void MotorsTimerCallback(void *argument) { (void)argument; @@ -141,6 +276,7 @@ static void MotorsTimerCallback(void *argument) for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { DriverMotor_Enable(&g_motors_ctrl.motors[i]); + ulPID_Reset(&g_motors_ctrl.pids[i]); } g_motors_state = MOTORS_STATE_RUN; @@ -150,6 +286,7 @@ static void MotorsTimerCallback(void *argument) case MOTORS_STATE_RUN: { + ulMotorsController_UpdateFeedback(&g_motors_ctrl); ulMotorsController_Run(&g_motors_ctrl); bool all_zero = true; @@ -207,14 +344,9 @@ void ulMotorsController_Task(void* argument) } - for (;;) { - g_motors_ctrl.setpoint_rpm[0] = 1200.0f; - g_motors_ctrl.setpoint_rpm[1] = 1200.0f; - g_motors_ctrl.measured_rpm[0] = 1100.0f; - g_motors_ctrl.measured_rpm[1] = 1200.0f; - + ulMotorsController_UpdateFromDB(&g_motors_ctrl); osDelay(10); } diff --git a/KPI_Rover/Motors/ulMotorsController.h b/KPI_Rover/Motors/ulMotorsController.h index 9947c2f..b8131c2 100644 --- a/KPI_Rover/Motors/ulMotorsController.h +++ b/KPI_Rover/Motors/ulMotorsController.h @@ -3,15 +3,15 @@ #include #include -#include +#include #include -#define ULMOTORS_NUM_MOTORS 2 +#define ULMOTORS_NUM_MOTORS 4 typedef struct { drvMotor_t motors[ULMOTORS_NUM_MOTORS]; ulPID_t pids[ULMOTORS_NUM_MOTORS]; - ulRLS_t rls[ULMOTORS_NUM_MOTORS]; + ulGD_t gd[ULMOTORS_NUM_MOTORS]; float last_pwm[ULMOTORS_NUM_MOTORS]; uint16_t adapt_counter[ULMOTORS_NUM_MOTORS]; diff --git a/KPI_Rover/Motors/ulPID.c b/KPI_Rover/Motors/ulPID.c index 575d2cd..6ddae4b 100644 --- a/KPI_Rover/Motors/ulPID.c +++ b/KPI_Rover/Motors/ulPID.c @@ -1,10 +1,10 @@ #include #include -#include #include #include "stm32f4xx_hal.h" -#define PID_AW_GAIN 0.2f +// Gain for dynamic anti-windup (how fast to reduce integral when saturated) +#define PID_AW_GAIN 0.5f void ulPID_Init(ulPID_t* pid, float kp, float ki, float kd){ pid->kp = kp; @@ -15,21 +15,30 @@ void ulPID_Init(ulPID_t* pid, float kp, float ki, float kd){ pid->prev_meas = 0.0f; pid->d_filtered = 0.0f; + /* Output limits (usually 0.0 to 1.0 for PWM) */ pid->out_min = 0.0f; - pid->out_max = 1000.0f; + pid->out_max = 1.0f; - pid->int_min = -500.0f; - pid->int_max = 500.0f; + /* Integral limits (prevent indefinite error accumulation) */ + pid->int_min = -1.0f; + pid->int_max = 1.0f; + /* Setpoint filter (1.0 = no filtering, lower = smoother target) */ pid->sp_filtered = 0.0f; - pid->sp_alpha = 0.25f; + pid->sp_alpha = 1.0f; - pid->d_alpha = 0.4f; - pid->slew_rate = 20.0f; - pid->deadzone = 300.0f; + /* Derivative filter (CRITICAL for noisy encoders, 0.05-0.2 is good) */ + pid->d_alpha = 0.2f; + + /* Max change of output per second (Soft Start) */ + pid->slew_rate = 100.0f; + + /* Static friction compensation */ + pid->deadzone = 0.0f; pid->last_output = 0.0f; + // pid->last_time is not used inside Compute anymore, but good to init pid->last_time = HAL_GetTick(); pid->initialized = false; } @@ -63,14 +72,19 @@ void ulPID_SetIntegralLimits(ulPID_t* pid, float int_min, float int_max){ pid->int_max = int_max; } -float ulPID_Compute(ulPID_t* pid, float setpoint, float measured){ - uint32_t now = HAL_GetTick(); - float dt = (now - pid->last_time) / 1000.0f; - pid->last_time = now; - - if (dt <= 0.0f) dt = 0.001f; - if (dt > 0.1f) dt = 0.1f; +/** + * Main PID calculation loop. + * @param dt: Delta time in seconds (must be constant for stability!) + */ +float ulPID_Compute(ulPID_t* pid, float setpoint, float measured, float dt) +{ + /* Safety: Avoid division by zero */ + if (dt <= 0.000001f) { + return pid->last_output; + } + /* Initialization (Bumpless Transfer) */ + /* Prevents "jumps" when PID is called for the first time */ if (!pid->initialized) { pid->sp_filtered = setpoint; @@ -80,51 +94,69 @@ float ulPID_Compute(ulPID_t* pid, float setpoint, float measured){ pid->initialized = true; } + /* Setpoint Filtering (Low Pass Filter) */ + /* Smooths out jerky changes in target speed */ pid->sp_filtered += pid->sp_alpha * (setpoint - pid->sp_filtered); float sp = pid->sp_filtered; + + /* Error Calculation */ float error = sp - measured; + /* Integral Term (Accumulate Error) */ pid->integral += error * dt; + + // Hard clamp for integral to prevent overflow if (pid->integral > pid->int_max) pid->integral = pid->int_max; if (pid->integral < pid->int_min) pid->integral = pid->int_min; + /* Derivative Term (Derivative on Measurement) */ + /* We calculate slope of Sensor data, NOT Error. Prevents spikes when Setpoint changes. */ float d_meas = (measured - pid->prev_meas) / dt; pid->prev_meas = measured; + // Note the minus sign: d(Error)/dt = -d(Measured)/dt (assuming constant setpoint) float d_raw = -d_meas; + + // Filter the noisy D-term (Low Pass Filter) pid->d_filtered += pid->d_alpha * (d_raw - pid->d_filtered); + /* PID Sum */ float out_unsat = pid->kp * error + pid->ki * pid->integral + pid->kd * pid->d_filtered; + /* Saturation (Clamping) */ float out_clamped = out_unsat; if (out_clamped > pid->out_max) out_clamped = pid->out_max; if (out_clamped < pid->out_min) out_clamped = pid->out_min; - if (pid->ki != 0.0f) + /* Dynamic Anti-Windup */ + /* If output is clamped, reduce Integral to stop it from growing uselessly */ + if (pid->ki != 0.0f && out_clamped != out_unsat) { float aw = out_clamped - out_unsat; - pid->integral += (PID_AW_GAIN * aw) / pid->ki; - - if (pid->integral > pid->int_max) pid->integral = pid->int_max; - if (pid->integral < pid->int_min) pid->integral = pid->int_min; + pid->integral += (PID_AW_GAIN * aw * dt); } float actuator = out_clamped; - if (fabsf(actuator) > 0.0f) - { - if (actuator > 0.0f) - actuator += pid->deadzone; - else - actuator -= pid->deadzone; - - if (actuator > pid->out_max) actuator = pid->out_max; - if (actuator < pid->out_min) actuator = pid->out_min; - } + /* Deadzone Compensation (Static Friction) */ + /* Adds a minimum power to overcome motor friction */ + if (fabsf(actuator) > 0.001f) + { + if (actuator > 0.0f) + actuator += pid->deadzone; + else + actuator -= pid->deadzone; + + // Re-clamp after adding deadzone + if (actuator > pid->out_max) actuator = pid->out_max; + if (actuator < pid->out_min) actuator = pid->out_min; + } + /* Slew Rate Limiter (Soft Start / Anti-Shock) */ + /* Limits how fast the output can change to protect gears/electronics */ float diff = actuator - pid->last_output; float max_change = pid->slew_rate * dt; @@ -137,33 +169,3 @@ float ulPID_Compute(ulPID_t* pid, float setpoint, float measured){ return actuator; } - -void ulPID_AutoTune_FromRLS(ulPID_t* pid, const ulRLS_t* rls, float Ts){ - float a = rls->theta[0]; - float b = rls->theta[1]; - - if (a > 0.999f) a = 0.999f; - if (a < 0.01f) a = 0.01f; - - float tau = -Ts / logf(a); - float K = b / (1.0f - a); - - if (K < 0.01f) K = 0.01f; - if (K > 1000.0f) K = 1000.0f; - - float lambda = 0.25f * tau; - if (lambda < 0.001f) lambda = 0.001f; - - float kp = tau / (K * lambda); - float ki = 1.0f / (K * lambda); - - if (kp < 0.0f) kp = 0.0f; - if (kp > 300.0f) kp = 300.0f; - - if (ki < 0.0f) ki = 0.0f; - if (ki > 300.0f) ki = 300.0f; - - pid->kp = kp; - pid->ki = ki; - pid->kd = 0.0f; -} diff --git a/KPI_Rover/Motors/ulPID.h b/KPI_Rover/Motors/ulPID.h index 43c0f23..f73248d 100644 --- a/KPI_Rover/Motors/ulPID.h +++ b/KPI_Rover/Motors/ulPID.h @@ -41,7 +41,8 @@ void ulPID_Init(ulPID_t* pid, float kp, float ki, float kd); void ulPID_Reset(ulPID_t* pid); void ulPID_SetParams(ulPID_t* pid, float kp, float ki, float kd); void ulPID_SetOutputLimits(ulPID_t* pid, float out_min, float out_max); -float ulPID_Compute(ulPID_t* pid, float setpoint, float measured); +void ulPID_SetIntegralLimits(ulPID_t* pid, float int_min, float int_max); +float ulPID_Compute(ulPID_t* pid, float setpoint, float measured, float dt); void ulPID_AutoTune_FromRLS(ulPID_t* pid, const ulRLS_t* rls, float Ts); void ulPID_ResetI(ulPID_t* pid); #endif // UL_PID_H diff --git a/KPI_Rover/Motors/ulRLS.c b/KPI_Rover/Motors/ulRLS.c deleted file mode 100644 index 488ffde..0000000 --- a/KPI_Rover/Motors/ulRLS.c +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include - -void ulRLS_Init(ulRLS_t* rls) -{ - rls->theta[0] = 0.8f; // Початкове a - rls->theta[1] = 0.2f; // Початкове b - - // Початкова коваріаційна матриця (високе початкове значення для швидкої адаптації) - rls->P[0][0] = 500.0f; - rls->P[0][1] = 0.0f; - rls->P[1][0] = 0.0f; - rls->P[1][1] = 500.0f; - - rls->lambda = 0.995f; - - rls->last_w = 0.0f; - rls->last_u = 0.0f; - rls->initialized = 0; -} - -void ulRLS_Update(ulRLS_t* rls, float w_k, float u_k){ - - if (!rls->initialized) { - rls->last_w = w_k; - rls->last_u = u_k; - rls->initialized = 1; - return; - } - - float phi0 = rls->last_w; - float phi1 = rls->last_u; - - float y = w_k; - - float Pphi0 = rls->P[0][0]*phi0 + rls->P[0][1]*phi1; - float Pphi1 = rls->P[1][0]*phi0 + rls->P[1][1]*phi1; - - float denom = rls->lambda + phi0*Pphi0 + phi1*Pphi1; - - if (fabsf(denom) < 1e-6f) - denom = (denom >= 0.0f) ? 1e-6f : -1e-6f; - - float K0 = Pphi0 / denom; - float K1 = Pphi1 / denom; - - float a = rls->theta[0]; - float b = rls->theta[1]; - - float y_hat = a*phi0 + b*phi1; - float eps = y - y_hat; - - float new_a = a + K0 * eps; - float new_b = b + K1 * eps; - - if (new_a < 0.7f) new_a = 0.7f; - if (new_a > 1.3f) new_a = 1.3f; - if (new_b < 0.0f) new_b = 0.0f; - if (new_b > 3.0f) new_b = 3.0f; - - rls->theta[0] = new_a; - rls->theta[1] = new_b; - - float P00_new = (rls->P[0][0] - K0 * Pphi0) / rls->lambda; - float P11_new = (rls->P[1][1] - K1 * Pphi1) / rls->lambda; - float P01_new = (rls->P[0][1] - K0 * Pphi1) / rls->lambda; - - rls->P[0][0] = P00_new; - rls->P[1][1] = P11_new; - - rls->P[0][1] = P01_new; - rls->P[1][0] = P01_new; - - rls->last_w = w_k; - rls->last_u = u_k; -} diff --git a/KPI_Rover/Motors/ulRLS.h b/KPI_Rover/Motors/ulRLS.h deleted file mode 100644 index b3ebf57..0000000 --- a/KPI_Rover/Motors/ulRLS.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef UL_RLS_H -#define UL_RLS_H - -#include - -typedef struct ulRLS_s { - float theta[2]; - float P[2][2]; - float lambda; - - float last_w; - float last_u; - uint8_t initialized; -} ulRLS_t; - -void ulRLS_Init(ulRLS_t* rls); -void ulRLS_Update(ulRLS_t* rls, float w_k, float u_k_1); - -#endif // UL_RLS_H From b580f6392ebdab195684e1f5c34c8e25d3f42b97 Mon Sep 17 00:00:00 2001 From: velichko Date: Thu, 18 Dec 2025 03:53:19 +0200 Subject: [PATCH 8/8] Issue #17: additional PID and period settings --- KPI_Rover/Encoders/ulEncoder.c | 4 ++-- KPI_Rover/KPIRover.c | 2 +- KPI_Rover/Motors/ulGD.c | 12 ++++++++++-- KPI_Rover/Motors/ulMotorsController.c | 9 ++++----- 4 files changed, 17 insertions(+), 10 deletions(-) diff --git a/KPI_Rover/Encoders/ulEncoder.c b/KPI_Rover/Encoders/ulEncoder.c index 7f77fb1..9004ba0 100644 --- a/KPI_Rover/Encoders/ulEncoder.c +++ b/KPI_Rover/Encoders/ulEncoder.c @@ -13,7 +13,7 @@ static uint16_t lastTicks_RPM[MOTORS_COUNT]; static uint16_t lastTicks_ROS[MOTORS_COUNT]; -static uint16_t encoder_period_ms = 20; +static uint16_t encoder_period_ms = 5; static float encoder_ticks_per_rev = 820.0f; static osTimerId_t encoderTimerHandle; @@ -46,7 +46,7 @@ void ulEncoder_Init(void) { drvEncoder_Init(); if (!ulDatabase_getUint16(ENCODER_CONTROL_PERIOD_MS, &encoder_period_ms) || encoder_period_ms == 0) { - encoder_period_ms = 20; + encoder_period_ms = 5; } if (!ulDatabase_getFloat(ENCODER_TICKS_PER_REVOLUTION, &encoder_ticks_per_rev) || encoder_ticks_per_rev < 1.0f) { diff --git a/KPI_Rover/KPIRover.c b/KPI_Rover/KPIRover.c index 2c955f7..f32b8dc 100644 --- a/KPI_Rover/KPIRover.c +++ b/KPI_Rover/KPIRover.c @@ -27,7 +27,7 @@ static struct ulDatabase_ParamMetadata ulDatabase_params[] = { {0, INT32, false, 0}, // MOTOR_FR_RPM, {0, INT32, false, 0}, // MOTOR_RL_RPM, {0, INT32, false, 0}, // MOTOR_RR_RPM, - {0, UINT16, true, 20}, // ENCODER_CONTROL_PERIOD_MS, + {0, UINT16, true, 5}, // ENCODER_CONTROL_PERIOD_MS, {0, FLOAT, true, 820.0f} // ENCODER_TICKS_PER_REVOLUTION, }; diff --git a/KPI_Rover/Motors/ulGD.c b/KPI_Rover/Motors/ulGD.c index fd376a6..4f71dec 100644 --- a/KPI_Rover/Motors/ulGD.c +++ b/KPI_Rover/Motors/ulGD.c @@ -2,11 +2,19 @@ #include #include +/** + * @file ulGD.c + * @brief Online Gradient Descent (GD) Auto-tuner for PID Controllers. + * + * This module implements an adaptive learning algorithm based on the MIT Rule / Gradient Descent. + * It automatically adjusts PID gains (Kp, Ki) in real-time to minimize error. + */ + void ulGD_Init(ulGD_t* gd) { /* Tuning Speed */ - gd->alpha_p = 0.005f; // Kp learning rate - gd->alpha_i = 0.0005f; // Ki learning rate + gd->alpha_p = 0.001f; // Kp learning rate + gd->alpha_i = 0.0001f; // Ki learning rate /* Safety Limits */ gd->kp_min = 0.040f; diff --git a/KPI_Rover/Motors/ulMotorsController.c b/KPI_Rover/Motors/ulMotorsController.c index 0408dd3..5904460 100644 --- a/KPI_Rover/Motors/ulMotorsController.c +++ b/KPI_Rover/Motors/ulMotorsController.c @@ -10,7 +10,7 @@ static osTimerId_t motors_timer_handle; -#define MOTORS_CONTROL_PERIOD_MS 20 +#define MOTORS_CONTROL_PERIOD_MS 5 const float dt_sec = MOTORS_CONTROL_PERIOD_MS / 1000.0f; @@ -129,10 +129,9 @@ void ulMotorsController_Init(ulMotorsController_t* ctrl) ulPID_Init(&ctrl->pids[i], kp, ki, kd); - ctrl->pids[i].deadzone = 0.02f; + ctrl->pids[i].deadzone = 0.05f; ctrl->pids[i].slew_rate = 25.0f; - ctrl->pids[i].d_alpha = 0.2f; - ctrl->pids[i].sp_alpha = 0.1f; + ctrl->pids[i].d_alpha = 0.05f; ulGD_Init(&ctrl->gd[i]); @@ -348,6 +347,6 @@ void ulMotorsController_Task(void* argument) ulMotorsController_UpdateFromDB(&g_motors_ctrl); - osDelay(10); + osDelay(20); } }