Add LPF to encoder origin

tfrog-motordriver/communication.c

@@ -1652,6 +1652,12 @@ int command_analyze(unsigned char* data, int len)
       case PARAM_enc_denominator:
         motor_param[imotor].enc_denominator = i.integer;
         break;
+      case PARAM_hall_delay_factor:
+        motor_param[imotor].hall_delay_factor = i.integer;
+        break;
+      case PARAM_lr_cutoff_vel:
+        motor_param[imotor].lr_cutoff_vel = i.integer;
+        break;
       case PARAM_enc_type:
         motor_param[imotor].enc_type = i.integer;
         break;

tfrog-motordriver/communication.h

@@ -82,6 +82,8 @@ typedef enum
   PARAM_control_cycle,
   PARAM_enc_div,
   PARAM_enc_denominator,
+  PARAM_hall_delay_factor,
+  PARAM_lr_cutoff_vel,
   PARAM_servo = 64,
   PARAM_watch_dog_limit,
   PARAM_io_dir = 96,

tfrog-motordriver/controlPWM.c

@@ -42,8 +42,12 @@ static const Pin pinPWMCycle2 = PIN_PWM_CYCLE2;
 // / PWM Enable pin instance.
 static const Pin pinPWMEnable = PIN_PWM_ENABLE;
 
+#define SinTB_2PI 4096
+#define AtanTB_LEN 512
+
 static char init = 1;
 short SinTB[1024];
+short AtanTB[AtanTB_LEN];
 int PWM_abs_max = 0;
 int PWM_abs_min = 0;
 int PWM_center = 0;
@@ -54,7 +58,6 @@ int PWM_cpms;
 
 void FIQ_PWMPeriod() RAMFUNC;
 
-#define SinTB_2PI 4096
 inline short sin_(int x)
 {
   if (x < 1024)
@@ -66,6 +69,17 @@ inline short sin_(int x)
   return -SinTB[4096 - x];
 }
 
+inline short atan_(const int x)
+{
+  if (x <= -(AtanTB_LEN - 1))
+    return -AtanTB[AtanTB_LEN - 1];
+  else if (x < 0)
+    return -AtanTB[-x];
+  else if (x < AtanTB_LEN - 1)
+    return AtanTB[x];
+  return AtanTB[AtanTB_LEN - 1];
+}
+
 extern Tfrog_EEPROM_data saved_param;
 extern volatile char rs485_timeout;
 
@@ -111,10 +125,11 @@ void controlPWM_config(int i)
   motor_param[i].enc_drev[4] = motor_param[i].enc_rev * 5 / 6;
   motor_param[i].enc_drev[5] = motor_param[i].enc_rev * 6 / 6;
 
-  motor_param[i].enc_10hz = motor_param[i].enc_rev * 10 * 16 / 1000;
-  motor_param[i].enc_rev_1p = motor_param[i].enc_rev / 300;
-  if (motor_param[i].enc_rev_1p == 0)
-    motor_param[i].enc_rev_1p = 1;
+  // Interrupt interval should be less than 1%(= 3.6deg of phase error) of
+  // hall signal edge interval.
+  motor_param[i].vel_rely_hall =
+      motor_param[i].enc_rev * 48000 /
+      (2 * PWM_resolution * driver_param.control_cycle * 100);
 
   motor_param[i].enc_mul =
       (int)((int64_t)SinTB_2PI * 0x40000 * motor_param[i].enc_denominator /
@@ -136,6 +151,12 @@ void controlPWM_config(int i)
 
   motor_param[i].enc0 = 0;
   motor_param[i].enc0tran = 0;
+  Filter1st_CreateLPF(&motor[i].enc0_lpf, ENC0_FILTER_TIME * 1200 / PWM_resolution);
+
+  if (motor_param[i].lr_cutoff_vel == 0)
+    motor_param[i].lr_cutoff_vel_inv = 0;
+  else
+    motor_param[i].lr_cutoff_vel_inv = 32768 * AtanTB_LEN / (motor_param[i].lr_cutoff_vel * 16);
 
   if (motor_param[i].motor_type != MOTOR_TYPE_DC &&
       motor_param[i].enc_type != 0)
@@ -312,10 +333,15 @@ void FIQ_PWMPeriod()
         }
         case MOTOR_TYPE_AC3:
         {
+          // Estimate LR circuit delay.
+          // Use reference velocity to avoid unstability due to disturbance.
+          const int tan = motor[j].ref.vel * motor_param[j].lr_cutoff_vel_inv / 32768;
+          const int delay = atan_(tan);
+
           phase[2] = motor[j].pos - motor_param[j].enc0tran;
           phase[2] = (int64_t)(phase[2] + motor_param[j].phase_offset) *
                          motor_param[j].enc_mul / 0x40000 +
-                     SinTB_2PI + SinTB_2PI / 4;
+                     SinTB_2PI + SinTB_2PI / 4 + delay;
           phase[1] = phase[2] - SinTB_2PI / 3;
           phase[0] = phase[2] - SinTB_2PI * 2 / 3;
 
@@ -372,6 +398,29 @@ void FIQ_PWMPeriod()
         }
       }
     }
