From 2b7f724ffd1e3f1a4303104f2bcd7a86a5afa810 Mon Sep 17 00:00:00 2001
From: Atsushi Watanabe <atsushi.w@ieee.org>
Date: Fri, 5 Apr 2019 00:00:57 +0900
Subject: [PATCH] Fix initial absolute encoder value handling (#103)

---
 include/odometry.h |  1 +
 src/odometry.c     | 10 ++++++++++
 2 files changed, 11 insertions(+)

diff --git a/include/odometry.h b/include/odometry.h
index 6aa07a6..57cbcae 100644
--- 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
index adaf402..9e05c42 100644
--- 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
     {