From 43ebd9fdb59ef138e11d5deb3cdc684283260cca Mon Sep 17 00:00:00 2001 From: LeonardoBizzoni Date: Wed, 22 Oct 2025 16:37:02 +0200 Subject: [PATCH] Upgraded `MotorController` to C + started `Encoder` too --- otto_controller/Core/Inc/control/encoder.h | 112 ++++++++---------- .../Core/Inc/control/motor_controller.h | 82 ++++--------- otto_controller/Core/Inc/main.h | 9 +- .../Core/Src/control/motor_controller.c | 32 +++++ otto_controller/Core/Src/main.cpp | 70 ++++++----- otto_controller/Core/Src/sysmem.c | 33 +++--- 6 files changed, 168 insertions(+), 170 deletions(-) create mode 100644 otto_controller/Core/Src/control/motor_controller.c diff --git a/otto_controller/Core/Inc/control/encoder.h b/otto_controller/Core/Inc/control/encoder.h index 8a262de..933e5b5 100644 --- a/otto_controller/Core/Inc/control/encoder.h +++ b/otto_controller/Core/Inc/control/encoder.h @@ -3,70 +3,54 @@ #include "main.h" -class Encoder { - private: - TIM_HandleTypeDef *timer_; - uint32_t previous_millis_; - uint32_t current_millis_; - int32_t ticks_; //if negative the wheel is going backwards - float wheel_circumference_; - int ticks_per_revolution_; - - public: - Encoder() { - timer_ = NULL; - wheel_circumference_ = 0; - ticks_per_revolution_ = 0; - } - - Encoder(TIM_HandleTypeDef *timer, float wheel_circ, - int ticks_per_revolution) { - timer_ = timer; - wheel_circumference_ = wheel_circ; - ticks_per_revolution_ = ticks_per_revolution; - - } - - int GetCount() { - int count = ((int) __HAL_TIM_GET_COUNTER(this->timer_) - - ((this->timer_->Init.Period) / 2)); - return count; - } - - void ResetCount() { - //set counter to half its maximum value - __HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period / 2)); - } - - void Setup() { - HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL); - this->ResetCount(); - this->previous_millis_ = 0; - this->current_millis_ = HAL_GetTick(); - } - - void UpdateValues() { - this->previous_millis_ = this->current_millis_; - this->current_millis_ = HAL_GetTick(); - this->ticks_ = this->GetCount(); - this->ResetCount(); - } - - float GetMeters() { - float meters = ((float) this->ticks_ * this->wheel_circumference_) - / ticks_per_revolution_; - return meters; - } +typedef struct Encoder { + TIM_HandleTypeDef *timer; + uint32_t previous_millis; + uint32_t current_millis; + int32_t ticks; //if negative the wheel is going backwards + float_t wheel_circumference; + int32_t ticks_per_revolution; +}; - float GetLinearVelocity() { - this->UpdateValues(); - float meters = this->GetMeters(); - float deltaTime = this->current_millis_ - this->previous_millis_; - if (deltaTime == 0) - return 0; - float linear_velocity = (meters / (deltaTime / 1000)); - return linear_velocity; - } +void Setup() { + HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL); + this->ResetCount(); + this->previous_millis_ = 0; + this->current_millis_ = HAL_GetTick(); +} + +void ResetCount() { + //set counter to half its maximum value + __HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period / 2)); +} + +int GetCount() { + int count = ((int) __HAL_TIM_GET_COUNTER(this->timer_) + - ((this->timer_->Init.Period) / 2)); + return count; +} + +void UpdateValues() { + this->previous_millis_ = this->current_millis_; + this->current_millis_ = HAL_GetTick(); + this->ticks_ = this->GetCount(); + this->ResetCount(); +} + +float GetMeters() { + float meters = ((float) this->ticks_ * this->wheel_circumference_) + / ticks_per_revolution_; + return meters; +} + +float GetLinearVelocity() { + this->UpdateValues(); + float meters = this->GetMeters(); + float deltaTime = this->current_millis_ - this->previous_millis_; + if (deltaTime == 0) + return 0; + float linear_velocity = (meters / (deltaTime / 1000)); + return linear_velocity; +} -}; #endif diff --git a/otto_controller/Core/Inc/control/motor_controller.h b/otto_controller/Core/Inc/control/motor_controller.h index dd87ba8..bbad5dd 100644 --- a/otto_controller/Core/Inc/control/motor_controller.h +++ b/otto_controller/Core/Inc/control/motor_controller.h @@ -3,68 +3,26 @@ #include "main.h" -class MotorController { - private: - GPIO_TypeDef *sleep_gpio_port_; - uint16_t sleep_pin_; - GPIO_TypeDef *dir_gpio_port_; - uint16_t dir_pin_; - TIM_HandleTypeDef *pwm_timer_; - uint32_t pwm_channel_; - int32_t max_dutycycle_; - - public: - MotorController(GPIO_TypeDef *sleep_gpio_port, uint16_t sleep_pin, - GPIO_TypeDef *dir_gpio_port, uint16_t dir_pin, - TIM_HandleTypeDef *pwm_timer, uint32_t pwm_channel) { - this->sleep_gpio_port_ = sleep_gpio_port; - this->sleep_pin_ = sleep_pin; - this->dir_gpio_port_ = dir_gpio_port; - this->dir_pin_ = dir_pin; - this->pwm_timer_ = pwm_timer; - this->pwm_channel_ = pwm_channel; - this->max_dutycycle_ = 0; - } - - void Setup() { - HAL_TIM_PWM_Start(pwm_timer_, pwm_channel_); - this->max_dutycycle_ = pwm_timer_->Instance->ARR; - } - - void SetSpeed(int duty_cycle) { - if (duty_cycle >= 0) { - //set direction to forward - HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_SET); - - //check if duty_cycle exceeds maximum - if (duty_cycle > max_dutycycle_) - __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_); - else - __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, duty_cycle); - - } else if (duty_cycle < 0){ - //set direction to backwards - HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_RESET); - - //check if duty_cycle is lower than minimum - if (duty_cycle < -max_dutycycle_) - __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_); - else - //invert sign to make duty_cycle positive - __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, -duty_cycle); - } - - HAL_GPIO_WritePin(sleep_gpio_port_, sleep_pin_, GPIO_PIN_SET); - - } +// TODO(lb): reorder after C++ removal to avoid padding +typedef struct MotorController { + GPIO_TypeDef *sleep_gpio_port; + uint16_t sleep_pin; + GPIO_TypeDef *dir_gpio_port; + uint16_t dir_pin; + TIM_HandleTypeDef *pwm_timer; + uint32_t pwm_channel; + int32_t max_dutycycle; +} MotorController; + +typedef uint8_t MotorDirection; +enum { + MotorDirection_Backward = 0, + MotorDirection_Forward, +}; - void Brake() { - HAL_GPIO_WritePin(sleep_gpio_port_, sleep_pin_, GPIO_PIN_SET); - __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, 0); - } +void motorcontroller_init(MotorController *motors); +void motorcontroller_speed_set(MotorController *motor, int32_t duty_cycle); +void motorcontroller_brake(MotorController *motor); +void motorcontroller_coast(MotorController * motor); - void Coast() { - HAL_GPIO_WritePin(sleep_gpio_port_, sleep_pin_, GPIO_PIN_RESET); - } -}; #endif diff --git a/otto_controller/Core/Inc/main.h b/otto_controller/Core/Inc/main.h index 80943f3..de71970 100644 --- a/otto_controller/Core/Inc/main.h +++ b/otto_controller/Core/Inc/main.h @@ -47,7 +47,14 @@ extern "C" { /* Exported macro ------------------------------------------------------------*/ /* USER CODE BEGIN EM */ - +#define ABS(a) ((a) >= 0 ? (a) : (-(a))) +#define MAX(a, b) ((a) >= (b) ? (a) : (b)) +#define MIN(a, b) ((a) <= (b) ? (a) : (b)) +#define CLAMP_TOP(a, b) MIN((a), (b)) +#define CLAMP_BOT(a, b) MAX((a), (b)) +#define CLAMP(v, min, max) CLAMP_BOT(CLAMP_TOP(v, max), min) + +#define ARRLENGHT(ARR) (sizeof((ARR)) / sizeof(*(ARR))) /* USER CODE END EM */ /* Exported functions prototypes ---------------------------------------------*/ diff --git a/otto_controller/Core/Src/control/motor_controller.c b/otto_controller/Core/Src/control/motor_controller.c new file mode 100644 index 0000000..66e9293 --- /dev/null +++ b/otto_controller/Core/Src/control/motor_controller.c @@ -0,0 +1,32 @@ +#include "control/motor_controller.h" + +// NOTE(lb): this assumes `motors` initialized and with 2 elements +void motorcontroller_init(MotorController *motors) { + // TODO(lb): is `assert` a thing? + // assert(motors); + HAL_TIM_PWM_Start(motors[0].pwm_timer, motors[0].pwm_channel); + motors[0].max_dutycycle = motors[0].pwm_timer->Instance->ARR; + + HAL_TIM_PWM_Start(motors[1].pwm_timer, motors[1].pwm_channel); + motors[1].max_dutycycle = motors[1].pwm_timer->Instance->ARR; +} + +// TODO(lb): just pass the direction yourself and work with abs values +void motorcontroller_speed_set(MotorController *motor, int32_t duty_cycle) { + HAL_GPIO_WritePin(motor->dir_gpio_port, motor->dir_pin, + (GPIO_PinState) (duty_cycle >= 0 + ? MotorDirection_Forward + : MotorDirection_Backward)); + duty_cycle = CLAMP_TOP(ABS(duty_cycle), motor->max_dutycycle); + __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, duty_cycle); + HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET); +} + +void motorcontroller_brake(MotorController *motor) { + HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET); + __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, 0); +} + +void motorcontroller_coast(MotorController * motor) { + HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_RESET); +} diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp index b3c67a8..5fb089c 100644 --- a/otto_controller/Core/Src/main.cpp +++ b/otto_controller/Core/Src/main.cpp @@ -35,6 +35,9 @@ #include "communication/otto_messages.h" +// NOTE(lb): couldn't get it to link in the final executable +#include "./control/motor_controller.c" + /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -56,8 +59,8 @@ /* USER CODE BEGIN PV */ //Odometry -Encoder right_encoder; -Encoder left_encoder; +Encoder encoder_left = {0}; +Encoder encoder_right = {0}; Odometry odom; //PID @@ -66,16 +69,28 @@ Pid right_pid; Pid cross_pid; //MotorController -MotorController right_motor(sleep1_GPIO_Port, -sleep1_Pin, - dir1_GPIO_Port, - dir1_Pin, - &htim4, TIM_CHANNEL_4); -MotorController left_motor(sleep2_GPIO_Port, -sleep2_Pin, - dir2_GPIO_Port, - dir2_Pin, - &htim4, TIM_CHANNEL_3); +static MotorController motors[] = { + { + // Right motor + .sleep_gpio_port = sleep1_GPIO_Port, + .sleep_pin = sleep1_Pin, + .dir_gpio_port = dir1_GPIO_Port, + .dir_pin = dir1_Pin, + .pwm_timer = &htim4, + .pwm_channel = TIM_CHANNEL_4, + }, + { + // Left motor + .sleep_gpio_port = sleep2_GPIO_Port, + .sleep_pin = sleep2_Pin, + .dir_gpio_port = dir2_GPIO_Port, + .dir_pin = dir2_Pin, + .pwm_timer = &htim4, + .pwm_channel = TIM_CHANNEL_3, + }, +}; +MotorController *motor_right = &motors[0]; +MotorController *motor_left = &motors[1]; //Communication ConfigMessage config_msg; @@ -150,18 +165,20 @@ int main(void) { } } - left_encoder = Encoder(&htim2, config_msg.left_wheel_circumference, - config_msg.ticks_per_revolution); - right_encoder = Encoder(&htim5, config_msg.right_wheel_circumference, - config_msg.ticks_per_revolution); + left_encoder.timer = &htim2; + left_encoder.wheel_circumference = config_msg.left_wheel_circumference; + left_encoder.ticks_per_revolution = config_msg.ticks_per_revolution; + + right_encoder.timer = &htim5; + right_encoder.wheel_circumference = config_msg.right_wheel_circumference; + right_encoder.ticks_per_revolution = config_msg.ticks_per_revolution; odom = Odometry(config_msg.baseline); left_encoder.Setup(); right_encoder.Setup(); - left_motor.Setup(); - right_motor.Setup(); + motorcontroller_init(motors); //right and left motors have the same parameters uint32_t max_dutycycle = *(&htim4.Instance->ARR); @@ -174,8 +191,8 @@ int main(void) { right_pid.Config(config_msg.kp_right, config_msg.ki_right, config_msg.kd_right, pid_min, pid_max); cross_pid.Config(config_msg.kp_cross, config_msg.ki_cross, config_msg.kd_cross, pid_min, pid_max); - left_motor.Coast(); - right_motor.Coast(); + motorcontroller_brake(motor_left); + motorcontroller_brake(motor_right); //Enables TIM6 interrupt (used for PID control) HAL_TIM_Base_Start_IT(&htim6); @@ -289,9 +306,8 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { left_dutycycle += cross_dutycycle; right_dutycycle -= cross_dutycycle; - left_motor.SetSpeed(left_dutycycle); - right_motor.SetSpeed(right_dutycycle); - + motorcontroller_speed_set(motor_left, left_dutycycle); + motorcontroller_speed_set(motor_right, right_dutycycle); } uint8_t porcoddio = 0; @@ -372,14 +388,14 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if (GPIO_Pin == user_button_Pin) { //TODO ci può servire il bottone blu? } else if (GPIO_Pin == fault1_Pin) { - left_motor.Brake(); - right_motor.Brake(); + motorcontroller_brake(motor_left); + motorcontroller_brake(motor_right); //stop TIM6 interrupt (used for PID control) HAL_TIM_Base_Stop_IT(&htim6); otto_status = 4; } else if (GPIO_Pin == fault2_Pin) { - left_motor.Brake(); - right_motor.Brake(); + motorcontroller_brake(motor_left); + motorcontroller_brake(motor_right); //stop TIM6 interrupt (used for PID control) HAL_TIM_Base_Stop_IT(&htim6); otto_status = 4; diff --git a/otto_controller/Core/Src/sysmem.c b/otto_controller/Core/Src/sysmem.c index e5e1bc2..0f5b73d 100644 --- a/otto_controller/Core/Src/sysmem.c +++ b/otto_controller/Core/Src/sysmem.c @@ -3,11 +3,11 @@ ** ** File : sysmem.c ** -** Author : Auto-generated by STM32CubeIDE +** Author : Auto-generated by STM32CubeIDE ** ** Abstract : STM32CubeIDE Minimal System Memory calls file ** -** For more information about which c-functions +** For more information about which c-functions ** need which of these lowlevel functions ** please consult the Newlib libc-manual ** @@ -55,6 +55,8 @@ extern int errno; register char * stack_ptr asm("sp"); /* Functions */ +// NOTE(lb): definition was missing +typedef void* caddr_t; /** _sbrk @@ -62,22 +64,21 @@ register char * stack_ptr asm("sp"); **/ caddr_t _sbrk(int incr) { - extern char end asm("end"); - static char *heap_end; - char *prev_heap_end; + extern char end asm("end"); + static char *heap_end; + char *prev_heap_end; - if (heap_end == 0) - heap_end = &end; + if (heap_end == 0) + heap_end = &end; - prev_heap_end = heap_end; - if (heap_end + incr > stack_ptr) - { - errno = ENOMEM; - return (caddr_t) -1; - } + prev_heap_end = heap_end; + if (heap_end + incr > stack_ptr) + { + errno = ENOMEM; + return (caddr_t) -1; + } - heap_end += incr; + heap_end += incr; - return (caddr_t) prev_heap_end; + return (caddr_t) prev_heap_end; } - -- 2.52.0