Fix torque_offset handling (#54)

T-frog · Apr 6, 2019 · 52fda2c · 52fda2c
1 parent a4fc8d3
commit 52fda2c
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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        \
   {                                 \