+
+    // LPF encoder origin
+    for (j = 0; j < 2; j++)
+    {
+      if (motor[j].servo_level == SERVO_LEVEL_STOP ||
+          motor[j].servo_level == SERVO_LEVEL_OPENFREE)
+      {
+        continue;
+      }
+      int diff = motor_param[j].enc0 - motor_param[j].enc0tran;
+      if (_abs(diff) > motor_param[j].enc_rev_raw)
+      {
+        motor_param[j].enc0tran = motor_param[j].enc0;
+      }
+      else
+      {
+        normalize(&diff, -motor_param[j].enc_rev_h, motor_param[j].enc_rev);
+
+        const int diff_filtered = Filter1st_Filter(&motor[j].enc0_lpf, diff * 256);
+        motor_param[j].enc0tran += diff_filtered / 256;
+        normalize(&motor_param[j].enc0tran, 0, motor_param[j].enc_rev_raw);
+      }
+    }
   }
 
   // ゼロ点計算
@@ -594,10 +643,13 @@ void FIQ_PWMPeriod()
       }
 
       // ホール素子は高速域では信頼できない
-      if (_abs(motor[i].vel) > motor_param[i].enc_10hz &&
+      if (_abs(motor[i].vel1) > motor_param[i].vel_rely_hall &&
           !saved_param.rely_hall)
         continue;
 
+      // Compensate hall signal delay
+      enc0 -= motor[i].vel1 * motor_param[i].hall_delay_factor / 32768;
+
       // Fill enc0_buf and calculate average
       if (hall_pos >= ENC0_BUF_MAX)
         hall_pos = hall_pos % ENC0_BUF_MAX;
@@ -649,6 +701,11 @@ void controlPWM_init()
 
     SinTB[j] = ival;
   }
+  for (j = 0; j < AtanTB_LEN; j++)
+  {
+    const fixp4 a = fp4atan(FP4_ONE * j / AtanTB_LEN);
+    AtanTB[j] = a * SinTB_2PI / FP4_PI2;
+  }
 
   PIO_Configure(&pinPWMCycle2, 1);
   AIC_ConfigureIT(AT91C_ID_IRQ0, 6 | AT91C_AIC_SRCTYPE_POSITIVE_EDGE, (void (*)(void))FIQ_PWMPeriod);

tfrog-motordriver/controlVelocity.c

@@ -354,26 +354,6 @@ void timer0_vel_calc()
     }
   }
 
-  // encoder absolute angle LPF
-  for (i = 0; i < 2; i++)
-  {
-    if (motor[i].servo_level == SERVO_LEVEL_STOP)
-      continue;
-
-    int enc0 = motor_param[i].enc0;
-    int diff;
-    diff = motor_param[i].enc0tran - enc0;
-    normalize(&diff, -motor_param[i].enc_rev_h, motor_param[i].enc_rev);
-
-    if (_abs(diff) <= motor_param[i].enc_rev_1p)
-      motor_param[i].enc0tran = enc0;
-    else if (diff > 0)
-      motor_param[i].enc0tran -= motor_param[i].enc_rev_1p;
-    else
-      motor_param[i].enc0tran += motor_param[i].enc_rev_1p;
-
-    normalize(&motor_param[i].enc0tran, 0, motor_param[i].enc_rev_raw);
-  }
   ISR_VelocityControl();
   LED_off(1);
 
@@ -385,7 +365,6 @@ void timer0_vel_calc()
 // ------------------------------------------------------------------------------
 void controlVelocity_init()
 {
-#define ACCEL_FILTER_TIME 15.0
   int i;
 
   Filter1st_CreateLPF(&accelf0, ACCEL_FILTER_TIME);
@@ -418,11 +397,13 @@ void controlVelocity_init()
     motor_param[i].enc_denominator = 1;
     motor_param[i].phase_offset = 0;
     motor_param[i].enc_type = 2;
+    motor_param[i].hall_delay_factor = 0;
+    motor_param[i].lr_cutoff_vel = 0;
   }
 }
 void controlVelocity_config()
 {
-  Filter1st_CreateLPF(&accelf0, 15 / driver_param.control_cycle);
+  Filter1st_CreateLPF(&accelf0, ACCEL_FILTER_TIME / driver_param.control_cycle);
   accelf[0] = accelf[1] = accelf0;
 
   {

tfrog-motordriver/controlVelocity.h

@@ -21,7 +21,9 @@
 #include <board.h>
 #include <stdint.h>
 #include <assert.h>
+
 #include "communication.h"
+#include "filter.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -43,6 +45,9 @@ typedef enum _ErrorID
 #define ENC0_BUF_MAX (ENC0_BUF_MAX_DENOMINATOR * 6)
 #define ENC0_BUF_UNKNOWN 0x7FFFFFF
 
+#define ACCEL_FILTER_TIME 15  // Velocity control steps
+#define ENC0_FILTER_TIME 16   // PWM interrupt steps
+
 typedef struct _MotorState
 {
   int vel;                // count/ms
@@ -77,16 +82,17 @@ typedef struct _MotorState
   char control_init;
   YPSpur_servo_level servo_level;
   ErrorID error_state;
+  Filter1st enc0_lpf;
 } MotorState;
 
 typedef struct _MotorParam
 {
   int enc_rev;  // count/rev
   int phase_offset;
-  int enc_rev_h;  // count/rev
-  int enc_rev_1p;
+  int enc_rev_h;              // count/rev
+  int deprecated_enc_rev_1p;  // [DEPRECATED]
   unsigned char enc_type;
-  int enc_10hz;
+  int vel_rely_hall;
   int enc_drev[6];
   int enc_mul;
   int enc0;      // count
@@ -116,6 +122,9 @@ typedef struct _MotorParam
   short enc_denominator;
   int enc_rev_raw;  // count/rev
   char rotation_dir;
+  short hall_delay_factor;  // velocity * hall_delay_factor / 32768 = delay count
+  int lr_cutoff_vel;
+  int lr_cutoff_vel_inv;
 } MotorParam;
 
 typedef struct _DriverParam