From d4b9b065718725e7ec0c3f236f03a2f7db445b1e Mon Sep 17 00:00:00 2001 From: LeonardoBizzoni Date: Thu, 23 Oct 2025 10:05:33 +0200 Subject: [PATCH] fixed message config struct and status codes - used magic values for status codes - hard to copy pid constants from config --- .../Core/Inc/communication/otto_messages.h | 54 ++++++------ otto_controller/Core/Inc/control/pid.h | 23 +++-- otto_controller/Core/Inc/main.h | 3 + otto_controller/Core/Src/control.c | 12 ++- otto_controller/Core/Src/main.c | 87 ++++++++++--------- 5 files changed, 96 insertions(+), 83 deletions(-) diff --git a/otto_controller/Core/Inc/communication/otto_messages.h b/otto_controller/Core/Inc/communication/otto_messages.h index 9fef73c..51db7db 100644 --- a/otto_controller/Core/Inc/communication/otto_messages.h +++ b/otto_controller/Core/Inc/communication/otto_messages.h @@ -8,39 +8,43 @@ #ifndef INC_COMMUNICATION_OTTO_MESSAGES_H_ #define INC_COMMUNICATION_OTTO_MESSAGES_H_ -typedef struct _ConfigMessage { - uint32_t ticks_per_revolution; //x4 resolution - float baseline; //in meters - float left_wheel_circumference; //in meters - float right_wheel_circumference; //in meters - float kp_left; - float ki_left; - float kd_left; - float kp_right; - float ki_right; - float kd_right; - float kp_cross; - float ki_cross; - float kd_cross; +typedef struct { + // NOTE(lb): Why do we need to receive the wheels circumference and baseline? + // Also why BOTH wheels circumference? Aren't they always the same? + // Same for ticksXrevolution, we should already know all of this stuff. + // Even if this has to run on different devices its weird not to + // use compile time constants. + uint32_t ticks_per_revolution; // x4 resolution + float baseline; // in meters + float left_wheel_circumference; // in meters + float right_wheel_circumference; // in meters + + PidConstants pid_ks_left; + PidConstants pid_ks_right; + PidConstants pid_ks_cross; + + // NOTE(lb): must be either the last or the first parameter + // to skip it during CRC validation uint32_t crc; } ConfigMessage; -typedef struct _VelocityMessage { +typedef struct { float linear_velocity; float angular_velocity; uint32_t crc; } VelocityMessage; -typedef struct _StatusMessage { - /* - * Status codes: - * 0 - waiting for config - * 1 - running - * 2 - error receiving config - * 3 - error receiving vel - * 4 - H-Bridge fault - */ - uint16_t status; +typedef uint16_t MessageStatusCode; +enum { + MessageStatusCode_Waiting4Config = 0, + MessageStatusCode_Running = 1, + MessageStatusCode_Error_Config = 2, + MessageStatusCode_Error_Velocity = 3, + MessageStatusCode_Fault_HBridge = 4, +}; + +typedef struct { + MessageStatusCode status; uint16_t delta_millis; int32_t left_ticks; int32_t right_ticks; diff --git a/otto_controller/Core/Inc/control/pid.h b/otto_controller/Core/Inc/control/pid.h index 79a1d10..cfadaec 100644 --- a/otto_controller/Core/Inc/control/pid.h +++ b/otto_controller/Core/Inc/control/pid.h @@ -1,12 +1,22 @@ #ifndef PID_H #define PID_H -typedef struct { - //PID constants - float kp; - float ki; - float kd; +typedef union { + struct { + float proportional; + float integral; + float derivative; + }; + struct { + float kp; + float ki; + float kd; + }; + float values[3]; +} PidConstants; +typedef struct { + PidConstants ks; float error; float setpoint; @@ -15,9 +25,6 @@ typedef struct { //needed for derivative term float previous_error; - - int32_t min; - int32_t max; } Pid; int32_t pid_update(Pid *pid, float measure); diff --git a/otto_controller/Core/Inc/main.h b/otto_controller/Core/Inc/main.h index de71970..94ab0f0 100644 --- a/otto_controller/Core/Inc/main.h +++ b/otto_controller/Core/Inc/main.h @@ -43,6 +43,9 @@ extern "C" { /* Exported constants --------------------------------------------------------*/ /* USER CODE BEGIN EC */ +extern int32_t pid_max; +extern int32_t pid_min; + /* USER CODE END EC */ /* Exported macro ------------------------------------------------------------*/ diff --git a/otto_controller/Core/Src/control.c b/otto_controller/Core/Src/control.c index 77c1499..52dd753 100644 --- a/otto_controller/Core/Src/control.c +++ b/otto_controller/Core/Src/control.c @@ -81,20 +81,18 @@ void odometry_setpoint_from_cmdvel(Odometry *odom, float linear_vel, int32_t pid_update(Pid *pid, float measure) { pid->error = pid->setpoint - measure; - //proportional term - float output = pid->error * pid->kp; + float output = pid->error * pid->ks.proportional; //integral term without windup pid->error_sum += pid->error; - output += pid->error_sum * pid->ki; + output += pid->error_sum * pid->ks.integral; - //derivative term - output += (pid->error - pid->previous_error) * pid->kd; + output += (pid->error - pid->previous_error) * pid->ks.derivative; pid->previous_error = pid->error; - //anti windup + // anti windup pid->error_sum -= pid->error; - int32_t integer_output = CLAMP(((int32_t)output), pid->min, pid->max); + int32_t integer_output = CLAMP(((int32_t)output), pid_min, pid_max); return integer_output; } diff --git a/otto_controller/Core/Src/main.c b/otto_controller/Core/Src/main.c index 1de90b0..18d1686 100644 --- a/otto_controller/Core/Src/main.c +++ b/otto_controller/Core/Src/main.c @@ -28,6 +28,8 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ +#include // memcpy + #include "control/encoder.h" #include "control/odometry.h" #include "control/motor_controller.h" @@ -55,7 +57,14 @@ /* USER CODE BEGIN PV */ -static Encoder encoders[2] = {{0}, {0}}; +static Encoder encoders[2] = { + { + .timer = &htim5, + }, + { + .timer = &htim2, + }, +}; Encoder *encoder_right = &encoders[0]; Encoder *encoder_left = &encoders[1]; @@ -65,6 +74,9 @@ Pid pid_left = {0}; Pid pid_right = {0}; Pid pid_cross = {0}; +int32_t pid_max = 0; +int32_t pid_min = 0; + static MotorController motors[2] = { { // Right motor @@ -97,7 +109,7 @@ volatile int32_t left_ticks; volatile int32_t right_ticks; volatile float previous_tx_millis; volatile uint8_t tx_done_flag = 1; -volatile uint16_t otto_status = 0; +volatile MessageStatusCode otto_status = MessageStatusCode_Waiting4Config; /* USER CODE END PV */ @@ -150,56 +162,45 @@ int main(void) { MX_NVIC_Init(); /* USER CODE BEGIN 2 */ - //wait for config - HAL_StatusTypeDef config_status = HAL_UART_Receive(&huart6, (uint8_t*) &config_msg, sizeof(config_msg), 60*1000); - uint32_t config_crc = HAL_CRC_Calculate(&hcrc, (uint32_t*) &config_msg, sizeof(config_msg) - 4); - if (config_crc != config_msg.crc || config_status != HAL_OK){ - status_msg.status = 2; - status_msg.crc = HAL_CRC_Calculate(&hcrc, (uint32_t*) &status_msg, sizeof(status_msg) - 4); - while(1){ - HAL_UART_Transmit(&huart6, (uint8_t*) &status_msg, sizeof(status_msg), 1000); + // NOTE(lb): timeout is in milliseconds + // wait for config + HAL_StatusTypeDef config_status = + HAL_UART_Receive(&huart6, (uint8_t*)&config_msg, + sizeof config_msg, 60 * 1000); // 60sec + uint32_t config_crc = + HAL_CRC_Calculate(&hcrc, (uint32_t*)&config_msg, + (sizeof config_msg) - (sizeof config_msg.crc)); + if (config_crc != config_msg.crc || config_status != HAL_OK) { + status_msg.status = MessageStatusCode_Error_Config; + status_msg.crc = + HAL_CRC_Calculate(&hcrc, (uint32_t*)&status_msg, sizeof(status_msg) - 4); + for (;;) { + HAL_UART_Transmit(&huart6, (uint8_t*)&status_msg, + sizeof(status_msg), 1000); // 1sec } } - encoder_left->timer = &htim2; + // ====================================================================== + // NOTE(lb): all of this should be transformed in compile time constants + odom.baseline = config_msg.baseline; encoder_left->wheel_circumference = config_msg.left_wheel_circumference; encoder_left->ticks_per_revolution = config_msg.ticks_per_revolution; - - encoder_right->timer = &htim5; encoder_right->wheel_circumference = config_msg.right_wheel_circumference; encoder_right->ticks_per_revolution = config_msg.ticks_per_revolution; - odom.baseline = config_msg.baseline; + // NOTE(lb): maybe even this but i'm not sure. And at this point + // i'm not even sure that there is a need for a config message. + memcpy(&pid_left.ks, &config_msg.pid_ks_left, sizeof pid_left.ks); + memcpy(&pid_right.ks, &config_msg.pid_ks_right, sizeof pid_right.ks); + memcpy(&pid_cross.ks, &config_msg.pid_ks_cross, sizeof pid_cross.ks); + // ====================================================================== encoder_init(encoders); motorcontroller_init(motors); //right and left motors have the same parameters - uint32_t max_dutycycle = *(&htim4.Instance->ARR); - int pid_min = 0; - int pid_max = 0; - pid_min = -(int) max_dutycycle; - pid_max = (int) max_dutycycle; - - - 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; - + pid_max = (int32_t)htim4.Instance->ARR; + pid_min = -pid_max; motorcontroller_brake(motor_left); motorcontroller_brake(motor_right); @@ -336,11 +337,11 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) { if (crc_rx == vel_msg.crc) { linear_velocity = vel_msg.linear_velocity; angular_velocity = vel_msg.angular_velocity; - otto_status = 1; + otto_status = MessageStatusCode_Running; } else { linear_velocity = 0; angular_velocity = 0; - otto_status = 3; + otto_status = MessageStatusCode_Error_Velocity; } odometry_setpoint_from_cmdvel(&odom, linear_velocity, angular_velocity); @@ -403,13 +404,13 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { motorcontroller_brake(motor_right); //stop TIM6 interrupt (used for PID control) HAL_TIM_Base_Stop_IT(&htim6); - otto_status = 4; + otto_status = MessageStatusCode_Fault_HBridge; } else if (GPIO_Pin == fault2_Pin) { motorcontroller_brake(motor_left); motorcontroller_brake(motor_right); //stop TIM6 interrupt (used for PID control) HAL_TIM_Base_Stop_IT(&htim6); - otto_status = 4; + otto_status = MessageStatusCode_Fault_HBridge; } } -- 2.52.0