upgraded `Odometry` to C

diff --git a/otto_controller/Core/Inc/control/odometry.h b/otto_controller/Core/Inc/control/odometry.h
index 417f062d02b1043fa090421368b25a4e2a4d06ec..4669af80df1db24b68e7fb31f33d760e13f7919e 100644 (file)
--- a/otto_controller/Core/Inc/control/odometry.h
+++ b/otto_controller/Core/Inc/control/odometry.h
@@ -1,51 +1,39 @@
 #ifndef ODOMETRY_H
 #define ODOMETRY_H
 
+typedef struct {
+  union {
+    struct {
+      float right;
+      float left;
+    } setpoint;
+    float setpoints[2];
+    struct {
+      float right;
+      float left;
+    } velocity;
+    float velocites[2];
+  };
+
+  float linear_velocity;
+  float angular_velocity;
+  float baseline;
+} 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;
+}
+
+#if 0
 class Odometry {
- private:
-
-  float left_setpoint_;
-  float right_setpoint_;
-  float linear_velocity_;
-  float angular_velocity_;
-  float baseline_;
-
- public:
-  Odometry() {
-    left_setpoint_ = 0;
-    right_setpoint_ = 0;
-    baseline_ = 0;
-  }
-
-  Odometry(float baseline) {
-    left_setpoint_ = 0;
-    right_setpoint_ = 0;
-    baseline_ = baseline;
-  }
-
-  void FromCmdVelToSetpoint(float linear_vel, float angular_vel) {
-    left_setpoint_ = linear_vel - (baseline_ * angular_vel) / 2;
-    right_setpoint_ = linear_vel + (baseline_ * angular_vel) / 2;
-  }
-
+  // NOTE(lb): can i delete this?
   void FromWheelVelToOdom(float left_wheel_vel, float right_wheel_vel){
     linear_velocity_ = (left_wheel_vel + right_wheel_vel)/2;
     angular_velocity_ = (right_wheel_vel - left_wheel_vel)/baseline_;
   }
-
-  float GetLeftVelocity() {
-    return left_setpoint_;
-  }
-  float GetRightVelocity() {
-    return right_setpoint_;
-  }
-  float GetLinearVelocity(){
-    return linear_velocity_;
-  }
-  float GetAngularVelocity(){
-    return angular_velocity_;
-  }
-
 };
+#endif
 
 #endif

diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp
index b646ff67c542e259f5133a9ba7f43dbe44c29510..6dab9012821ea2f3da01313ed94b6063d2682c11 100644 (file)
--- a/otto_controller/Core/Src/main.cpp
+++ b/otto_controller/Core/Src/main.cpp
@@ -64,7 +64,7 @@ static Encoder encoders[2] = {{0}, {0}};
 Encoder *encoder_right = &encoders[0];\r
 Encoder *encoder_left = &encoders[1];\r
 \r
-Odometry odom;\r
+Odometry odom = {0};\r
 \r
 //PID\r
 Pid left_pid;\r
@@ -176,7 +176,7 @@ int main(void) {
   encoder_right->wheel_circumference = config_msg.right_wheel_circumference;\r
   encoder_right->ticks_per_revolution = config_msg.ticks_per_revolution;\r
 \r
-  odom = Odometry(config_msg.baseline);\r
+  odom.baseline = config_msg.baseline;\r
 \r
   encoder_init(encoders);\r
   motorcontroller_init(motors);\r
@@ -313,7 +313,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
   motorcontroller_speed_set(motor_right, right_dutycycle);\r
 }\r
 \r
-uint8_t porcoddio = 0;\r
+uint8_t porcoddio = 0; // NOTE(lb): LOL\r
 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {\r
   /*\r
    * Manage received message\r
@@ -334,10 +334,9 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
     otto_status = 3;\r
   }\r
 \r
-  odom.FromCmdVelToSetpoint(linear_velocity, angular_velocity);\r
-\r
-  float left_setpoint = odom.GetLeftVelocity();\r
-  float right_setpoint = odom.GetRightVelocity();\r
+  odometry_setpoint_from_cmdvel(&odom, linear_velocity, angular_velocity);\r
+  float left_setpoint = odom.velocity.left;\r
+  float right_setpoint = odom.velocity.right;\r
 \r
   left_pid.Set(left_setpoint);\r
   right_pid.Set(right_setpoint);\r