From aa6d1ac4386186b65aaa56a857c1dcef4ac8e2a8 Mon Sep 17 00:00:00 2001 From: LeonardoBizzoni Date: Wed, 22 Oct 2025 23:10:05 +0200 Subject: [PATCH] finished C upgrade --- otto_controller/Core/Inc/control/encoder.h | 6 +- .../Core/Inc/control/motor_controller.h | 2 +- otto_controller/Core/Inc/control/odometry.h | 5 +- otto_controller/Core/Inc/control/pid.h | 103 ++--------------- otto_controller/Core/Src/control.c | 107 ++++++++++++++++++ .../Core/Src/control/motor_controller.c | 32 ------ otto_controller/Core/Src/{main.cpp => main.c} | 47 ++++---- 7 files changed, 152 insertions(+), 150 deletions(-) create mode 100644 otto_controller/Core/Src/control.c delete mode 100644 otto_controller/Core/Src/control/motor_controller.c rename otto_controller/Core/Src/{main.cpp => main.c} (90%) diff --git a/otto_controller/Core/Inc/control/encoder.h b/otto_controller/Core/Inc/control/encoder.h index 296ae27..349c744 100644 --- a/otto_controller/Core/Inc/control/encoder.h +++ b/otto_controller/Core/Inc/control/encoder.h @@ -19,8 +19,8 @@ float encoder_linear_velocity(Encoder *encoder); void encoder_count_reset(Encoder *encoder); int encoder_count_get(Encoder *encoder); -inline float meters_from_ticks(float encoder_ticks, - float wheel_circumference, - float ticks_per_revolution); +float meters_from_ticks(float encoder_ticks, + float wheel_circumference, + float ticks_per_revolution); #endif diff --git a/otto_controller/Core/Inc/control/motor_controller.h b/otto_controller/Core/Inc/control/motor_controller.h index bbad5dd..3bf6270 100644 --- a/otto_controller/Core/Inc/control/motor_controller.h +++ b/otto_controller/Core/Inc/control/motor_controller.h @@ -4,7 +4,7 @@ #include "main.h" // TODO(lb): reorder after C++ removal to avoid padding -typedef struct MotorController { +typedef struct { GPIO_TypeDef *sleep_gpio_port; uint16_t sleep_pin; GPIO_TypeDef *dir_gpio_port; diff --git a/otto_controller/Core/Inc/control/odometry.h b/otto_controller/Core/Inc/control/odometry.h index 4669af8..462c705 100644 --- a/otto_controller/Core/Inc/control/odometry.h +++ b/otto_controller/Core/Inc/control/odometry.h @@ -21,10 +21,7 @@ typedef struct { } Odometry; void odometry_setpoint_from_cmdvel(Odometry *odom, float linear_vel, - float angular_vel) { - odom->setpoint.left = linear_vel - (odom->baseline * angular_vel) / 2; - odom->setpoint.right = linear_vel + (odom->baseline * angular_vel) / 2; -} + float angular_vel); #if 0 class Odometry { diff --git a/otto_controller/Core/Inc/control/pid.h b/otto_controller/Core/Inc/control/pid.h index a8c2b6c..79a1d10 100644 --- a/otto_controller/Core/Inc/control/pid.h +++ b/otto_controller/Core/Inc/control/pid.h @@ -1,104 +1,25 @@ #ifndef PID_H #define PID_H -class Pid { - private: +typedef struct { //PID constants - float kp_; - float ki_; - float kd_; + float kp; + float ki; + float kd; - float error_; - float setpoint_; + float error; + float setpoint; //needed for integrative term - float error_sum_; + float error_sum; //needed for derivative term - float previous_error_; + float previous_error; - int min_; - int max_; + int32_t min; + int32_t max; +} Pid; - public: - Pid() { - this->kp_ = 0; - this->ki_ = 0; - this->kd_ = 0; +int32_t pid_update(Pid *pid, float measure); - this->error_ = 0; - this->setpoint_ = 0; - - this->previous_error_ = 0; - this->error_sum_ = 0; - - this->min_ = 0; - this->max_ = 0; - } - Pid(float kp, float ki, float kd, int min, int max) { - this->kp_ = kp; - this->ki_ = ki; - this->kd_ = kd; - - this->error_ = 0; - this->setpoint_ = 0; - - this->previous_error_ = 0; - this->error_sum_ = 0; - - this->min_ = min; - this->max_ = max; - - } - - void Config(float kp, float ki, float kd, int min, int max) { - this->kp_ = kp; - this->ki_ = ki; - this->kd_ = kd; - - this->error_ = 0; - this->setpoint_ = 0; - - this->previous_error_ = 0; - this->error_sum_ = 0; - - this->min_ = min; - this->max_ = max; - - } - - void Set(float setpoint) { - this->setpoint_ = setpoint; - } - - int Update(float measure) { - - this->error_ = this->setpoint_ - measure; - - //proportional term - float output = this->error_ * this->kp_; - - //integral term without windup - error_sum_ += this->error_; - output += error_sum_ * this->ki_; - - //derivative term - output += (this->error_ - this->previous_error_) * kd_; - this->previous_error_ = this->error_; - - int integer_output = static_cast(output); - - //anti windup - if (integer_output > this->max_) { - integer_output = this->max_; - this->error_sum_ -= this->error_; - } else if (integer_output < this->min_) { - integer_output = this->min_; - this->error_sum_ -= this->error_; - } - - return integer_output; - - } -}; #endif diff --git a/otto_controller/Core/Src/control.c b/otto_controller/Core/Src/control.c new file mode 100644 index 0000000..77c1499 --- /dev/null +++ b/otto_controller/Core/Src/control.c @@ -0,0 +1,107 @@ +#include "control/encoder.h" +#include "control/odometry.h" +#include "control/motor_controller.h" +#include "control/pid.h" + +// NOTE(lb): this assumes `motors` initialized and with 2 elements +void motorcontroller_init(MotorController *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); +} + +// NOTE(lb): this assumes `encoders` initialized and with 2 elements +void encoder_init(Encoder *encoders) { + HAL_TIM_Encoder_Start(encoders[0].timer, TIM_CHANNEL_ALL); + encoder_count_reset(&encoders[0]); + encoders[0].previous_millis = 0; + encoders[0].current_millis = HAL_GetTick(); + + HAL_TIM_Encoder_Start(encoders[1].timer, TIM_CHANNEL_ALL); + encoder_count_reset(&encoders[1]); + encoders[1].previous_millis = 0; + encoders[1].current_millis = HAL_GetTick(); +} + +void encoder_count_reset(Encoder *encoder) { + //set counter to half its maximum value + __HAL_TIM_SET_COUNTER(encoder->timer, (encoder->timer->Init.Period / 2)); +} + +int encoder_count_get(Encoder *encoder) { + int count = (int)__HAL_TIM_GET_COUNTER(encoder->timer) - + (encoder->timer->Init.Period / 2); + return count; +} + +void encoder_update(Encoder *encoder) { + encoder->previous_millis = encoder->current_millis; + encoder->current_millis = HAL_GetTick(); + encoder->ticks = encoder_count_get(encoder); + encoder_count_reset(encoder); +} + +float encoder_linear_velocity(Encoder *encoder) { + float meters = meters_from_ticks(encoder->ticks, + encoder->wheel_circumference, + encoder->ticks_per_revolution); + float deltatime = encoder->current_millis - encoder->previous_millis; + float linear_velocity = deltatime ? (meters / (deltatime / 1000.f)) : 0.f; + return linear_velocity; +} + +void odometry_setpoint_from_cmdvel(Odometry *odom, float linear_vel, + float angular_vel) { + odom->setpoint.left = linear_vel - (odom->baseline * angular_vel) / 2; + odom->setpoint.right = linear_vel + (odom->baseline * angular_vel) / 2; +} + +int32_t pid_update(Pid *pid, float measure) { + pid->error = pid->setpoint - measure; + + //proportional term + float output = pid->error * pid->kp; + + //integral term without windup + pid->error_sum += pid->error; + output += pid->error_sum * pid->ki; + + //derivative term + output += (pid->error - pid->previous_error) * pid->kd; + pid->previous_error = pid->error; + + //anti windup + pid->error_sum -= pid->error; + int32_t integer_output = CLAMP(((int32_t)output), pid->min, pid->max); + return integer_output; +} + + +float meters_from_ticks(float encoder_ticks, + float wheel_circumference, + float ticks_per_revolution) { + float meters = (encoder_ticks * wheel_circumference) / ticks_per_revolution; + return meters; +} diff --git a/otto_controller/Core/Src/control/motor_controller.c b/otto_controller/Core/Src/control/motor_controller.c deleted file mode 100644 index 66e9293..0000000 --- a/otto_controller/Core/Src/control/motor_controller.c +++ /dev/null @@ -1,32 +0,0 @@ -#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.c similarity index 90% rename from otto_controller/Core/Src/main.cpp rename to otto_controller/Core/Src/main.c index 6dab901..1de90b0 100644 --- a/otto_controller/Core/Src/main.cpp +++ b/otto_controller/Core/Src/main.c @@ -35,10 +35,6 @@ #include "communication/otto_messages.h" -// NOTE(lb): couldn't get it to link in the final executable -#include "./control/motor_controller.c" -#include "./control/encoder.c" - /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -59,19 +55,16 @@ /* USER CODE BEGIN PV */ -//Odometry static Encoder encoders[2] = {{0}, {0}}; Encoder *encoder_right = &encoders[0]; Encoder *encoder_left = &encoders[1]; Odometry odom = {0}; -//PID -Pid left_pid; -Pid right_pid; -Pid cross_pid; +Pid pid_left = {0}; +Pid pid_right = {0}; +Pid pid_cross = {0}; -//MotorController static MotorController motors[2] = { { // Right motor @@ -188,9 +181,25 @@ int main(void) { pid_min = -(int) max_dutycycle; pid_max = (int) max_dutycycle; - left_pid.Config(config_msg.kp_left, config_msg.ki_left, config_msg.kd_left, pid_min, pid_max); - 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); + + pid_left.kp = config_msg.kp_left; + pid_left.ki = config_msg.ki_left; + pid_left.kd = config_msg.kd_left; + pid_left.min = pid_min; + pid_left.max = pid_max; + + pid_right.kp = config_msg.kp_right; + pid_right.ki = config_msg.ki_right; + pid_right.kd = config_msg.kd_right; + pid_right.min = pid_min; + pid_right.max = pid_max; + + pid_cross.kp = config_msg.kp_cross; + pid_cross.ki = config_msg.ki_cross; + pid_cross.kd = config_msg.kd_cross; + pid_cross.min = pid_min; + pid_cross.max = pid_max; + motorcontroller_brake(motor_left); motorcontroller_brake(motor_right); @@ -297,14 +306,14 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { //PID control encoder_update(encoder_left); float left_velocity = encoder_linear_velocity(encoder_left); - int left_dutycycle = left_pid.Update(left_velocity); + int left_dutycycle = pid_update(&pid_left, left_velocity); encoder_update(encoder_right); float right_velocity = encoder_linear_velocity(encoder_right); - int right_dutycycle = right_pid.Update(right_velocity); + int right_dutycycle = pid_update(&pid_right, right_velocity); float difference = left_velocity - right_velocity; - int cross_dutycycle = cross_pid.Update(difference); + int cross_dutycycle = pid_update(&pid_cross, difference); left_dutycycle += cross_dutycycle; right_dutycycle -= cross_dutycycle; @@ -338,11 +347,11 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) { float left_setpoint = odom.velocity.left; float right_setpoint = odom.velocity.right; - left_pid.Set(left_setpoint); - right_pid.Set(right_setpoint); + pid_left.setpoint = left_setpoint; + pid_right.setpoint = right_setpoint; float cross_setpoint = left_setpoint - right_setpoint; - cross_pid.Set(cross_setpoint); + pid_cross.setpoint = cross_setpoint; /* * Manage new transmission -- 2.52.0