diff --git a/Core/Src/main.c b/Core/Src/main.c index 824509b..559d1fd 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -760,7 +760,7 @@ 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 */ + /*Configure GPIO pins : OTG_FS_PowerSwitchOn_Pin PC4 PC5 */ GPIO_InitStruct.Pin = OTG_FS_PowerSwitchOn_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; 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/KPIRover.c b/KPI_Rover/KPIRover.c index d04d367..f32b8dc 100644 --- a/KPI_Rover/KPIRover.c +++ b/KPI_Rover/KPIRover.c @@ -1,10 +1,28 @@ +#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, @@ -16,4 +34,12 @@ static struct ulDatabase_ParamMetadata ulDatabase_params[] = { 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 new file mode 100644 index 0000000..69608f3 --- /dev/null +++ b/KPI_Rover/Motors/PCA9685.c @@ -0,0 +1,101 @@ +#include "PCA9685.h" +#include "stm32f4xx_hal.h" + +/* I2C Handle defined in main.c */ +extern I2C_HandleTypeDef hi2c1; + +/* 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, + PCA9685_ADDR, + reg, + I2C_MEMADD_SIZE_8BIT, + &data, + 1, + 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]; + /* 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, + reg, + I2C_MEMADD_SIZE_8BIT, + buf, + 4, + 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); + + /* 2. Setup PWM Frequency ~1000Hz */ + /* Formula: prescale = 25MHz / (4096 * update_rate) - 1 */ + /* For 1000Hz: 25000000 / (4096 * 1000) - 1 ~= 5 */ + uint8_t prescale = 5; + + /* To change pre-scaler, we must be in SLEEP mode */ + pca9685_write(MODE1, 0x10); // Sleep mode + HAL_Delay(1); + + 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/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 new file mode 100644 index 0000000..46a6271 --- /dev/null +++ b/KPI_Rover/Motors/drvMotors.c @@ -0,0 +1,55 @@ +#include +#include +#include "ulog.h" + +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); + } + + motor->enabled = false; + DriverMotor_setDirection(motor, true); +} + +void DriverMotor_Enable(drvMotor_t* motor) +{ + motor->enabled = true; + DriverMotor_setPwm(motor, 0); +} + +void DriverMotor_Disable(drvMotor_t* motor) +{ + motor->enabled = false; + DriverMotor_setPwm(motor, 0); +} + +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); +} + +void DriverMotor_setPwm(drvMotor_t* motor, uint16_t pwm) +{ + if (!motor->enabled) + 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 new file mode 100644 index 0000000..ee6d5f8 --- /dev/null +++ b/KPI_Rover/Motors/drvMotors.h @@ -0,0 +1,41 @@ +#ifndef MOTORS_DRIVER_H +#define MOTORS_DRIVER_H + +#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* IN2_port; + uint16_t IN2_pin; + + pwm_source_t pwm_src; + + union { + struct { + TIM_HandleTypeDef* htim; + uint32_t channel; + } tim; + + struct { + uint8_t channel; + } pca; + } pwm; + + bool enabled; +} drvMotor_t; + +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/ulGD.c b/KPI_Rover/Motors/ulGD.c new file mode 100644 index 0000000..4f71dec --- /dev/null +++ b/KPI_Rover/Motors/ulGD.c @@ -0,0 +1,85 @@ +#include "ulGD.h" +#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.001f; // Kp learning rate + gd->alpha_i = 0.0001f; // 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 new file mode 100644 index 0000000..5904460 --- /dev/null +++ b/KPI_Rover/Motors/ulMotorsController.c @@ -0,0 +1,352 @@ +#include "ulog.h" +#include "cmsis_os.h" +#include +#include +#include "Motors/PCA9685.h" +#include +#include +#include +#include "usbd_cdc_if.h" + +static osTimerId_t motors_timer_handle; + +#define MOTORS_CONTROL_PERIOD_MS 5 + +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_FL + 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_4; + ctrl->motors[0].pwm_src = PWM_SRC_PCA9685; + ctrl->motors[0].pwm.pca.channel = 0; + + // MOTOR_2_FR + 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].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; + +} + + +void ulMotorsController_Init(ulMotorsController_t* ctrl) +{ + ULOG_INFO("MotorsController init"); + + init_motors_hw_mapping(ctrl); + + for (int i = 0; i < ULMOTORS_NUM_MOTORS; i++) { + + float kp = 0.0f; + float ki = 0.0f; + float kd = 0.0f; + + 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.05f; + ctrl->pids[i].slew_rate = 25.0f; + ctrl->pids[i].d_alpha = 0.05f; + + 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; + + + 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] pwm_max=%.0f", i, ctrl->pwm_max[i]); + } +} + + +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), 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; + + float max_val = ctrl->pwm_max[i]; + if (max_val < 1.0f) max_val = 4095.0f; + + uint32_t pwm_hw = (uint32_t)(pwm_norm * max_val); + + 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; + + +// 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; + + 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]); + ulPID_Reset(&g_motors_ctrl.pids[i]); + } + + g_motors_state = MOTORS_STATE_RUN; + } + break; + } + + case MOTORS_STATE_RUN: + { + ulMotorsController_UpdateFeedback(&g_motors_ctrl); + 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); + + PCA9685_Init(); + + 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 (;;) { + + ulMotorsController_UpdateFromDB(&g_motors_ctrl); + + osDelay(20); + } +} diff --git a/KPI_Rover/Motors/ulMotorsController.h b/KPI_Rover/Motors/ulMotorsController.h new file mode 100644 index 0000000..b8131c2 --- /dev/null +++ b/KPI_Rover/Motors/ulMotorsController.h @@ -0,0 +1,47 @@ +#ifndef UL_MOTORS_CONTROLLER_H +#define UL_MOTORS_CONTROLLER_H + +#include +#include +#include +#include + +#define ULMOTORS_NUM_MOTORS 4 + +typedef struct { + drvMotor_t motors[ULMOTORS_NUM_MOTORS]; + ulPID_t pids[ULMOTORS_NUM_MOTORS]; + ulGD_t gd[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/ulPID.c b/KPI_Rover/Motors/ulPID.c new file mode 100644 index 0000000..6ddae4b --- /dev/null +++ b/KPI_Rover/Motors/ulPID.c @@ -0,0 +1,171 @@ +#include +#include +#include +#include "stm32f4xx_hal.h" + +// 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; + pid->ki = ki; + pid->kd = kd; + + pid->integral = 0.0f; + 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 = 1.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 = 1.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; +} + +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; +} + +/** + * 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; + pid->prev_meas = measured; + pid->d_filtered = 0.0f; + pid->last_output = 0.0f; + 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; + + /* 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 * dt); + } + + float actuator = out_clamped; + + /* 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; + + 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; +} diff --git a/KPI_Rover/Motors/ulPID.h b/KPI_Rover/Motors/ulPID.h new file mode 100644 index 0000000..f73248d --- /dev/null +++ b/KPI_Rover/Motors/ulPID.h @@ -0,0 +1,48 @@ +#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); +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