diff --git a/Arduino/ODriveArduino/ODriveEnums.h b/Arduino/ODriveArduino/ODriveEnums.h index f196ecbd0..855aae3de 100644 --- a/Arduino/ODriveArduino/ODriveEnums.h +++ b/Arduino/ODriveArduino/ODriveEnums.h @@ -55,6 +55,7 @@ enum AxisState { AXIS_STATE_HOMING = 11, AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION = 12, AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION = 13, + AXIS_STATE_BRAKE = 14, }; // ODrive.Encoder.Mode diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 89c56f6bd..351c0d8bb 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -169,16 +169,30 @@ void Axis::watchdog_feed() { // @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired. bool Axis::watchdog_check() { - if (!config_.enable_watchdog) return true; - + if (!config_.enable_watchdog) { + return true; + } // explicit check here to ensure that we don't underflow back to UINT32_MAX if (watchdog_current_value_ > 0) { watchdog_current_value_--; return true; - } else { - error_ |= ERROR_WATCHDOG_TIMER_EXPIRED; - return false; } + // Go to braking state from closed loop control when watchdog times out. + if (current_state_ == AXIS_STATE_CLOSED_LOOP_CONTROL && + config_.enable_brake_state_fallthrough_on_watchdog_timeout) { + //controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; + //controller_.config_.input_mode = Controller::INPUT_MODE_PASSTHROUGH; + //controller_.input_vel_ = 0; + requested_state_ = AXIS_STATE_BRAKE; + return true; + } + // Don't care about timeouts in AXIS_STATE_BRAKE if brake state fallthrough is enabled. + if (current_state_ == AXIS_STATE_BRAKE && + config_.enable_brake_state_fallthrough_on_watchdog_timeout) { + return true; + } + error_ |= ERROR_WATCHDOG_TIMER_EXPIRED; + return false; } bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed, @@ -362,6 +376,22 @@ bool Axis::run_closed_loop_control_loop() { return check_for_errors(); } +bool Axis::run_brake_loop() { + start_closed_loop_control(); + set_step_dir_active(config_.enable_step_dir); + + while ((requested_state_ == AXIS_STATE_UNDEFINED) && motor_.is_armed_) { + controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; + controller_.input_vel_ = 0.0f; + osDelay(1); + } + + set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on); + stop_closed_loop_control(); + + return check_for_errors(); +} + // Slowly drive in the negative direction at homing_speed until the min endstop is pressed // When pressed, set the linear count to the offset (default 0), and then go to position 0 @@ -578,6 +608,15 @@ void Axis::run_state_machine_loop() { status = run_closed_loop_control_loop(); } break; + case AXIS_STATE_BRAKE: { + //if (odrv.any_error()) + // goto invalid_state_label; + if (!motor_.is_calibrated_ || (encoder_.config_.direction==0 && !config_.enable_sensorless_mode)) + goto invalid_state_label; + watchdog_feed(); + status = run_brake_loop(); + } break; + case AXIS_STATE_IDLE: { run_idle_loop(); status = true; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 14f05a63c..5ae333fff 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -77,6 +77,9 @@ class Axis : public ODriveIntf::AxisIntf { float watchdog_timeout = 0.0f; // [s] bool enable_watchdog = false; + bool enable_brake_state_fallthrough_on_watchdog_timeout = false; // loop_cb = {} ); bool run_closed_loop_control_loop(); + bool run_brake_loop(); bool run_homing(); bool run_idle_loop(); diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index fdc582a49..e822aa59f 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -477,6 +477,7 @@ interfaces: type: float32 unit: s enable_watchdog: bool + enable_brake_state_fallthrough_on_watchdog_timeout: bool step_gpio_pin: {type: uint16, c_setter: 'set_step_gpio_pin'} dir_gpio_pin: {type: uint16, c_setter: 'set_dir_gpio_pin'} calibration_lockin: # TODO: this is a subset of lockin state @@ -1368,6 +1369,12 @@ valuetypes: brief: Rotate the motor for 30s to calibrate hall sensor edge offsets doc: The phase offset is not calibrated at this time, so the map is only relative + BRAKE: + brief: Like CLOSED_LOOP_CONTROL with velocity control but forces desired velocity to zero + doc: | + This can be configured as watchdog timeout fallback state instead of idle to avoid + unintentional rolling e.g. when a vehicle is getting a software error or reboot on an + incline. ODrive.Encoder.Mode: values: diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 292ee3be1..5adb2813d 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -44,6 +44,7 @@ AXIS_STATE_HOMING = 11 AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION = 12 AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION = 13 +AXIS_STATE_BRAKE = 14 # ODrive.Encoder.Mode ENCODER_MODE_INCREMENTAL = 0