diff --git a/tfrog-motordriver/communication.c b/tfrog-motordriver/communication.c index cfe5e59..93ff2ff 100644 --- 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 index d7459db..3dc6899 100644 --- 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 index 6c3340d..761b12c 100644 --- 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 = 0x7FFFFFFF; // 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 index ac08498..959f16c 100644 --- 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 index 44ea732..cb58b5b 100644 --- a/tfrog-motordriver/main.c +++ b/tfrog-motordriver/main.c @@ -1008,11 +1008,7 @@ 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; } @@ -1020,7 +1016,7 @@ int main() { 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;