Fix initial absolute encoder value handling (#103)

openspur · Apr 4, 2019 · 2b7f724 · 2b7f724
1 parent 22515b6
commit 2b7f724
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 0 deletions.
diff --git a/include/odometry.h b/include/odometry.h
@@ -39,6 +39,7 @@ typedef struct _odometry
   double wvel[YP_PARAM_MAX_MOTOR_NUM];
   double wang[YP_PARAM_MAX_MOTOR_NUM];
   short enc[YP_PARAM_MAX_MOTOR_NUM];
+  int enc_init[YP_PARAM_MAX_MOTOR_NUM];
   double wtorque[YP_PARAM_MAX_MOTOR_NUM];
   double torque_trans;
   double torque_angular;

diff --git a/src/odometry.c b/src/odometry.c
@@ -85,6 +85,8 @@ void init_odometry(void)
   g_odometry.theta = 0;
   for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
   {
+    g_odometry.enc[i] = 0;
+    g_odometry.enc_init[i] = 0;
     g_odometry.wang[i] = 0;
     g_odometry.wtorque[i] = 0;
     g_odometry.wvel[i] = 0;
@@ -160,6 +162,14 @@ void odometry(OdometryPtr xp, short *cnt, short *pwm, double dt, double time)
     {
       cnt_diff = (short)cnt[i] - (short)xp->enc[i];
       xp->enc[i] = cnt[i];
+      if (!xp->enc_init[i])
+      {
+        cnt_diff = 0;
+        if (cnt[i] != 0)
+        {
+          xp->enc_init[i] = 1;
+        }
+      }
     }
     else
     {