From ae262e4cd1ecaeba64a0d347c8ab3543315e0860 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Mon, 21 Mar 2022 19:21:14 +0100 Subject: [PATCH 1/7] Sets steps_ in CANSimple::set_input_pos_callback ... because otherwise, the effect of the callback is canceled by an assignment in the main update() loop, that calculates input_pos based on steps_, if we're in step/dir mode. - Fixes a steps_ underflow/wrapping bug --- CHANGELOG.md | 1 + Firmware/MotorControl/axis.cpp | 4 +--- Firmware/MotorControl/controller.cpp | 6 ++++++ Firmware/MotorControl/controller.hpp | 2 ++ Firmware/communication/can/can_simple.cpp | 2 +- 5 files changed, 11 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index bd3a75c2..0ab98c4c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Ensure endstops update before being checked for errors, to prevent [#625](https://github.com/odriverobotics/ODrive/issues/625) * Reset trajectory_done_ during homing to ensure a new trajectory is actually computed [#634](https://github.com/odriverobotics/ODrive/issues/634) * Use `input_xxx` as a DC offset in tuning mode +* Syncs `steps_` with input pos when set explicitly via CAN. # Releases ## [0.5.4] - 2021-10-12 diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index d8579c5b..a94ec641 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -295,9 +295,7 @@ bool Axis::start_closed_loop_control() { return false; } else { controller_.pos_setpoint_ = *pos_init; - controller_.input_pos_ = *pos_init; - float range = controller_.config_.circular_setpoint_range; - steps_ = (int64_t)(fmodf_pos(*pos_init, range) / range * controller_.config_.steps_per_circular_range); + controller_.set_input_pos_and_steps(*pos_init); } } controller_.input_pos_updated(); diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index e17e8629..cbedb140 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -87,6 +87,12 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } } +void Controller::set_input_pos_and_steps(float const pos) { + input_pos_ = pos; + float const range = config_.circular_setpoint_range; + axis_->steps_ = (int64_t)(wrap_pm(pos, range) / range * config_.steps_per_circular_range); +} + void Controller::update_filter_gains() { float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz); input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index da19149e..35036ab9 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -80,6 +80,8 @@ public: void start_anticogging_calibration(); bool anticogging_calibration(float pos_estimate, float vel_estimate); + void set_input_pos_and_steps(float pos); + void update_filter_gains(); bool update(); diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 961d4c82..904af01c 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -253,7 +253,7 @@ bool CANSimple::get_encoder_count_callback(const Axis& axis) { } void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.input_pos_ = can_getSignal(msg, 0, 32, true); + axis.controller_.set_input_pos_and_steps(can_getSignal(msg, 0, 32, true)); axis.controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.001f, 0); axis.controller_.input_torque_ = can_getSignal(msg, 48, 16, true, 0.001f, 0); axis.controller_.input_pos_updated(); From d272fbe2b4d4a6a87acf75f344db7a098c29179e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Tue, 29 Mar 2022 22:31:12 +0200 Subject: [PATCH 2/7] Puts fmodf_pos back into the steps_ assignment --- Firmware/MotorControl/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index cbedb140..ab7e62eb 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -90,7 +90,7 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) void Controller::set_input_pos_and_steps(float const pos) { input_pos_ = pos; float const range = config_.circular_setpoint_range; - axis_->steps_ = (int64_t)(wrap_pm(pos, range) / range * config_.steps_per_circular_range); + axis_->steps_ = (int64_t)(fmodf_pos(pos, range) / range * config_.steps_per_circular_range); } void Controller::update_filter_gains() { From 58a6567f837c8ed9c8c31271a4201c8cc03c4ecc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Tue, 5 Apr 2022 19:09:33 +0200 Subject: [PATCH 3/7] Stands still upon switching to position mode via can --- Firmware/communication/can/can_simple.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 904af01c..563e936e 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -269,7 +269,20 @@ void CANSimple::set_input_torque_callback(Axis& axis, const can_Message_t& msg) } void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.config_.control_mode = static_cast(can_getSignal(msg, 0, 32, true)); + + Controller::ControlMode const mode = static_cast(can_getSignal(msg, 0, 32, true)); + if (mode == Controller::CONTROL_MODE_POSITION_CONTROL) { + float estimate = 0.0f; + if (axis.controller_.config_.circular_setpoints) { + estimate = axis.encoder_.pos_circular_.any().value_or(0.0f); + } else { + estimate = axis.encoder_.pos_estimate_.any().value_or(0.0f); + } + + axis.controller_.set_input_pos_and_steps(estimate); + } + + axis.controller_.config_.control_mode = static_cast(mode); axis.controller_.config_.input_mode = static_cast(can_getSignal(msg, 32, 32, true)); } From c5017f4baf2f50b78cdcb2c28339a779bec62b30 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Wed, 4 May 2022 16:30:30 +0200 Subject: [PATCH 4/7] Making circular_setpoints optional in step/dir mode .. because circular_setpoints was broken beyond repair. --- Firmware/MotorControl/controller.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index ab7e62eb..95d38ab0 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -89,10 +89,15 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) void Controller::set_input_pos_and_steps(float const pos) { input_pos_ = pos; - float const range = config_.circular_setpoint_range; - axis_->steps_ = (int64_t)(fmodf_pos(pos, range) / range * config_.steps_per_circular_range); + if (config_.circular_setpoints) { + float const range = config_.circular_setpoint_range; + axis_->steps_ = (int64_t)(fmodf_pos(pos, range) / range * config_.steps_per_circular_range); + } else { + axis_->steps_ = (int64_t)(pos * config_.steps_per_circular_range); + } } + void Controller::update_filter_gains() { float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz); input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time @@ -115,11 +120,15 @@ bool Controller::update() { std::optional anticogging_vel_estimate = axis_->encoder_.vel_estimate_.present(); if (axis_->step_dir_active_) { - if (!pos_wrap.has_value()) { - set_error(ERROR_INVALID_CIRCULAR_RANGE); - return false; + if (config_.circular_setpoints) { + if (!pos_wrap.has_value()) { + set_error(ERROR_INVALID_CIRCULAR_RANGE); + return false; + } + input_pos_ = (float)(axis_->steps_ % config_.steps_per_circular_range) * (*pos_wrap / (float)(config_.steps_per_circular_range)); + } else { + input_pos_ = (float)(axis_->steps_) / (float)(config_.steps_per_circular_range); } - input_pos_ = (float)(axis_->steps_ % config_.steps_per_circular_range) * (*pos_wrap / (float)(config_.steps_per_circular_range)); } if (config_.anticogging.calib_anticogging) { From fa144cff291f8d80f6a7a8c7626e92b7bbe1b71c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Thu, 5 May 2022 10:51:26 +0200 Subject: [PATCH 5/7] Makes behavior consistent across interfaces Defines and uses set_setpoint_to_estimate() reusable function Uses it in three places: - axis.cpp to avoid transient on startup - can_simple.cpp to avoid sudden movement upon mode change - odrive-interface.yaml just to make it executable via USB and UART Swaps set_input_pos() to set_input_pos_and_steps() so that we get the same behavior when changing input_pos over USB and UART as we get when we change it via CAN. --- Firmware/MotorControl/axis.cpp | 10 +--------- Firmware/MotorControl/controller.cpp | 13 +++++++++++++ Firmware/MotorControl/controller.hpp | 3 ++- Firmware/communication/can/can_simple.cpp | 9 +-------- Firmware/odrive-interface.yaml | 2 ++ 5 files changed, 19 insertions(+), 18 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index a94ec641..95c3ace7 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -288,15 +288,7 @@ bool Axis::start_closed_loop_control() { // To avoid any transient on startup, we intialize the setpoint to be the current position if (controller_.config_.control_mode >= Controller::CONTROL_MODE_POSITION_CONTROL) { - std::optional pos_init = (controller_.config_.circular_setpoints ? - controller_.pos_estimate_circular_src_ : - controller_.pos_estimate_linear_src_).any(); - if (!pos_init.has_value()) { - return false; - } else { - controller_.pos_setpoint_ = *pos_init; - controller_.set_input_pos_and_steps(*pos_init); - } + controller_.set_setpoint_to_estimate(); } controller_.input_pos_updated(); diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 95d38ab0..bfdd8d3c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -97,6 +97,19 @@ void Controller::set_input_pos_and_steps(float const pos) { } } +bool Controller::set_setpoint_to_estimate() { + std::optional estimate = (config_.circular_setpoints ? + pos_estimate_circular_src_ : + pos_estimate_linear_src_).any(); + if (!estimate.has_value()) { + return false; + } + + pos_setpoint_ = *estimate; + set_input_pos_and_steps(*estimate); + return true; +} + void Controller::update_filter_gains() { float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz); diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 35036ab9..5d8e7ac1 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -81,6 +81,7 @@ public: bool anticogging_calibration(float pos_estimate, float vel_estimate); void set_input_pos_and_steps(float pos); + bool set_setpoint_to_estimate(); void update_filter_gains(); bool update(); @@ -124,7 +125,7 @@ public: OutputPort torque_output_ = 0.0f; // custom setters - void set_input_pos(float value) { input_pos_ = value; input_pos_updated(); } + void set_input_pos(float value) { set_input_pos_and_steps(value); input_pos_updated(); } }; #endif // __CONTROLLER_HPP diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 563e936e..338060b8 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -272,14 +272,7 @@ void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& m Controller::ControlMode const mode = static_cast(can_getSignal(msg, 0, 32, true)); if (mode == Controller::CONTROL_MODE_POSITION_CONTROL) { - float estimate = 0.0f; - if (axis.controller_.config_.circular_setpoints) { - estimate = axis.encoder_.pos_circular_.any().value_or(0.0f); - } else { - estimate = axis.encoder_.pos_estimate_.any().value_or(0.0f); - } - - axis.controller_.set_input_pos_and_steps(estimate); + axis.controller_.set_setpoint_to_estimate(); } axis.controller_.config_.control_mode = static_cast(mode); diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 2f9da82b..1d30dfa0 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -1013,6 +1013,7 @@ interfaces: c_setter: set_input_pos doc: | Set the desired position of the axis. Only valid in `CONTROL_MODE_POSITION_CONTROL`.startup. + Also updates the step count. In `INPUT_MODE_TUNING`, this acts as a DC offset for the position sine wave. input_vel: type: float32 @@ -1610,6 +1611,7 @@ valuetypes: doc: | Uses the inner torque loop, the velocity control loop, and the outer position control loop. Use `input_pos` to command desired position, `input_vel` to command velocity feed-forward, and `input_torque` for torque feed-forward. + c_setter: set_setpoint_to_estimate ODrive.Controller.InputMode: values: From 6444197251fafde5c4ff957aee6149b93bf22970 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 5 May 2022 13:40:07 -0700 Subject: [PATCH 6/7] make input mode change trigger consistent with all sources --- CHANGELOG.md | 3 ++- Firmware/MotorControl/axis.cpp | 4 +--- Firmware/MotorControl/controller.cpp | 20 +++++++++++--------- Firmware/MotorControl/controller.hpp | 5 ++--- Firmware/communication/can/can_simple.cpp | 6 +----- Firmware/odrive-interface.yaml | 2 +- 6 files changed, 18 insertions(+), 22 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ab98c4c..8dc021f7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,7 +5,8 @@ Please add a note of your changes below this heading if you make a Pull Request. * Ensure endstops update before being checked for errors, to prevent [#625](https://github.com/odriverobotics/ODrive/issues/625) * Reset trajectory_done_ during homing to ensure a new trajectory is actually computed [#634](https://github.com/odriverobotics/ODrive/issues/634) * Use `input_xxx` as a DC offset in tuning mode -* Syncs `steps_` with input pos when set explicitly via CAN. +* Sync `steps_` with input pos. +* Trigger reset of input_pos and pos_setpoint to estimate when changing control mode into position control # Releases ## [0.5.4] - 2021-10-12 diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 95c3ace7..f11a9878 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -287,9 +287,7 @@ bool Axis::start_closed_loop_control() { } // To avoid any transient on startup, we intialize the setpoint to be the current position - if (controller_.config_.control_mode >= Controller::CONTROL_MODE_POSITION_CONTROL) { - controller_.set_setpoint_to_estimate(); - } + controller_.control_mode_updated(); controller_.input_pos_updated(); // Avoid integrator windup issues diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index bfdd8d3c..4cf2700c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -97,16 +97,18 @@ void Controller::set_input_pos_and_steps(float const pos) { } } -bool Controller::set_setpoint_to_estimate() { - std::optional estimate = (config_.circular_setpoints ? - pos_estimate_circular_src_ : - pos_estimate_linear_src_).any(); - if (!estimate.has_value()) { - return false; - } +bool Controller::control_mode_updated() { + if (config_.control_mode >= CONTROL_MODE_POSITION_CONTROL) { + std::optional estimate = (config_.circular_setpoints ? + pos_estimate_circular_src_ : + pos_estimate_linear_src_).any(); + if (!estimate.has_value()) { + return false; + } - pos_setpoint_ = *estimate; - set_input_pos_and_steps(*estimate); + pos_setpoint_ = *estimate; + set_input_pos_and_steps(*estimate); + } return true; } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 5d8e7ac1..6701eb5c 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -69,6 +69,8 @@ public: constexpr void input_pos_updated() { input_pos_updated_ = true; } + bool control_mode_updated(); + void set_input_pos_and_steps(float pos); bool select_encoder(size_t encoder_num); @@ -80,9 +82,6 @@ public: void start_anticogging_calibration(); bool anticogging_calibration(float pos_estimate, float vel_estimate); - void set_input_pos_and_steps(float pos); - bool set_setpoint_to_estimate(); - void update_filter_gains(); bool update(); diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 338060b8..93b9f7bc 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -269,14 +269,10 @@ void CANSimple::set_input_torque_callback(Axis& axis, const can_Message_t& msg) } void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& msg) { - Controller::ControlMode const mode = static_cast(can_getSignal(msg, 0, 32, true)); - if (mode == Controller::CONTROL_MODE_POSITION_CONTROL) { - axis.controller_.set_setpoint_to_estimate(); - } - axis.controller_.config_.control_mode = static_cast(mode); axis.controller_.config_.input_mode = static_cast(can_getSignal(msg, 32, 32, true)); + axis.controller_.control_mode_updated(); } void CANSimple::set_limits_callback(Axis& axis, const can_Message_t& msg) { diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 1d30dfa0..40e90a96 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -1591,6 +1591,7 @@ valuetypes: doc: MagAlpha MA732 magnetic encoder ODrive.Controller.ControlMode: + c_setter: set_setpoint_to_estimate values: # Note: these should be sorted from lowest level of control to # highest level of control, to allow "<" style comparisons. @@ -1611,7 +1612,6 @@ valuetypes: doc: | Uses the inner torque loop, the velocity control loop, and the outer position control loop. Use `input_pos` to command desired position, `input_vel` to command velocity feed-forward, and `input_torque` for torque feed-forward. - c_setter: set_setpoint_to_estimate ODrive.Controller.InputMode: values: From 4ca99a62c88278a332fa83b23256c05dcea5ebdd Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 5 May 2022 13:58:33 -0700 Subject: [PATCH 7/7] fix setter logic --- Firmware/MotorControl/controller.hpp | 1 + Firmware/odrive-interface.yaml | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 6701eb5c..2fc54861 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -58,6 +58,7 @@ public: Controller* parent; void set_input_filter_bandwidth(float value) { input_filter_bandwidth = value; parent->update_filter_gains(); } void set_steps_per_circular_range(uint32_t value) { steps_per_circular_range = value > 0 ? value : steps_per_circular_range; } + void set_control_mode(ControlMode value) { control_mode = value; parent->control_mode_updated(); } }; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 40e90a96..77b997fe 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -1069,7 +1069,7 @@ interfaces: enable_overspeed_error: type: bool doc: Enables the velocity controller's overspeed error - control_mode: ControlMode + control_mode: {type: ControlMode, c_setter: set_control_mode} input_mode: InputMode pos_gain: type: float32 @@ -1591,7 +1591,6 @@ valuetypes: doc: MagAlpha MA732 magnetic encoder ODrive.Controller.ControlMode: - c_setter: set_setpoint_to_estimate values: # Note: these should be sorted from lowest level of control to # highest level of control, to allow "<" style comparisons.