diff --git a/CHANGELOG.md b/CHANGELOG.md index bd3a75c2f..0ab98c4cc 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 d8579c5b2..95c3ace78 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -288,17 +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_.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_setpoint_to_estimate(); } controller_.input_pos_updated(); diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index e17e86294..bfdd8d3c8 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -87,6 +87,30 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } } +void Controller::set_input_pos_and_steps(float const pos) { + input_pos_ = pos; + 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); + } +} + +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); input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time @@ -109,11 +133,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) { diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index da19149ed..5d8e7ac1d 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -80,6 +80,9 @@ class Controller : public ODriveIntf::ControllerIntf { 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(); @@ -122,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 961d4c826..338060b89 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(); @@ -269,7 +269,13 @@ 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) { + 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)); } 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: