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] 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 a94ec6413..95c3ace78 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 95d38ab00..bfdd8d3c8 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 35036ab97..5d8e7ac1d 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -81,6 +81,7 @@ class Controller : public ODriveIntf::ControllerIntf { 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 @@ class Controller : public ODriveIntf::ControllerIntf { 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 563e936ef..338060b89 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 2f9da82bb..1d30dfa0d 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: