fixed message config struct and status codes

- used magic values for status codes
- hard to copy pid constants from config

diff --git a/otto_controller/Core/Inc/communication/otto_messages.h b/otto_controller/Core/Inc/communication/otto_messages.h
index 9fef73ce5ff28724e19c8657aa12d9cfc9cb1ab1..51db7db78c4e20af8b1f5c0e975b0f3ff6d95b5b 100644 (file)
--- 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 79a1d109c1dc0c3b3c6b36609c5ed4db287feb54..cfadaece42e89ec97515fb99a85762b3a4c3e944 100644 (file)
--- 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 de7197068002ca248c007a4f6a3a99506388cc06..94ab0f0bc364b36e002d084f0aead25012fb0619 100644 (file)
--- a/otto_controller/Core/Inc/main.h
+++ b/otto_controller/Core/Inc/main.h
@@ -43,6 +43,9 @@ extern "C" {
 /* Exported constants --------------------------------------------------------*/\r
 /* USER CODE BEGIN EC */\r
 \r
+extern int32_t pid_max;\r
+extern int32_t pid_min;\r
+\r
 /* USER CODE END EC */\r
 \r
 /* Exported macro ------------------------------------------------------------*/\r

diff --git a/otto_controller/Core/Src/control.c b/otto_controller/Core/Src/control.c
index 77c1499be54ed2df37e8efd58c29fdd05b213f5f..52dd753e443bdfd76fa05b10a15d7ca3bdddcd41 100644 (file)
--- 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 1de90b002d34784010f3b76769ccdc31ef306f78..18d1686e3a2a646806d3fae881f976ca61bd0fa6 100644 (file)
--- a/otto_controller/Core/Src/main.c
+++ b/otto_controller/Core/Src/main.c
@@ -28,6 +28,8 @@
 /* Private includes ----------------------------------------------------------*/\r
 /* USER CODE BEGIN Includes */\r
 \r
+#include <string.h> // memcpy\r
+\r
 #include "control/encoder.h"\r
 #include "control/odometry.h"\r
 #include "control/motor_controller.h"\r
@@ -55,7 +57,14 @@
 \r
 /* USER CODE BEGIN PV */\r
 \r
-static Encoder encoders[2] = {{0}, {0}};\r
+static Encoder encoders[2] = {\r
+  {\r
+    .timer = &htim5,\r
+  },\r
+  {\r
+    .timer = &htim2,\r
+  },\r
+};\r
 Encoder *encoder_right = &encoders[0];\r
 Encoder *encoder_left = &encoders[1];\r
 \r
@@ -65,6 +74,9 @@ Pid pid_left  = {0};
 Pid pid_right = {0};\r
 Pid pid_cross = {0};\r
 \r
+int32_t pid_max = 0;\r
+int32_t pid_min = 0;\r
+\r
 static MotorController motors[2] = {\r
   {\r
     // Right motor\r
@@ -97,7 +109,7 @@ volatile int32_t left_ticks;
 volatile int32_t right_ticks;\r
 volatile float previous_tx_millis;\r
 volatile uint8_t tx_done_flag = 1;\r
-volatile uint16_t otto_status = 0;\r
+volatile MessageStatusCode otto_status = MessageStatusCode_Waiting4Config;\r
 \r
 /* USER CODE END PV */\r
 \r
@@ -150,56 +162,45 @@ int main(void) {
   MX_NVIC_Init();\r
   /* USER CODE BEGIN 2 */\r
 \r
-  //wait for config\r
-  HAL_StatusTypeDef config_status = HAL_UART_Receive(&huart6, (uint8_t*) &config_msg, sizeof(config_msg), 60*1000);\r
-  uint32_t config_crc = HAL_CRC_Calculate(&hcrc, (uint32_t*) &config_msg, sizeof(config_msg) - 4);\r
-  if (config_crc != config_msg.crc || config_status != HAL_OK){\r
-    status_msg.status = 2;\r
-    status_msg.crc = HAL_CRC_Calculate(&hcrc, (uint32_t*) &status_msg, sizeof(status_msg) - 4);\r
-    while(1){\r
-      HAL_UART_Transmit(&huart6, (uint8_t*) &status_msg, sizeof(status_msg), 1000);\r
+  // NOTE(lb): timeout is in milliseconds\r
+  // wait for config\r
+  HAL_StatusTypeDef config_status =\r
+    HAL_UART_Receive(&huart6, (uint8_t*)&config_msg,\r
+                     sizeof config_msg, 60 * 1000); // 60sec\r
+  uint32_t config_crc =\r
+    HAL_CRC_Calculate(&hcrc, (uint32_t*)&config_msg,\r
+                      (sizeof config_msg) - (sizeof config_msg.crc));\r
+  if (config_crc != config_msg.crc || config_status != HAL_OK) {\r
+    status_msg.status = MessageStatusCode_Error_Config;\r
+    status_msg.crc =\r
+      HAL_CRC_Calculate(&hcrc, (uint32_t*)&status_msg, sizeof(status_msg) - 4);\r
+    for (;;) {\r
+      HAL_UART_Transmit(&huart6, (uint8_t*)&status_msg,\r
+                        sizeof(status_msg), 1000); // 1sec\r
     }\r
   }\r
 \r
-  encoder_left->timer = &htim2;\r
+  // ======================================================================\r
+  // NOTE(lb): all of this should be transformed in compile time constants\r
+  odom.baseline = config_msg.baseline;\r
   encoder_left->wheel_circumference = config_msg.left_wheel_circumference;\r
   encoder_left->ticks_per_revolution = config_msg.ticks_per_revolution;\r
-\r
-  encoder_right->timer = &htim5;\r
   encoder_right->wheel_circumference = config_msg.right_wheel_circumference;\r
   encoder_right->ticks_per_revolution = config_msg.ticks_per_revolution;\r
 \r
-  odom.baseline = config_msg.baseline;\r
+  // NOTE(lb): maybe even this but i'm not sure. And at this point\r
+  //           i'm not even sure that there is a need for a config message.\r
+  memcpy(&pid_left.ks,  &config_msg.pid_ks_left,  sizeof pid_left.ks);\r
+  memcpy(&pid_right.ks, &config_msg.pid_ks_right, sizeof pid_right.ks);\r
+  memcpy(&pid_cross.ks, &config_msg.pid_ks_cross, sizeof pid_cross.ks);\r
+  // ======================================================================\r
 \r
   encoder_init(encoders);\r
   motorcontroller_init(motors);\r
 \r
   //right and left motors have the same parameters\r
-  uint32_t max_dutycycle = *(&htim4.Instance->ARR);\r
-  int pid_min = 0;\r
-  int pid_max = 0;\r
-  pid_min = -(int) max_dutycycle;\r
-  pid_max = (int) max_dutycycle;\r
-\r
-\r
-  pid_left.kp = config_msg.kp_left;\r
-  pid_left.ki = config_msg.ki_left;\r
-  pid_left.kd = config_msg.kd_left;\r
-  pid_left.min = pid_min;\r
-  pid_left.max = pid_max;\r
-\r
-  pid_right.kp = config_msg.kp_right;\r
-  pid_right.ki = config_msg.ki_right;\r
-  pid_right.kd = config_msg.kd_right;\r
-  pid_right.min = pid_min;\r
-  pid_right.max = pid_max;\r
-\r
-  pid_cross.kp = config_msg.kp_cross;\r
-  pid_cross.ki = config_msg.ki_cross;\r
-  pid_cross.kd = config_msg.kd_cross;\r
-  pid_cross.min = pid_min;\r
-  pid_cross.max = pid_max;\r
-\r
+  pid_max = (int32_t)htim4.Instance->ARR;\r
+  pid_min = -pid_max;\r
 \r
   motorcontroller_brake(motor_left);\r
   motorcontroller_brake(motor_right);\r
@@ -336,11 +337,11 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
   if (crc_rx == vel_msg.crc) {\r
     linear_velocity = vel_msg.linear_velocity;\r
     angular_velocity = vel_msg.angular_velocity;\r
-    otto_status = 1;\r
+    otto_status = MessageStatusCode_Running;\r
   } else {\r
     linear_velocity = 0;\r
     angular_velocity = 0;\r
-    otto_status = 3;\r
+    otto_status = MessageStatusCode_Error_Velocity;\r
   }\r
 \r
   odometry_setpoint_from_cmdvel(&odom, linear_velocity, angular_velocity);\r
@@ -403,13 +404,13 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
     motorcontroller_brake(motor_right);\r
     //stop TIM6 interrupt (used for PID control)\r
     HAL_TIM_Base_Stop_IT(&htim6);\r
-    otto_status = 4;\r
+    otto_status = MessageStatusCode_Fault_HBridge;\r
   } else if (GPIO_Pin == fault2_Pin) {\r
     motorcontroller_brake(motor_left);\r
     motorcontroller_brake(motor_right);\r
     //stop TIM6 interrupt (used for PID control)\r
     HAL_TIM_Base_Stop_IT(&htim6);\r
-    otto_status = 4;\r
+    otto_status = MessageStatusCode_Fault_HBridge;\r
   }\r
 \r
 }\r