Added calibration sequence for encoder diameter diff

firmware/base/src/config.h

@@ -20,6 +20,6 @@
 #define ENCODER_CPR         3840
 
 #define WHEEL_RADIUS     0.0279
-#define WHEEL_SEPARATION 0.31133
+#define WHEEL_SEPARATION 0.310735
 #define TIMER_CYCLE_US   1000
 //======================================================

firmware/base/src/main.c

@@ -43,6 +43,18 @@ typedef struct instr_buf_t{
 
 instr_buf_t stepper_buf = {};
 
+typedef struct calib_diff_t {
+    long long left_pulse;
+    long long right_pulse;
+    double left_gain;
+    double right_gain;
+} calib_diff_t;
+
+static calib_diff_t calib_enc = {
+    .left_gain = 0.997648,
+    .right_gain = 1.002333
+};
+
 bool update_pos_cb() {
 
     substep_update(&state_l);
@@ -51,16 +63,22 @@ bool update_pos_cb() {
     int position_l= state_l.position;
     int position_r= state_r.position;
 
-    double vel_l = position_l - prev_position_l;
-    double vel_r = position_r - prev_position_r;
+    const int diff_l = position_l - prev_position_l;
+    const int diff_r = position_r - prev_position_r;
+
+    calib_enc.left_pulse += diff_l;
+    calib_enc.right_pulse -= diff_r;
+
+    double vel_l = diff_l;
+    double vel_r = diff_r;
 
     prev_position_l = state_l.position;
     prev_position_r = state_r.position;
 
     vel_l /=64 * ENCODER_CPR;
     vel_r /=64 * ENCODER_CPR;
-    vel_l *=  WHEEL_RADIUS * 2 * M_PI;
-    vel_r *= -WHEEL_RADIUS * 2 * M_PI;
+    vel_l *=  WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
+    vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
 
     const double linear = (vel_l + vel_r) / 2;
     const double angular = (vel_r - vel_l) / wheel_separation;
@@ -85,6 +103,23 @@ void zero() {
     base_x = 0;
     base_y = 0; 
     base_theta = 0;  
+    calib_enc.left_pulse = 0;
+    calib_enc.right_pulse = 0;
+}
+
+void start_calib() {
+    calib_enc.left_pulse = 0;
+    calib_enc.right_pulse = 0;
+}
+
+void end_calib() {
+    const double l_adjust = (double)calib_enc.left_pulse * calib_enc.left_gain;
+    const double r_adjust = (double)calib_enc.right_pulse * calib_enc.right_gain;
+    const double delta_angle = l_adjust - r_adjust;
+    const double distance = 0.5 * (l_adjust + r_adjust);
+    const double factor = delta_angle / distance * 0.7;
+    calib_enc.left_gain = (1.0 - factor) * calib_enc.left_gain;
+    calib_enc.right_gain = (1.0 + factor) * calib_enc.right_gain;
 }
 
 
@@ -140,6 +175,9 @@ void core2_entry()
                 type = (cmd >> 8) & 0xFF;
             } else if(cmd == (((uint16_t)'z' << 8) | ';')) {
                 zero();
+            } else if(cmd == (((uint16_t)'c' << 8) | ';')) {
+                end_calib();
+                printf("%lf %lf\n", calib_enc.left_gain, calib_enc.right_gain);
             }
         }
 

mg_bt/behavior_trees/calib_bt.xml

@@ -1,8 +1,41 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="calib2_bt">
+    <Sequence>
+      <ZeroNode service_name="/zero"/>
+      <MovePoint action_name="/MovePoint"
+                 tolerance="0.02"
+                 max_angular="0.1"
+                 x_goal="0.7"
+                 y_goal="0.0"/>
+      <RotateNode angle="-2"
+                  action_name="/Rotate"/>
+      <RotateNode angle="-3.14"
+                  action_name="/Rotate"/>
+      <MovePoint action_name="/MovePoint"
+                 tolerance="0.02"
+                 x_goal="0.3"
+                 y_goal="0.0"/>
+      <RotateNode angle="-2"
+                  action_name="/Rotate"/>
+      <RotateNode angle="0"
+                  action_name="/Rotate"/>
+            <Fallback>
+              <Timeout msec="5000">
+                <MovePoint action_name="/MovePoint"
+                           max_angular="0.1"
+                           x_goal="-0.1"
+                           y_goal="0.0"/>
+              </Timeout>
+              <AlwaysSuccess/>
+            </Fallback>
+      <ZeroNode service_name="/endCalib"/>
+    </Sequence>
+  </BehaviorTree>
+
   <BehaviorTree ID="calib_bt">
     <Sequence>
-      <SetBlackboard value="0.31133"
+      <SetBlackboard value="0.311010"
                      output_key="width"/>
       <Delay delay_msec="10000">
         <Repeat num_cycles="5">

mg_odometry/src/mg_odom_publisher.cpp

@@ -47,6 +47,9 @@ class MgOdomPublisher : public rclcpp::Node {
 
         zero_service_
           = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
+
+        calib_service_
+          = create_service<ZeroSrv>("/endCalib", std::bind(&MgOdomPublisher::calib, this, std::placeholders::_1));
     }
 
   private:
@@ -54,6 +57,7 @@ class MgOdomPublisher : public rclcpp::Node {
     rclcpp::Service<SendDoubleSrv>::SharedPtr      set_width_service_;
     rclcpp::Service<SendDoubleSrv>::SharedPtr      set_ratio_service_;
     rclcpp::Service<ZeroSrv>::SharedPtr            zero_service_;
+    rclcpp::Service<ZeroSrv>::SharedPtr            calib_service_;
     rclcpp::TimerBase::SharedPtr                   timer_;
     LibSerial::SerialStream                        enc_serial_port_;
 
@@ -87,6 +91,18 @@ class MgOdomPublisher : public rclcpp::Node {
         }
     }
 
+    void calib(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
+        RCLCPP_INFO(get_logger(), "Finishing wheel diameter calib");
+
+        if (enc_serial_port_.IsOpen()) {
+            enc_serial_port_ << "c;";
+            double left_gain  = 0;
+            double right_gain = 0;
+            enc_serial_port_ >> left_gain >> right_gain;
+            RCLCPP_INFO(this->get_logger(), "The new gains are %lf %lf", left_gain, right_gain);
+        }
+    }
+
     void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
         RCLCPP_INFO(get_logger(), "Zeroing odometry");