From 1808bedf1a44de321e8aac293efd72985639e1ad Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 18 Jul 2020 19:02:28 -0700 Subject: [PATCH] Fix phase_vel units --- Firmware/MotorControl/axis.cpp | 6 +++--- Firmware/MotorControl/motor.cpp | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7fcff0a3..bf34f2eb 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -315,7 +315,7 @@ bool Axis::run_closed_loop_control_loop() { if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; - float phase_vel = encoder_.vel_estimate_ * motor_.config_.pole_pairs; + float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs; if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ @@ -364,7 +364,7 @@ bool Axis::run_homing() { if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; - float phase_vel = encoder_.vel_estimate_ * motor_.config_.pole_pairs; + float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs; if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ @@ -393,7 +393,7 @@ bool Axis::run_homing() { if (!controller_.update(&torque_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; - float phase_vel = encoder_.vel_estimate_ * motor_.config_.pole_pairs; + float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs; if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index d74fbcec..b104619c 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -454,7 +454,9 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha return true; } - +// torque_setpoint [Nm] +// phase [rad electrical] +// phase_vel [rad/s electrical] bool Motor::update(float torque_setpoint, float phase, float phase_vel) { float current_setpoint = 0.0f; phase *= config_.direction;