From 97469235d712da1297dd07ae399e21183cb9d8e7 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 25 Jul 2020 20:24:55 -0700 Subject: [PATCH] Add _src to estimate pointers --- Firmware/MotorControl/axis.cpp | 11 ++++++----- Firmware/MotorControl/controller.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index bf34f2eb..f3cf93f3 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -278,8 +278,8 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. bool Axis::run_sensorless_control_loop() { - controller_.pos_estimate_linear_ = nullptr; - controller_.pos_estimate_circular_ = nullptr; + controller_.pos_estimate_linear_src_ = nullptr; + controller_.pos_estimate_circular_src_ = nullptr; controller_.pos_estimate_valid_src_ = nullptr; controller_.vel_estimate_src_ = &sensorless_estimator_.vel_estimate_; controller_.vel_estimate_valid_src_ = &sensorless_estimator_.vel_estimate_valid_; @@ -302,8 +302,9 @@ bool Axis::run_closed_loop_control_loop() { } // To avoid any transient on startup, we intialize the setpoint to be the current position - controller_.pos_setpoint_ = *controller_.pos_estimate_linear_; - controller_.input_pos_ = *controller_.pos_estimate_linear_; + // TODO: use circular src if in circular mode. + controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_; + controller_.input_pos_ = *controller_.pos_estimate_linear_src_; // Avoid integrator windup issues controller_.vel_integrator_torque_ = 0.0f; @@ -353,7 +354,7 @@ bool Axis::run_homing() { } // To avoid any transient on startup, we intialize the setpoint to be the current position - controller_.pos_setpoint_ = *controller_.pos_estimate_linear_; + controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_; // Avoid integrator windup issues controller_.vel_integrator_torque_ = 0.0f; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index c0845ff3..a25bdefe 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -33,9 +33,9 @@ void Controller::input_pos_updated() { bool Controller::select_encoder(size_t encoder_num) { if (encoder_num < AXIS_COUNT) { Axis* ax = axes[encoder_num]; - pos_estimate_circular_ = &ax->encoder_.pos_circular_; + pos_estimate_circular_src_ = &ax->encoder_.pos_circular_; pos_wrap_src_ = &config_.circular_setpoint_range; - pos_estimate_linear_ = &ax->encoder_.pos_estimate_; + pos_estimate_linear_src_ = &ax->encoder_.pos_estimate_; pos_estimate_valid_src_ = &ax->encoder_.pos_estimate_valid_; vel_estimate_src_ = &ax->encoder_.vel_estimate_; vel_estimate_valid_src_ = &ax->encoder_.vel_estimate_valid_; @@ -119,9 +119,9 @@ static float limitVel(const float vel_limit, const float vel_estimate, const flo bool Controller::update(float* torque_setpoint_output) { float* pos_estimate_linear = (pos_estimate_valid_src_ && *pos_estimate_valid_src_) - ? pos_estimate_linear_ : nullptr; + ? pos_estimate_linear_src_ : nullptr; float* pos_estimate_circular = (pos_estimate_valid_src_ && *pos_estimate_valid_src_) - ? pos_estimate_circular_ : nullptr; + ? pos_estimate_circular_src_ : nullptr; float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_) ? vel_estimate_src_ : nullptr;