Issue #17: ecu stm32 implement motors control

KPI_Rover/Database/ulDatabase.h

@@ -19,6 +19,22 @@ enum ulDatabase_ParamType {
 enum ulDatabase_ParamId {
 	ENCODER_CONTROL_PERIOD_MS,
 	ENCODER_TICKS_PER_REVOLUTION,
+	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,6 +1,7 @@
+#include <Motors/ulMotorsController.h>
+#include <Encoders/ulEncoder.h>
 #include "KPIRover.h"
 #include "cmsis_os.h"
-
 #include "Database/ulDatabase.h"
 #include "Database/ulStorage.h"
 #include "Encoders/ulEncoder.h"
@@ -13,6 +14,22 @@
 static struct ulDatabase_ParamMetadata ulDatabase_params[] = {
 		{0, UINT16, true, 5}, // ENCODER_CONTROL_PERIOD_MS,
 		{0, FLOAT, true, 820.0f}, // ENCODER_TICKS_PER_REVOLUTION,
+		{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,
@@ -38,4 +55,12 @@ void KPIRover_Init(void) {
 	ulEncoder_Init();
 	ulImu_Init(&hi2c3);
 	ProtocolHandler_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,114 @@
+#include "PCA9685.h"
+#include "stm32f4xx_hal.h"
+
+/* I2C Handle defined in main.c */
+extern I2C_HandleTypeDef hi2c3;
+
+/* 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(&hi2c3,
+                      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(&hi2c3,
+                      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);
+}
+
+void PCA9685_SetPin(uint8_t channel, uint8_t val)
+{
+    if (channel > 15) return;
+
+    if (val) {
+        // Full ON: Bit 4 в ON_H 1 - 4096, OFF в 0
+        pca9685_write4(LED0_ON_L + 4 * channel, 0x1000, 0x0000);
+    } else {
+        // Full OFF: ON в 0, Bit 4 в OFF_H 1 - 4096
+        pca9685_write4(LED0_ON_L + 4 * channel, 0x0000, 0x1000);
+    }
+}

KPI_Rover/Motors/PCA9685.h

@@ -0,0 +1,11 @@
+#ifndef PCA_9685_H_
+#define PCA_9685_H_
+
+#pragma once
+#include <stdint.h>
+
+void PCA9685_Init(void);
+void PCA9685_SetPWM(uint8_t channel, uint16_t on, uint16_t off);
+void PCA9685_SetPin(uint8_t channel, uint8_t val);
+
+#endif // PCA_9685_H_

KPI_Rover/Motors/drvMotors.c

@@ -0,0 +1,74 @@
+#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);
+        PCA9685_SetPin(motor->in1_pca_channel, 0);
+        PCA9685_SetPin(motor->in2_pca_channel, 0);
+    }
+
+    motor->enabled = false;
+    motor->last_forward = 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){
+    if (motor->pwm_src == PWM_SRC_PCA9685 && motor->last_forward == forward) {
+        return;
+    }
+
+    if (motor->pwm_src == PWM_SRC_TIM) {
+        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);
+    }
+    else if (motor->pwm_src == PWM_SRC_PCA9685) {
+        if (forward) {
+            PCA9685_SetPin(motor->in1_pca_channel, 1); // IN1 = HIGH
+            PCA9685_SetPin(motor->in2_pca_channel, 0); // IN2 = LOW
+        } else {
+            PCA9685_SetPin(motor->in1_pca_channel, 0); // IN1 = LOW
+            PCA9685_SetPin(motor->in2_pca_channel, 1); // IN2 = HIGH
+        }
+
+        motor->last_forward = forward;
+    }
+}
+
+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,47 @@
+#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;
+
+    uint8_t in1_pca_channel;
+    uint8_t in2_pca_channel;
+
+    bool          last_forward;
+
+
+    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