diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index c94fdfa0..1d4a8bf1 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -224,9 +224,11 @@ bool Controller::update() { } break; case INPUT_MODE_TUNING: { autotuning_phase_ = wrap_pm_pi(autotuning_phase_ + (2.0f * M_PI * autotuning_.frequency * current_meas_period)); - pos_setpoint_ = autotuning_.pos_amplitude * our_arm_sin_f32(autotuning_phase_ + autotuning_.pos_phase); - vel_setpoint_ = autotuning_.vel_amplitude * our_arm_sin_f32(autotuning_phase_ + autotuning_.vel_phase); - torque_setpoint_ = autotuning_.torque_amplitude * our_arm_sin_f32(autotuning_phase_ + autotuning_.torque_phase); + float c = our_arm_cos_f32(autotuning_phase_); + float s = our_arm_sin_f32(autotuning_phase_); + pos_setpoint_ = autotuning_.pos_amplitude * s; // + pos_amp_c * c + vel_setpoint_ = autotuning_.vel_amplitude * c; + torque_setpoint_ = autotuning_.torque_amplitude * -s; } break; default: { set_error(ERROR_INVALID_INPUT_MODE); diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 39f4cd0c..e1212016 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -17,11 +17,11 @@ public: struct Autotuning_t { float frequency = 0.0f; float pos_amplitude = 0.0f; - float pos_phase = 0.0f; float vel_amplitude = 0.0f; - float vel_phase = 0.0f; float torque_amplitude = 0.0f; - float torque_phase = 0.0f; + // float pos_phase = 0.0f; + // float vel_phase = 0.0f; + // float torque_phase = 0.0f; }; struct Config_t { diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 42796c58..40aa68e7 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -892,6 +892,7 @@ interfaces: trajectory_done: readonly bool vel_integrator_torque: float32 anticogging_valid: bool + autotuning_phase: float32 config: c_is_class: False attributes: @@ -986,11 +987,11 @@ interfaces: attributes: frequency: float32 pos_amplitude: float32 - pos_phase: float32 vel_amplitude: float32 - vel_phase: float32 torque_amplitude: float32 - torque_phase: float32 + # pos_phase: float32 + # vel_phase: float32 + # torque_phase: float32 mechanical_power: type: readonly float32 unit: Watt diff --git a/GUI/src/views/Dashboard.vue b/GUI/src/views/Dashboard.vue index 2964d79d..3f04d6e5 100644 --- a/GUI/src/views/Dashboard.vue +++ b/GUI/src/views/Dashboard.vue @@ -308,6 +308,10 @@ let odriveEnums = { { text: "Mirror", value: 7 + }, + { + text: "Tuning", + value: 8 } ] }