Ignore single-shot hall signal error (#59)

T-frog · May 7, 2019 · ac0d851 · ac0d851
1 parent 2c9d607
commit ac0d851
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 3 deletions.
diff --git a/tfrog-motordriver/controlPWM.c b/tfrog-motordriver/controlPWM.c
@@ -394,8 +394,11 @@ void FIQ_PWMPeriod()
         //if( halldiff == 3 || halldiff >= 5 ) printf( "ENC error: %x->%x\n\r", _hall[i], hall[i] );
         // ホール素子信号が全相1、全相0のとき
         // ホース素子信号が2ビット以上変化したときはエラー
-        if (driver_state.error.hall[i] < 128)
+
+        // Skip next one error to avoid counting another edge of this error.
+        if (driver_state.error.hall[i] < 12)
           driver_state.error.hall[i] += 12;
+
         if (driver_state.error.hall[i] > 12)
         {
           // エラー検出後、1周以内に再度エラーがあれば停止
@@ -538,8 +541,23 @@ void FIQ_PWMPeriod()
         // In worst case, initial encoder origin can have offset of motor_param[i].enc_rev/12.
         if (_abs(err) > motor_param[i].enc_rev / 6)
         {
-          motor[i].error_state |= ERROR_HALL_ENC;
-          printf("PWM:enc-hall err (%d)\n\r", err);
+          // Skip next one error to avoid counting another edge of this error.
+          if (driver_state.error.hallenc[i] < 12)
+            driver_state.error.hallenc[i] += 12;
+
+          if (driver_state.error.hallenc[i] > 12)
+          {
+            // Enter error stop mode if another error occurs within one revolution
+            motor[i].error_state |= ERROR_HALL_ENC;
+            printf("PWM:enc-hall err (%d)\n\r", err);
+          }
+          // Don't apply erroneous absolute angle
+          continue;
+        }
+        else
+        {
+          if (driver_state.error.hallenc[i] > 0)
+            driver_state.error.hallenc[i]--;
         }
       }
 

diff --git a/tfrog-motordriver/controlVelocity.h b/tfrog-motordriver/controlVelocity.h
@@ -139,6 +139,7 @@ typedef struct _DriverState
   {
     unsigned char low_voltage;
     unsigned char hall[2];
+    unsigned char hallenc[2];
   } error;
   enum
   {

diff --git a/tfrog-motordriver/main.c b/tfrog-motordriver/main.c
@@ -593,6 +593,8 @@ int main()
   driver_state.error.low_voltage = 0;
   driver_state.error.hall[0] = 0;
   driver_state.error.hall[1] = 0;
+  driver_state.error.hallenc[0] = 0;
+  driver_state.error.hallenc[1] = 0;
 
   printf("Velocity Control init\n\r");
   // Configure velocity control loop
@@ -704,6 +706,8 @@ int main()
   driver_state.error.low_voltage = 0;
   driver_state.error.hall[0] = 0;
   driver_state.error.hall[1] = 0;
+  driver_state.error.hallenc[0] = 0;
+  driver_state.error.hallenc[1] = 0;
   driver_state.ifmode = 0;
   driver_state.watchdog = 0;
   // Driver loop
@@ -761,6 +765,8 @@ int main()
       }
       driver_state.error.hall[0] = 0;
       driver_state.error.hall[1] = 0;
+      driver_state.error.hallenc[0] = 0;
+      driver_state.error.hallenc[1] = 0;
       motor[0].error_state |= ERROR_WATCHDOG;
       motor[1].error_state |= ERROR_WATCHDOG;
       printf("Watchdog - init parameters\n\r");