diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 20f207a87..d677198e5 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -387,8 +387,14 @@ void update_brake_current() { static void update_analog_endpoint(const struct PWMMapping_t *map, int gpio) { - float fraction = get_adc_voltage(get_gpio(gpio)) / 3.3f; - float value = map->min + (fraction * (map->max - map->min)); + float fraction = get_adc_relative_voltage(get_gpio(gpio)); + float value = apply_deadband(fraction, + 0.0, map->min, + 1.0, map->max, + map->deadband_enable, + map->deadband_start, map->deadband_level, + map->deadband_end, map->deadband_level + ); fibre::set_endpoint_from_float(map->endpoint, value); } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 7c76f5d56..83f088df6 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -329,7 +329,7 @@ bool Motor::setup() { // Clip all current control to actual usable range max_allowed_current_ = max_unity_gain_current * phase_current_rev_gain_; - max_dc_calib_ = 0.1f * max_allowed_current_; + max_dc_calib_ = config_.max_dc_calib_koef * max_allowed_current_; if (!gate_driver_.init()) return false; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 7091e2538..a4587ce41 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -48,6 +48,7 @@ class Motor : public ODriveIntf::MotorIntf { float I_leak_max = 0.1f; float dc_calib_tau = 0.2f; + float max_dc_calib_koef = 0.1f; // custom property setters Motor* parent = nullptr; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4ddba244b..0243d8693 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -57,6 +57,10 @@ struct PWMMapping_t { endpoint_ref_t endpoint = {0, 0}; float min = 0; float max = 0; + bool deadband_enable = false; + float deadband_start = 0; + float deadband_end = 1; + float deadband_level = 0; }; // @brief general user configurable board configuration diff --git a/Firmware/MotorControl/pwm_input.cpp b/Firmware/MotorControl/pwm_input.cpp index 176b7eb23..0a8cb6934 100644 --- a/Firmware/MotorControl/pwm_input.cpp +++ b/Firmware/MotorControl/pwm_input.cpp @@ -1,4 +1,6 @@ +#include + #include "pwm_input.hpp" #include "odrive_main.h" @@ -39,8 +41,13 @@ void handle_pulse(int channel, uint32_t high_time) { if (high_time > PWM_MAX_HIGH_TIME) high_time = PWM_MAX_HIGH_TIME; float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME); - float value = odrv.config_.pwm_mappings[channel].min + - (fraction * (odrv.config_.pwm_mappings[channel].max - odrv.config_.pwm_mappings[channel].min)); + float value = apply_deadband(fraction, + 0.0, odrv.config_.pwm_mappings[channel].min, + 1.0, odrv.config_.pwm_mappings[channel].max, + odrv.config_.pwm_mappings[channel].deadband_enable, + odrv.config_.pwm_mappings[channel].deadband_start, odrv.config_.pwm_mappings[channel].deadband_level, + odrv.config_.pwm_mappings[channel].deadband_end, odrv.config_.pwm_mappings[channel].deadband_level + ); fibre::set_endpoint_from_float(odrv.config_.pwm_mappings[channel].endpoint, value); } diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp index f41d9813b..ae3b6659a 100644 --- a/Firmware/MotorControl/utils.cpp +++ b/Firmware/MotorControl/utils.cpp @@ -188,3 +188,49 @@ void delay_us(uint32_t us) } } +float apply_deadband(float fraction, + float fraction_min, float level_min, + float fraction_max, float level_max, + bool deadband_enable, + float deadband_start, float deadband_start_level, + float deadband_end, float deadband_end_level) { + float x1, y1, x2, y2; + float x = fraction; + + if (fraction <= fraction_min) + return level_min; + else if (fraction >= fraction_max) + return level_max; + else if (!(deadband_enable && + fraction_min <= deadband_start && + deadband_start <= deadband_end && + deadband_end <= fraction_max)) { + x1 = fraction_min; + x2 = fraction_max; + y1 = level_min; + y2 = level_max; + } + else if (x < deadband_start) { + x1 = fraction_min; + x2 = deadband_start; + y1 = level_min; + y2 = deadband_start_level; + } + else if (x <= deadband_end) { + x1 = deadband_start; + x2 = deadband_end; + y1 = deadband_start_level; + y2 = deadband_end_level; + } + else { + x1 = deadband_end; + x2 = fraction_max; + y1 = deadband_end_level; + y2 = level_max; + } + + if (x1 >= x2) + return (y1 + y2) * 0.5f; + + return y1 + (x - x1) / (x2 - x1) * (y2 - y1); +} diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp index 67c1fceac..3b088efad 100644 --- a/Firmware/MotorControl/utils.hpp +++ b/Firmware/MotorControl/utils.hpp @@ -71,6 +71,12 @@ uint32_t timeout_to_deadline(uint32_t timeout_ms); int is_in_the_future(uint32_t time_ms); uint32_t micros(void); void delay_us(uint32_t us); +float apply_deadband(float fraction, + float fraction_min, float level_min, + float fraction_max, float level_max, + bool deadband_enable, + float deadband_start, float deadband_start_level, + float deadband_end, float deadband_end_level); extern "C" { float our_arm_sin_f32(float x); diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 34cb09d06..67985366d 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -432,6 +432,10 @@ interfaces: endpoint: endpoint_ref min: float32 max: float32 + deadband_enable: bool + deadband_start: float32 + deadband_end: float32 + deadband_level: float32 ODrive.Axis: c_is_class: True @@ -935,6 +939,10 @@ interfaces: Note that this feature is only works on devices with three current sensors (e.g. ODrive v4). dc_calib_tau: float32 + max_dc_calib_koef: + type: float32 + status: experimental + brief: Max current deviation from zero-level, relative to maximum allowed current ODrive.Oscilloscope: c_is_class: True