Add slow short brake parameter (#103)

Add a parameter to gradually brake on error and init. Disabled by default and can be enabled by sending $SETSOFTBRAKEMS1000 $EEPROMSAVE
T-frog · Dec 20, 2023 · 2f98420 · 2f98420
1 parent 99d6b21
commit 2f98420
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 35 deletions.
diff --git a/tfrog-motordriver/communication.c b/tfrog-motordriver/communication.c
@@ -144,6 +144,10 @@ int32_t atoi(char* buf)
     }
     buf++;
   }
+  if (buf[0] == '-')
+  {
+    ret = -ret;
+  }
   return ret;
 }
 
@@ -1045,6 +1049,9 @@ int32_t extended_command_analyze(char* data)
     send("; \nINITIODATA:");
     nhex(val, saved_param.io_data, 2);
     send(val);
+    send("; \nSOFTBRAKEMS:");
+    itoa10(val, saved_param.soft_brake_ms);
+    send(val);
     send("; \n\n");
   }
   else if (strstr(data, "$LOCKPARAM") == data)
@@ -1240,6 +1247,21 @@ int32_t extended_command_analyze(char* data)
     send(data);
     send("\n00P\n\n");
   }
+  else if (strstr(data, "$SETSOFTBRAKEMS") == data)
+  {
+    const int32_t v = atoi(data + 15);
+    if (v < 0 || v > 10000)
+    {
+      send(data);
+      send("\n01Q\nOut of range\n\n");
+    }
+    else
+    {
+      saved_param.soft_brake_ms = v;
+      send(data);
+      send("\n00P\n\n");
+    }
+  }
   else if (strstr(data, "$SETBUZZERLEVEL") == data)
   {
     saved_param.buz_lvl = atoi(data + 15);

diff --git a/tfrog-motordriver/controlPWM.c b/tfrog-motordriver/controlPWM.c
@@ -59,6 +59,31 @@ int32_t PWM_cpms;
 
 void FIQ_PWMPeriod() RAMFUNC;
 
+void soft_brake(const int m)
+{
+  if (driver_state.brake_cnt_max == 0)
+  {
+    for (int8_t i = 0; i < 3; i++)
+    {
+      THEVA.MOTOR[m].PWM[i].H = PWM_resolution;
+      THEVA.MOTOR[m].PWM[i].L = PWM_resolution;
+    }
+    return;
+  }
+
+  const int32_t pwm = PWM_resolution * motor[m].brake_cnt / driver_state.brake_cnt_max;
+  for (int8_t i = 0; i < 3; i++)
+  {
+    THEVA.MOTOR[m].PWM[i].H = PWM_resolution;
+    THEVA.MOTOR[m].PWM[i].L = pwm;
+  }
+  motor[m].brake_cnt++;
+  if (motor[m].brake_cnt > driver_state.brake_cnt_max)
+  {
+    motor[m].brake_cnt = driver_state.brake_cnt_max;
+  }
+}
+
 inline int16_t sin_(int32_t x)
 {
   if (x < 1024)
@@ -87,25 +112,8 @@ void controlPWM_config(int32_t i)
 {
   THEVA.MOTOR[i].INVERT = 0;
 
-  switch (motor_param[i].motor_type)
-  {
-    case MOTOR_TYPE_DC:
-      THEVA.MOTOR[i].PWM[0].H = PWM_resolution;
-      THEVA.MOTOR[i].PWM[1].H = 0;
-      THEVA.MOTOR[i].PWM[2].H = PWM_resolution;
-      THEVA.MOTOR[i].PWM[0].L = PWM_resolution;
-      THEVA.MOTOR[i].PWM[1].L = 0;
-      THEVA.MOTOR[i].PWM[2].L = PWM_resolution;
-      break;
-    case MOTOR_TYPE_AC3:
-      THEVA.MOTOR[i].PWM[0].H = PWM_resolution;
-      THEVA.MOTOR[i].PWM[1].H = PWM_resolution;
-      THEVA.MOTOR[i].PWM[2].H = PWM_resolution;
-      THEVA.MOTOR[i].PWM[0].L = PWM_resolution;
-      THEVA.MOTOR[i].PWM[1].L = PWM_resolution;
-      THEVA.MOTOR[i].PWM[2].L = PWM_resolution;
-      break;
-  }
+  soft_brake(0);
+  soft_brake(1);
 
   motor[i].ref.rate = 0;
 
@@ -256,17 +264,16 @@ void FIQ_PWMPeriod()
   if (motor[0].error_state || motor[1].error_state)
   {
     // Short-mode brake
-    for (i = 0; i < 3 * 2; i++)
-    {
-      THEVA.MOTOR[i % 2].PWM[i / 2].H = PWM_resolution;
-      THEVA.MOTOR[i % 2].PWM[i / 2].L = PWM_resolution;
-    }
+    soft_brake(0);
+    soft_brake(1);
     init = 1;
     disabled = 1;
   }
   else if (init)
   {
     init = 0;
+    motor[0].brake_cnt = 0;
+    motor[1].brake_cnt = 0;
     return;
   }
 
@@ -380,11 +387,7 @@ void FIQ_PWMPeriod()
     {
       if (motor[j].servo_level == SERVO_LEVEL_STOP)
       {
-        for (i = 0; i < 3; i++)
-        {
-          THEVA.MOTOR[j].PWM[i].H = PWM_resolution;
-          THEVA.MOTOR[j].PWM[i].L = PWM_resolution;
-        }
+        soft_brake(j);
       }
       else if (_abs(motor[j].ref.torque) < driver_state.zero_torque ||
                motor[j].servo_level == SERVO_LEVEL_OPENFREE)
@@ -688,12 +691,12 @@ void controlPWM_init()
   THEVA.GENERAL.PWM.HALF_PERIOD = PWM_resolution;
   THEVA.GENERAL.PWM.DEADTIME = PWM_deadtime;
 
-  // Short-mode brake
-  for (j = 0; j < 3 * 2; j++)
-  {
-    THEVA.MOTOR[j % 2].PWM[j / 2].H = PWM_resolution;
-    THEVA.MOTOR[j % 2].PWM[j / 2].L = PWM_resolution;
-  }
+  driver_state.brake_cnt_max =
+      48000 * (int32_t)saved_param.soft_brake_ms /
+      (PWM_resolution * 2 * (PWM_thinning + 1));
+
+  soft_brake(0);
+  soft_brake(1);
 
   driver_state.PWM_resolution = PWM_resolution;
 

diff --git a/tfrog-motordriver/controlVelocity.h b/tfrog-motordriver/controlVelocity.h
@@ -93,6 +93,8 @@ typedef struct _MotorState
   YPSpur_servo_level servo_level;
   ErrorID error_state;
   FilterExp enc0_lpf;
+
+  int32_t brake_cnt;
 } MotorState;
 
 typedef struct _MotorParam
@@ -178,6 +180,7 @@ typedef struct _DriverState
   uint32_t ping_request;
   uint32_t odom_drop;
   int32_t vsrc_max;
+  int32_t brake_cnt_max;
 } DriverState;
 
 #ifdef static_assert

diff --git a/tfrog-motordriver/eeprom.h b/tfrog-motordriver/eeprom.h
@@ -41,6 +41,7 @@ typedef struct _Tfrog_EEPROM_data
   uint8_t rely_hall;
   uint8_t io_dir;
   uint8_t io_data;
+  uint16_t soft_brake_ms;
   char __endbyte;  // must be at the end of the struct to detect actual struct size
 } Tfrog_EEPROM_data;
 
@@ -70,6 +71,7 @@ typedef struct _Tfrog_EEPROM_data
         0,                          \
         0,                          \
         0,                          \
+        0,                          \
   }
 
 #define TFROG_EEPROM_DATA_TEXT       0