From 52fda2c57f2bf06713fee57a588a1316ea6d320f Mon Sep 17 00:00:00 2001
From: Atsushi Watanabe <atsushi.w@ieee.org>
Date: Sat, 6 Apr 2019 13:52:21 +0900
Subject: [PATCH] Fix torque_offset handling (#54)

---
 tfrog-motordriver/communication.c   |  6 +++---
 tfrog-motordriver/controlVelocity.c |  2 +-
 tfrog-motordriver/controlVelocity.h | 12 ++++++------
 tfrog-motordriver/eeprom.h          |  2 +-
 4 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/tfrog-motordriver/communication.c b/tfrog-motordriver/communication.c
index a0214d5..b7a75a4 100644
--- a/tfrog-motordriver/communication.c
+++ b/tfrog-motordriver/communication.c
@@ -1519,6 +1519,9 @@ int command_analyze(unsigned char* data, int len)
         motor[imotor].ref.vel_changed = 1;
       motor[imotor].ref.vel = i.integer;
       break;
+    case PARAM_p_toq_offset:
+      motor[imotor].ref.torque_offset = i.integer;
+      break;
     case PARAM_servo:
       if (motor[imotor].servo_level < SERVO_LEVEL_TORQUE &&
           i.integer >= SERVO_LEVEL_TORQUE)
@@ -1614,9 +1617,6 @@ int command_analyze(unsigned char* data, int len)
       case PARAM_toq_limit:
         motor_param[imotor].torque_limit = i.integer;
         break;
-      case PARAM_p_toq_offset:
-        motor_param[imotor].torque_offset = i.integer;
-        break;
       case PARAM_int_max:
         motor_param[imotor].integ_max = i.integer * 16;
         break;
diff --git a/tfrog-motordriver/controlVelocity.c b/tfrog-motordriver/controlVelocity.c
index 1d9e00b..12ae8df 100644
--- a/tfrog-motordriver/controlVelocity.c
+++ b/tfrog-motordriver/controlVelocity.c
@@ -198,7 +198,7 @@ void ISR_VelocityControl()
         toq[i] -= (motor_param[i].fr_wminus * (-motor[i].vel) / 16 + motor_param[i].fr_minus);
       }
       // トルク補償
-      toq[i] += motor_param[i].torque_offset;
+      toq[i] += motor[i].ref.torque_offset;
 
       // トルクでクリッピング
       if (toq[i] >= motor_param[i].torque_limit)
diff --git a/tfrog-motordriver/controlVelocity.h b/tfrog-motordriver/controlVelocity.h
index 8f52f48..6313400 100644
--- a/tfrog-motordriver/controlVelocity.h
+++ b/tfrog-motordriver/controlVelocity.h
@@ -58,11 +58,12 @@ typedef struct _MotorState
     int vel_buf;       // count/ms
     int vel_buf_prev;  // count/ms
     int vel_interval;
-    int vel_diff;  // count/ms
-    int torque;    // 1/100000 Nm
-    int rate;      // -PWM_max < rate < PWM_max
-    int rate2;     //
-    int rate_buf;  //
+    int vel_diff;       // count/ms
+    int torque;         // 1/100000 Nm
+    int torque_offset;  // 1/100000 Nm
+    int rate;           // -PWM_max < rate < PWM_max
+    int rate2;          //
+    int rate_buf;       //
     char vel_changed;
   } ref;
   int error;
@@ -92,7 +93,6 @@ typedef struct _MotorParam
   int torque_max;
   int torque_min;
   int torque_limit;
-  int torque_offset;
   int fr_plus;
   int fr_wplus;
   int fr_minus;
diff --git a/tfrog-motordriver/eeprom.h b/tfrog-motordriver/eeprom.h
index 64b121c..58657ee 100644
--- a/tfrog-motordriver/eeprom.h
+++ b/tfrog-motordriver/eeprom.h
@@ -48,7 +48,7 @@ typedef struct _Tfrog_EEPROM_data
 // Basically, don't change it destructively, but add new field at the end.
 #define TFROG_EEPROM_KEY 0x00AA7701
 // Increment if MotorParam, DriverParam struct is changed.
-#define TFROG_EEPROM_PARAM_VERSION 0x0001
+#define TFROG_EEPROM_PARAM_VERSION 0x0002
 
 #define TFROG_EEPROM_DEFAULT        \
   {                                 \