upgraded `Encoder` to C

diff --git a/otto_controller/Core/Inc/control/encoder.h b/otto_controller/Core/Inc/control/encoder.h
index 933e5b590fbfc7ad9136509b95fdcaef38c99b06..296ae27205b02bc29c1b3e8335985295878a0555 100644 (file)
--- a/otto_controller/Core/Inc/control/encoder.h
+++ b/otto_controller/Core/Inc/control/encoder.h
@@ -3,54 +3,24 @@
 
 #include "main.h"
 
-typedef struct Encoder {
+typedef struct {
   TIM_HandleTypeDef *timer;
   uint32_t previous_millis;
   uint32_t current_millis;
   int32_t ticks;  //if negative the wheel is going backwards
-  float_t wheel_circumference;
   int32_t ticks_per_revolution;
-};
+  float wheel_circumference;
+} Encoder;
 
-void Setup() {
-  HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL);
-  this->ResetCount();
-  this->previous_millis_ = 0;
-  this->current_millis_ = HAL_GetTick();
-}
+void encoder_init(Encoder *encoders);
+void encoder_update(Encoder *encoder);
+float encoder_linear_velocity(Encoder *encoder);
 
-void ResetCount() {
-  //set counter to half its maximum value
-  __HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period / 2));
-}
+void encoder_count_reset(Encoder *encoder);
+int encoder_count_get(Encoder *encoder);
 
-int GetCount() {
-  int count = ((int) __HAL_TIM_GET_COUNTER(this->timer_)
-               - ((this->timer_->Init.Period) / 2));
-  return count;
-}
-
-void UpdateValues() {
-  this->previous_millis_ = this->current_millis_;
-  this->current_millis_ = HAL_GetTick();
-  this->ticks_ = this->GetCount();
-  this->ResetCount();
-}
-
-float GetMeters() {
-  float meters = ((float) this->ticks_ * this->wheel_circumference_)
-                 / ticks_per_revolution_;
-  return meters;
-}
-
-float GetLinearVelocity() {
-  this->UpdateValues();
-  float meters = this->GetMeters();
-  float deltaTime = this->current_millis_ - this->previous_millis_;
-  if (deltaTime == 0)
-    return 0;
-  float linear_velocity = (meters / (deltaTime / 1000));
-  return linear_velocity;
-}
+inline float meters_from_ticks(float encoder_ticks,
+                               float wheel_circumference,
+                               float ticks_per_revolution);
 
 #endif

diff --git a/otto_controller/Core/Src/main.cpp b/otto_controller/Core/Src/main.cpp
index 5fb089c61a4fef53fc01e349e24da86f49abb18c..b646ff67c542e259f5133a9ba7f43dbe44c29510 100644 (file)
--- a/otto_controller/Core/Src/main.cpp
+++ b/otto_controller/Core/Src/main.cpp
@@ -37,6 +37,7 @@
 \r
 // NOTE(lb): couldn't get it to link in the final executable\r
 #include "./control/motor_controller.c"\r
+#include "./control/encoder.c"\r
 \r
 /* USER CODE END Includes */\r
 \r
@@ -59,8 +60,10 @@
 /* USER CODE BEGIN PV */\r
 \r
 //Odometry\r
-Encoder encoder_left = {0};\r
-Encoder encoder_right = {0};\r
+static Encoder encoders[2] = {{0}, {0}};\r
+Encoder *encoder_right = &encoders[0];\r
+Encoder *encoder_left = &encoders[1];\r
+\r
 Odometry odom;\r
 \r
 //PID\r
@@ -69,7 +72,7 @@ Pid right_pid;
 Pid cross_pid;\r
 \r
 //MotorController\r
-static MotorController motors[] = {\r
+static MotorController motors[2] = {\r
   {\r
     // Right motor\r
     .sleep_gpio_port = sleep1_GPIO_Port,\r
@@ -165,19 +168,17 @@ int main(void) {
     }\r
   }\r
 \r
-  left_encoder.timer = &htim2;\r
-  left_encoder.wheel_circumference = config_msg.left_wheel_circumference;\r
-  left_encoder.ticks_per_revolution = config_msg.ticks_per_revolution;\r
+  encoder_left->timer = &htim2;\r
+  encoder_left->wheel_circumference = config_msg.left_wheel_circumference;\r
+  encoder_left->ticks_per_revolution = config_msg.ticks_per_revolution;\r
 \r
-  right_encoder.timer = &htim5;\r
-  right_encoder.wheel_circumference = config_msg.right_wheel_circumference;\r
-  right_encoder.ticks_per_revolution = config_msg.ticks_per_revolution;\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 = Odometry(config_msg.baseline);\r
 \r
-  left_encoder.Setup();\r
-  right_encoder.Setup();\r
-\r
+  encoder_init(encoders);\r
   motorcontroller_init(motors);\r
 \r
   //right and left motors have the same parameters\r
@@ -290,14 +291,16 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
   //TIMER 100Hz PID control\r
 \r
   //accumulate ticks for transmission\r
-  left_ticks += left_encoder.GetCount();\r
-  right_ticks += right_encoder.GetCount();\r
+  left_ticks += encoder_count_get(encoder_left);\r
+  right_ticks += encoder_count_get(encoder_right);\r
 \r
   //PID control\r
-  float left_velocity = left_encoder.GetLinearVelocity();\r
+  encoder_update(encoder_left);\r
+  float left_velocity = encoder_linear_velocity(encoder_left);\r
   int left_dutycycle = left_pid.Update(left_velocity);\r
 \r
-  float right_velocity = right_encoder.GetLinearVelocity();\r
+  encoder_update(encoder_right);\r
+  float right_velocity = encoder_linear_velocity(encoder_right);\r
   int right_dutycycle = right_pid.Update(right_velocity);\r
 \r
   float difference = left_velocity - right_velocity;\r
@@ -346,8 +349,8 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {
    * Manage new transmission\r
    */\r
 \r
-  int32_t left_ticks_tx = left_ticks + left_encoder.GetCount();\r
-  int32_t right_ticks_tx = right_ticks + right_encoder.GetCount();\r
+  int32_t left_ticks_tx = left_ticks + encoder_count_get(encoder_left);\r
+  int32_t right_ticks_tx = right_ticks + encoder_count_get(encoder_right);\r
 \r
   status_msg.left_ticks = left_ticks_tx;\r
   status_msg.right_ticks = right_ticks_tx;\r