Add minimal voltage param (#94)

T-frog · Oct 27, 2020 · fc70c87 · fc70c87
1 parent 4fefc94
commit fc70c87
Show file tree

Hide file tree

Showing 5 changed files with 9 additions and 6 deletions.
diff --git a/tfrog-motordriver/communication.c b/tfrog-motordriver/communication.c
@@ -1716,6 +1716,9 @@ int32_t command_analyze(uint8_t* data, int32_t len)
         // ad = 1024 * ( vsrc * VSRC_DIV ) / 3.3
         driver_param.vsrc_rated = 310 * ((int32_t)i.integer * VSRC_DIV) / 256;
         break;
+      case PARAM_vmin:
+        driver_param.vmin = 310 * ((int32_t)i.integer * VSRC_DIV) / 256;
+        break;
       default:
         return 0;
     }

diff --git a/tfrog-motordriver/communication.h b/tfrog-motordriver/communication.h
@@ -85,6 +85,7 @@ typedef enum
   PARAM_enc_denominator,
   PARAM_hall_delay_factor,
   PARAM_lr_cutoff_vel,
+  PARAM_vmin,
   PARAM_servo = 64,
   PARAM_watch_dog_limit,
   PARAM_io_dir = 96,

diff --git a/tfrog-motordriver/controlVelocity.c b/tfrog-motordriver/controlVelocity.c
@@ -410,6 +410,7 @@ void controlVelocity_init()
   Filter1st_CreateLPF(&accelf0, ACCEL_FILTER_TIME);
   accelf[0] = accelf[1] = accelf0;
 
+  driver_param.vmin = 0;  // no soft limit by default
   driver_param.watchdog_limit = 600;
   driver_param.control_cycle = 1;
   driver_state.protocol_version = 0;

diff --git a/tfrog-motordriver/controlVelocity.h b/tfrog-motordriver/controlVelocity.h
@@ -143,6 +143,7 @@ typedef struct _DriverParam
   int32_t control_s;
   int32_t vsrc_rated;
   uint16_t watchdog_limit;
+  int32_t vmin;
 } DriverParam;
 
 typedef struct _DriverState

diff --git a/tfrog-motordriver/main.c b/tfrog-motordriver/main.c
@@ -1008,19 +1008,15 @@ int main()
       driver_state.vsrc = Filter1st_Filter(&voltf, (int32_t)(analog[7] & 0x0FFF));
       ADC_Start();
 
-      if (driver_state.vsrc < driver_param.vsrc_rated / 4)
-      {
-        driver_state.vsrc_factor = 0;
-      }
-      else if (driver_param.vsrc_rated >= 0x03FF)
+      if (driver_param.vsrc_rated >= 0x03FF)
       {
         driver_state.vsrc_factor = 32768;
       }
       else
       {
         driver_state.vsrc_factor = driver_param.vsrc_rated * 32768 / driver_state.vsrc;
       }
-      if (driver_state.vsrc > 310 * 8 * VSRC_DIV)
+      if (driver_state.vsrc > 310 * 8 * VSRC_DIV && driver_state.vsrc > driver_param.vmin)
       {
         if (driver_state.error.low_voltage < 100)
         {
@@ -1034,6 +1030,7 @@ int main()
       }
       else
       {
+        driver_state.vsrc_factor = 0;
         driver_state.error.low_voltage = 0;
         motor[0].error_state |= ERROR_LOW_VOLTAGE;
         motor[1].error_state |= ERROR_LOW_VOLTAGE;