diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 98d790bd..e2400c9a 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -206,7 +206,7 @@ void Axis::watchdog_feed() { // @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired. bool Axis::watchdog_check() { // reset value = 0 means watchdog disabled. - if(!config_.enable_watchdog) return true; + if (!config_.enable_watchdog) return true; if (get_watchdog_reset() == 0) return true; // explicit check here to ensure that we don't underflow back to UINT32_MAX @@ -305,12 +305,21 @@ bool Axis::run_closed_loop_control_loop() { // Avoid integrator windup issues controller_.vel_integrator_current_ = 0.0f; - + set_step_dir_active(config_.enable_step_dir); run_control_loop([this]() { // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; - if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) + if (controller_.config_.use_load_encoder) { + if (controller_.config_.load_encoder_axis < AXIS_COUNT) { + Axis* ax = axes[controller_.config_.load_encoder_axis]; + if (!controller_.update(ax->encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) + return error_ |= ERROR_CONTROLLER_FAILED, false; + } else{ + controller_.set_error(Controller::ERROR_INVALID_LOAD_ENCODER); + return error_ |= ERROR_CONTROLLER_FAILED, false; + } + } else if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index efadbcea..a52eb858 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -88,6 +88,10 @@ class Axis { LockinConfig_t lockin; uint8_t can_node_id = 0; // Both axes will have the same id to start uint32_t can_heartbeat_rate_ms = 100; + + bool use_load_encoder = false; + uint8_t load_encoder_axis = -1; + float load_encoder_ratio = 1.0f; }; struct Homing_t { diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 48646068..215d3c17 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -129,8 +129,8 @@ float limitVel(const float vel_limit, const float vel_estimate, const float vel_ bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { // Only runs if config_.anticogging.calib_anticogging is true; non-blocking - anticogging_calibration(pos_estimate, vel_estimate); - float anticogging_pos = pos_estimate / axis_->encoder_.getCoggingRatio(); + anticogging_calibration(axis_->encoder_.pos_estimate_, vel_estimate); + float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio(); // Update inputs switch (config_.input_mode) { diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 54e2acdc..c31692b0 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -8,11 +8,12 @@ class Controller { public: enum Error_t { - ERROR_NONE = 0, - ERROR_OVERSPEED = 0x01, - ERROR_INVALID_INPUT_MODE = 0x02, - ERROR_UNSTABLE_GAIN = 0x04, - ERROR_INVALID_MIRROR_AXIS = 0x08, + ERROR_NONE = 0, + ERROR_OVERSPEED = 0x01, + ERROR_INVALID_INPUT_MODE = 0x02, + ERROR_UNSTABLE_GAIN = 0x04, + ERROR_INVALID_MIRROR_AXIS = 0x08, + ERROR_INVALID_LOAD_ENCODER = 0x10, }; // Note: these should be sorted from lowest level of control to @@ -69,6 +70,9 @@ class Controller { bool enable_current_vel_limit = true; uint8_t axis_to_mirror = -1; float mirror_ratio = 1.0f; + bool use_load_encoder = false; + uint8_t load_encoder_axis = -1; + float load_encoder_ratio = 1.0f; }; explicit Controller(Config_t& config); @@ -153,6 +157,9 @@ class Controller { make_protocol_property("inertia", &config_.inertia), make_protocol_property("axis_to_mirror", &config_.axis_to_mirror), make_protocol_property("mirror_ratio", &config_.mirror_ratio), + make_protocol_property("use_load_encoder", &config_.use_load_encoder), + make_protocol_property("load_encoder_ratio", &config_.load_encoder_ratio), + make_protocol_property("load_encoder_axis", &config_.load_encoder_axis), make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth, [](void* ctx) { static_cast(ctx)->update_filter_gains(); }, this), make_protocol_object("anticogging",