diff --git a/CHANGELOG.md b/CHANGELOG.md index b80d7cbf..6fc8c237 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -33,7 +33,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Accumulate integer steps in step/dir to avoid float precision errors ### API Migration Notes - +* `axis.config.turns_per_step` changed to `axis.controller.config.steps_per_circular_range` * `odrive.axis.fet_thermistor`, `odrive.axis.motor_thermistor` moved to `odrive.axis.motor` object * `enable_uart` and `uart_baudrate` were renamed to `enable_uart0` and `uart0_baudrate`. * `enable_i2c_instead_of_can` was replaced by the separate settings `enable_i2c0` and `enable_can0`. diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 34095b60..14f05a63 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -75,8 +75,6 @@ public: bool enable_sensorless_mode = false; - float turns_per_step = 1.0f / 1024.0f; - float watchdog_timeout = 0.0f; // [s] bool enable_watchdog = false; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d3cded69..7b73e93f 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -109,7 +109,11 @@ bool Controller::update() { std::optional anticogging_vel_estimate = axis_->encoder_.vel_estimate_.present(); if (axis_->step_dir_active_) { - input_pos_ = axis_->steps_ * axis_->config_.turns_per_step; + 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)); } if (config_.anticogging.calib_anticogging) { @@ -123,8 +127,11 @@ bool Controller::update() { // TODO also enable circular deltas for 2nd order filter, etc. if (config_.circular_setpoints) { - // Keep pos setpoint from drifting - input_pos_ = fmodf_pos(input_pos_, config_.circular_setpoint_range); + if (!pos_wrap.has_value()) { + set_error(ERROR_INVALID_CIRCULAR_RANGE); + return false; + } + input_pos_ = fmodf_pos(input_pos_, *pos_wrap); } // Update inputs @@ -155,6 +162,13 @@ bool Controller::update() { case INPUT_MODE_POS_FILTER: { // 2nd order pos tracking filter float delta_pos = input_pos_ - pos_setpoint_; // Pos error + if (config_.circular_setpoints) { + if (!pos_wrap.has_value()) { + set_error(ERROR_INVALID_CIRCULAR_RANGE); + return false; + } + delta_pos = wrap_pm(delta_pos, *pos_wrap); + } float delta_vel = input_vel_ - vel_setpoint_; // Vel error float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback torque_setpoint_ = accel * config_.inertia; // Accel diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 42274b27..f6e589f2 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -36,10 +36,11 @@ public: float vel_ramp_rate = 1.0f; // [(turn/s) / s] float torque_ramp_rate = 0.01f; // Nm / sec bool circular_setpoints = false; - float circular_setpoint_range = 1.0f; // Circular range when circular_setpoints is true. [turn] - float inertia = 0.0f; // [Nm/(turn/s^2)] - float input_filter_bandwidth = 2.0f; // [1/s] - float homing_speed = 0.25f; // [turn/s] + float circular_setpoint_range = 1.0f; // Circular range when circular_setpoints is true. [turn] + uint32_t steps_per_circular_range = 1024; + float inertia = 0.0f; // [Nm/(turn/s^2)] + float input_filter_bandwidth = 2.0f; // [1/s] + float homing_speed = 0.25f; // [turn/s] Anticogging_t anticogging; float gain_scheduling_width = 10.0f; bool enable_gain_scheduling = false; @@ -57,6 +58,7 @@ public: // custom setters 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; } }; Controller() {} diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 63850344..95ead47c 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -418,6 +418,7 @@ interfaces: doc: There isn't a valid position estimate available. step_dir_active: readonly bool last_drv_fault: readonly uint32 + steps: readonly int64 current_state: readonly AxisState requested_state: AxisState is_homed: {type: bool, c_name: homing_.is_homed} @@ -450,7 +451,6 @@ interfaces: This setting only takes effect on a state transition into idle or out of closed loop control. enable_sensorless_mode: bool - turns_per_step: float32 watchdog_timeout: type: float32 unit: s @@ -829,6 +829,7 @@ interfaces: INVALID_MIRROR_AXIS: INVALID_LOAD_ENCODER: INVALID_ESTIMATE: + INVALID_CIRCULAR_RANGE: last_error_time: float32 input_pos: type: float32 @@ -884,6 +885,10 @@ interfaces: circular_setpoint_range: type: float32 doc: circular range in [turns] for position setpoints when circular_setpoints is True + steps_per_circular_range: + type: int32 + doc: Number of steps within the circular setpoint range. Set this and the circular setpoint range to powers of 2 for the best results. + c_setter: set_steps_per_circular_range homing_speed: type: float32 unit: turns/s diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 81a136b3..3506dfbd 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -141,6 +141,7 @@ CONTROLLER_ERROR_UNSTABLE_GAIN = 0x00000004 CONTROLLER_ERROR_INVALID_MIRROR_AXIS = 0x00000008 CONTROLLER_ERROR_INVALID_LOAD_ENCODER = 0x00000010 CONTROLLER_ERROR_INVALID_ESTIMATE = 0x00000020 +CONTROLLER_ERROR_INVALID_CIRCULAR_RANGE = 0x00000040 # ODrive.Encoder.Error ENCODER_ERROR_NONE = 0x00000000