diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 15eaa3d5..4171c7e4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -207,7 +207,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme motor_.current_control_.phase_src_.connect_to(&open_loop_controller_.phase_); acim_estimator_.rotor_phase_src_.connect_to(&open_loop_controller_.phase_); - motor_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_); + motor_.vel_src_.disconnect(); motor_.current_control_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_); acim_estimator_.rotor_phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_); } @@ -318,9 +318,14 @@ bool Axis::start_closed_loop_control() { acim_estimator_.rotor_phase_src_.connect_to(phase_src); OutputPort* phase_vel_src = sensorless_mode ? &sensorless_estimator_.phase_vel_ : &encoder_.phase_vel_; - motor_.phase_vel_src_.connect_to(phase_vel_src); motor_.current_control_.phase_vel_src_.connect_to(phase_vel_src); acim_estimator_.rotor_phase_vel_src_.connect_to(phase_vel_src); + + if (motor_.config_.vel_setpoint_FF) { + motor_.vel_src_.connect_to(&controller_.vel_setpoint_); + } else { + motor_.vel_src_.connect_to(sensorless_mode ? &sensorless_estimator_.vel_estimate_ : &encoder_.vel_estimate_); + } if (sensorless_mode) { // Make the final velocity of the loĉk-in spin the setpoint of the diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index e8716965..58bdba56 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -364,7 +364,7 @@ bool Encoder::run_offset_calibration() { axis_->motor_.current_control_.phase_src_.connect_to(&axis_->open_loop_controller_.phase_); axis_->acim_estimator_.rotor_phase_src_.connect_to(&axis_->open_loop_controller_.phase_); - axis_->motor_.phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_); + axis_->motor_.vel_src_.disconnect(); axis_->motor_.current_control_.phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_); axis_->acim_estimator_.rotor_phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_); } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index c8a5e205..054ff816 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -555,27 +555,31 @@ void Motor::update(uint32_t timestamp) { float vd = 0.0f; float vq = 0.0f; - std::optional phase_vel = phase_vel_src_.present(); + std::optional vel = vel_src_.present(); if (config_.R_wL_FF_enable) { - if (!phase_vel.has_value()) { - error_ |= ERROR_UNKNOWN_PHASE_VEL; + if (!vel.has_value()) { + error_ |= ERROR_UNKNOWN_VEL; return; } - vd -= *phase_vel * config_.phase_inductance * iq; - vq += *phase_vel * config_.phase_inductance * id; + float phase_vel = (2*M_PI) * (*vel) * config_.pole_pairs * direction_; + + vd -= phase_vel * config_.phase_inductance * iq; + vq += phase_vel * config_.phase_inductance * id; vd += config_.phase_resistance * id; vq += config_.phase_resistance * iq; } if (config_.bEMF_FF_enable) { - if (!phase_vel.has_value()) { - error_ |= ERROR_UNKNOWN_PHASE_VEL; + if (!vel.has_value()) { + error_ |= ERROR_UNKNOWN_VEL; return; } - vq += *phase_vel * (2.0f/3.0f) * (config_.torque_constant / config_.pole_pairs); + float phase_vel = (2*M_PI) * (*vel) * config_.pole_pairs * direction_; + + vq += phase_vel * (2.0f/3.0f) * (config_.torque_constant / config_.pole_pairs); } if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) { diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index d22a1069..18c93109 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -42,6 +42,7 @@ public: bool R_wL_FF_enable = false; // Enable feedforwards for R*I and w*L*I terms bool bEMF_FF_enable = false; // Enable feedforward for bEMF + bool vel_setpoint_FF = false; float I_bus_hard_min = -INFINITY; float I_bus_hard_max = INFINITY; @@ -125,7 +126,7 @@ public: float max_dc_calib_ = 0.0f; // [A] set in setup() InputPort torque_setpoint_src_; // Usually points to the Controller object's output - InputPort phase_vel_src_; // Usually points to the Encoder object's output + InputPort vel_src_; // Usually points to the Encoder object's output float direction_ = 0.0f; // if -1 then positive torque is converted to negative Iq OutputPort Vdq_setpoint_ = {{0.0f, 0.0f}}; // fed to the FOC diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 7af706dc..cbae5039 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -651,7 +651,7 @@ interfaces: See `ODrive.Error` for more details. BAD_TIMING: {doc: The main control loop got out of sync with the motor control loop. This could indicate that the main control loop got stuck.} UNKNOWN_PHASE_ESTIMATE: {doc: The current controller did not get a valid angle input. Maybe you didn't calibrate the encoder.} - UNKNOWN_PHASE_VEL: {doc: The motor controller did not get a valid phase velocity input.} + UNKNOWN_VEL: {doc: The motor controller did not get a valid velocity input.} UNKNOWN_TORQUE: {doc: The motor controller did not get a valid torque input.} UNKNOWN_CURRENT_COMMAND: {doc: The current controller did not get a valid current setpoint. Maybe you didn't configure the controller correctly.} UNKNOWN_CURRENT_MEASUREMENT: {doc: The current controller did not get a valid current measurement.} @@ -729,6 +729,13 @@ interfaces: acim_autoflux_decay_gain: float32 R_wL_FF_enable: bool bEMF_FF_enable: bool + vel_setpoint_FF: + type: bool + doc: | + If true, the feedforward terms that are enabled by + `R_wL_FF_enable` and `bEMF_FF_enable` are calculated from the + controller's velocity setpoint rather than the velocity estimate. + Default: false. I_bus_hard_min: type: float32 unit: A diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index f1b9ff4b..22efea5b 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -115,7 +115,7 @@ MOTOR_ERROR_BRAKE_RESISTOR_DISARMED = 0x00800000 MOTOR_ERROR_SYSTEM_LEVEL = 0x01000000 MOTOR_ERROR_BAD_TIMING = 0x02000000 MOTOR_ERROR_UNKNOWN_PHASE_ESTIMATE = 0x04000000 -MOTOR_ERROR_UNKNOWN_PHASE_VEL = 0x08000000 +MOTOR_ERROR_UNKNOWN_VEL = 0x08000000 MOTOR_ERROR_UNKNOWN_TORQUE = 0x10000000 MOTOR_ERROR_UNKNOWN_CURRENT_COMMAND = 0x20000000 MOTOR_ERROR_UNKNOWN_CURRENT_MEASUREMENT = 0x40000000