Issue #17: ecu stm32 implement motors control

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;

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,

KPI_Rover/KPIRover.c

@@ -1,10 +1,28 @@
+#include <Motors/ulMotorsController.h>
+#include <Encoders/ulEncoder.h>
 #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);
+
 }

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);
+}

KPI_Rover/Motors/PCA9685.h

@@ -0,0 +1,5 @@
+#pragma once
+#include <stdint.h>
+
+void PCA9685_Init(void);
+void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off);

KPI_Rover/Motors/drvMotors.c

@@ -0,0 +1,55 @@
+#include <Motors/drvMotors.h>
+#include <Motors/PCA9685.h>
+#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);
+    }
+}
+
+
+

KPI_Rover/Motors/drvMotors.h

@@ -0,0 +1,41 @@
+#ifndef MOTORS_DRIVER_H
+#define MOTORS_DRIVER_H
+
+#include "stm32f4xx_hal.h"
+#include <stdbool.h>
+
+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

KPI_Rover/Motors/ulGD.c

@@ -0,0 +1,85 @@
+#include "ulGD.h"
+#include <math.h>
+#include <stdbool.h>
+
+/**
+ * @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;
+}

KPI_Rover/Motors/ulGD.h

@@ -0,0 +1,23 @@
+#ifndef MOTORS_ULGD_H_
+#define MOTORS_ULGD_H_
+
+#include <stdint.h>
+#include <math.h>
+
+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_ */