clean code

diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp
index 06adeceecef4b1ab7f720c11e855959649b5c8dc..a4c2b80812d379b3e871e5e5115913c005f6e35e 100644 (file)
--- a/otto_controller/Core/Src/main.cpp
+++ b/otto_controller/Core/Src/main.cpp
@@ -84,6 +84,9 @@ Pid cross_pid(50, 20, 0, pid_min, pid_max);
 int left_dutycycle;\r
 int right_dutycycle;\r
 \r
+float left_setpoint = 0;\r
+float right_setpoint = 0;\r
+\r
 //MotorController\r
 MotorController right_motor(sleep1_GPIO_Port,\r
 sleep1_Pin,\r
@@ -96,6 +99,7 @@ sleep2_Pin,
                            dir2_Pin,\r
                            &htim4, TIM_CHANNEL_3);\r
 \r
+//Communication\r
 uint8_t proto_buffer_rx[50];\r
 pb_istream_t in_pb_stream;\r
 \r
@@ -109,13 +113,8 @@ StatusMessage status_msg;
 bool tx_status;\r
 float previous_tx_millis;\r
 \r
-ConfigCommand config_cmd;\r
-\r
 int otto_status = 0;\r
 \r
-int test = 0;\r
-int error = 0;\r
-\r
 /* USER CODE END PV */\r
 \r
 /* Private function prototypes -----------------------------------------------*/\r
@@ -173,19 +172,20 @@ int main(void)
   left_encoder.Setup();\r
   right_encoder.Setup();\r
 \r
-  left_motor.setup();\r
-  right_motor.setup();\r
+  left_motor.Setup();\r
+  right_motor.Setup();\r
 \r
   //right and left motors have the same parameters\r
-  pid_min = -left_motor.max_dutycycle_;\r
-  pid_max = left_motor.max_dutycycle_;\r
+  uint32_t max_dutycycle = *(&htim4.Instance->ARR);\r
+  pid_min = - (int) max_dutycycle;\r
+  pid_max = (int) max_dutycycle;\r
 \r
-  left_pid.config(180, 200, 0, pid_min, pid_max);\r
-  right_pid.config(185, 195, 0, pid_min, pid_max);\r
-  cross_pid.config(50, 20, 0, pid_min, pid_max);\r
+  left_pid.Config(180, 200, 0, pid_min, pid_max);\r
+  right_pid.Config(185, 195, 0, pid_min, pid_max);\r
+  cross_pid.Config(50, 20, 0, pid_min, pid_max);\r
 \r
-  left_motor.coast();\r
-  right_motor.coast();\r
+  left_motor.Coast();\r
+  right_motor.Coast();\r
 \r
   //protobuffer messages init\r
   vel_cmd = VelocityCommand_init_zero;\r
@@ -278,16 +278,16 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
   if (htim->Instance == TIM3) {\r
 \r
     left_velocity = left_encoder.GetLinearVelocity();\r
-    left_dutycycle = left_pid.update(left_velocity);\r
-    left_motor.set_speed(left_dutycycle);\r
+    left_dutycycle = left_pid.Update(left_velocity);\r
+    left_motor.SetSpeed(left_dutycycle);\r
 \r
     right_velocity = right_encoder.GetLinearVelocity();\r
-    right_dutycycle = right_pid.update(right_velocity);\r
-    right_motor.set_speed(right_dutycycle);\r
+    right_dutycycle = right_pid.Update(right_velocity);\r
+    right_motor.SetSpeed(right_dutycycle);\r
 \r
     float difference = left_velocity - right_velocity;\r
 \r
-    int cross_dutycycle = cross_pid.update(difference);\r
+    int cross_dutycycle = cross_pid.Update(difference);\r
 \r
     left_dutycycle += cross_dutycycle;\r
     right_dutycycle -= cross_dutycycle;\r
@@ -302,9 +302,10 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
     float left_wheel = left_encoder.GetLinearVelocity();\r
     float right_wheel = right_encoder.GetLinearVelocity();\r
 \r
-//    odom.FromWheelVelToOdom(left_wheel, right_wheel);\r
+    odom.FromWheelVelToOdom(left_wheel, right_wheel);\r
 \r
-    odom.FromWheelVelToOdom(0.5, -0.5);\r
+    // used to debug\r
+    //odom.FromWheelVelToOdom(left_setpoint, right_setpoint);\r
 \r
     status_msg.linear_velocity = odom.GetLinearVelocity();\r
     status_msg.angular_velocity = odom.GetAngularVelocity();\r
@@ -324,8 +325,6 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
 \r
 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {\r
 \r
-  test++;\r
-\r
   float linear_velocity;\r
   float angular_velocity;\r
 \r
@@ -340,14 +339,14 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
 \r
     odom.FromCmdVelToSetpoint(linear_velocity, angular_velocity);\r
 \r
-    float left_setpoint = odom.GetLeftVelocity();\r
-    float right_setpoint = odom.GetRightVelocity();\r
+    left_setpoint = odom.GetLeftVelocity();\r
+    right_setpoint = odom.GetRightVelocity();\r
 \r
-    left_pid.set(left_setpoint);\r
-    right_pid.set(right_setpoint);\r
+    left_pid.Set(left_setpoint);\r
+    right_pid.Set(right_setpoint);\r
 \r
     float cross_setpoint = left_setpoint - right_setpoint;\r
-    cross_pid.set(cross_setpoint);\r
+    cross_pid.Set(cross_setpoint);\r
 \r
   }\r
 \r
@@ -357,6 +356,9 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
 \r
     //Enables TIM3 interrupt (used for PID control)\r
     HAL_TIM_Base_Start_IT(&htim3);\r
+\r
+    //running\r
+    otto_status = 3;\r
   }\r
 \r
   HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx,\r
@@ -364,7 +366,6 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
 }\r
 \r
 void HAL_UART_ErrorCallback(UART_HandleTypeDef *UartHandle) {\r
-  error++;\r
   HAL_UART_Receive_DMA(&huart6, (uint8_t*) &proto_buffer_rx,\r
   VelocityCommand_size);\r
 }\r
@@ -374,8 +375,8 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
   if (GPIO_Pin == user_button_Pin) {\r
     //TODO ci può servire il bottone blu?\r
   } else if (GPIO_Pin == fault1_Pin) {\r
-    left_motor.brake();\r
-    right_motor.brake();\r
+    left_motor.Brake();\r
+    right_motor.Brake();\r
     //stop TIM3 interrupt (used for PID control)\r
     HAL_TIM_Base_Stop_IT(&htim3);\r
 \r
@@ -401,8 +402,8 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
       ;\r
 \r
   } else if (GPIO_Pin == fault2_Pin) {\r
-    left_motor.brake();\r
-    right_motor.brake();\r
+    left_motor.Brake();\r
+    right_motor.Brake();\r
     //stop TIM3 interrupt (used for PID control)\r
     HAL_TIM_Base_Stop_IT(&htim3);\r
 \r