diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 00411b0b..231b2309 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -33,10 +33,10 @@ Axis::Axis(int axis_num, controller_.axis_ = this; motor_.axis_ = this; trap_.axis_ = this; - decode_step_dir_pins(); - watchdog_feed(); min_endstop_.axis_ = this; max_endstop_.axis_ = this; + decode_step_dir_pins(); + watchdog_feed(); } Axis::LockinConfig_t Axis::default_calibration() { @@ -182,10 +182,18 @@ bool Axis::do_checks() { // Sub-components should use set_error which will propegate to this error_ motor_.do_checks(); - encoder_.do_checks(); + // encoder_.do_checks(); // sensorless_estimator_.do_checks(); // controller_.do_checks(); + // Check for endstop presses + bool vel_dependent_stopping = (current_state_ == AXIS_STATE_HOMING) && (controller_.config_.control_mode >= Controller::CTRL_MODE_VELOCITY_CONTROL); + if (min_endstop_.config_.enabled && min_endstop_.get_state() && (!vel_dependent_stopping || controller_.vel_setpoint_ < 0.0f)) { + error_ |= ERROR_MIN_ENDSTOP_PRESSED; + } else if (max_endstop_.config_.enabled && max_endstop_.get_state() && (!vel_dependent_stopping || controller_.vel_setpoint_ > 0.0f)) { + error_ |= ERROR_MAX_ENDSTOP_PRESSED; + } + return check_for_errors(); } @@ -287,13 +295,15 @@ 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() { - run_control_loop([this](){ - if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL) - return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; + controller_.pos_estimate_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_; + run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; - if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, ¤t_setpoint)) + if (!controller_.update(¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_)) return false; // set_error should update axis.error_ @@ -303,8 +313,12 @@ bool Axis::run_sensorless_control_loop() { } bool Axis::run_closed_loop_control_loop() { + if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) { + return error_ |= ERROR_CONTROLLER_FAILED, false; + } + // To avoid any transient on startup, we intialize the setpoint to be the current position - controller_.pos_setpoint_ = encoder_.pos_estimate_; + controller_.pos_setpoint_ = *controller_.pos_estimate_src_; // Avoid integrator windup issues controller_.vel_integrator_current_ = 0.0f; @@ -313,62 +327,99 @@ bool Axis::run_closed_loop_control_loop() { run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop float current_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 + if (!controller_.update(¤t_setpoint)) + return error_ |= ERROR_CONTROLLER_FAILED, false; + 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)) return false; // set_error should update axis.error_ - // Handle the homing case - if (homing_.homing_state == HOMING_STATE_HOMING) { - if (min_endstop_.getEndstopState()) { - // pos_setpoint is the starting position for the trap_traj so we need to set it. - controller_.pos_setpoint_ = min_endstop_.config_.offset; - controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating - - // Set our current position in encoder counts to make control more logical - encoder_.set_linear_count(static_cast(controller_.pos_setpoint_)); - - controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; - controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; - - controller_.input_pos_ = 0.0f; - controller_.input_pos_updated(); - controller_.input_vel_ = 0.0f; - controller_.input_current_ = 0.0f; - - homing_.homing_state = HOMING_STATE_MOVE_TO_ZERO; - } - } else if (homing_.homing_state == HOMING_STATE_MOVE_TO_ZERO) { - if(!min_endstop_.getEndstopState() && controller_.trajectory_done_){ - controller_.config_.control_mode = homing_.storedControlMode; - controller_.config_.input_mode = homing_.storedInputMode; - homing_.homing_state = HOMING_STATE_IDLE; - homing_.isHomed = true; - } - } else { - // Check for endstop presses - if (min_endstop_.config_.enabled && min_endstop_.getEndstopState()) { - return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; - } else if (max_endstop_.config_.enabled && max_endstop_.getEndstopState()) { - return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false; - } - } return true; }); set_step_dir_active(false); return check_for_errors(); } + +// Slowly drive in the negative direction at homing_speed until the min endstop is pressed +// When pressed, set the linear count to the offset (default 0), and then go to position 0 +bool Axis::run_homing() { + Controller::ControlMode_t stored_control_mode = controller_.config_.control_mode; + Controller::InputMode_t stored_input_mode = controller_.config_.input_mode; + + if (!min_endstop_.config_.enabled) { + return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; // TODO: define new error code + } + + controller_.config_.control_mode = Controller::CTRL_MODE_VELOCITY_CONTROL; + controller_.config_.input_mode = Controller::INPUT_MODE_VEL_RAMP; + + controller_.input_pos_ = 0.0f; + controller_.input_pos_updated(); + controller_.input_vel_ = -controller_.config_.homing_speed; + controller_.input_current_ = 0.0f; + + homing_.is_homed = false; + + if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) { + return error_ |= ERROR_CONTROLLER_FAILED, false; + } + + // To avoid any transient on startup, we intialize the setpoint to be the current position + controller_.pos_setpoint_ = *controller_.pos_estimate_src_; + + // Avoid integrator windup issues + controller_.vel_integrator_current_ = 0.0f; + + run_control_loop([this](){ + // Note that all estimators are updated in the loop prefix in run_control_loop + float current_setpoint; + if (!controller_.update(¤t_setpoint)) + return error_ |= ERROR_CONTROLLER_FAILED, false; + + 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)) + return false; // set_error should update axis.error_ + + return !min_endstop_.get_state(); + }); + error_ &= ~ERROR_MIN_ENDSTOP_PRESSED; // clear this error since we deliberately drove into the endstop + + // pos_setpoint is the starting position for the trap_traj so we need to set it. + controller_.pos_setpoint_ = min_endstop_.config_.offset; + controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating + + // Set our current position in encoder counts to make control more logical + encoder_.set_linear_count(static_cast(controller_.pos_setpoint_)); + + controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; + controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; + + controller_.input_pos_ = 0.0f; + controller_.input_pos_updated(); + controller_.input_vel_ = 0.0f; + controller_.input_current_ = 0.0f; + + run_control_loop([this](){ + // Note that all estimators are updated in the loop prefix in run_control_loop + float current_setpoint; + if (!controller_.update(¤t_setpoint)) + return error_ |= ERROR_CONTROLLER_FAILED, false; + + 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)) + return false; // set_error should update axis.error_ + + return !controller_.trajectory_done_; + }); + + controller_.config_.control_mode = stored_control_mode; + controller_.config_.input_mode = stored_input_mode; + homing_.is_homed = true; + + return check_for_errors(); +} + bool Axis::run_idle_loop() { // run_control_loop ignores missed modulation timing updates // if and only if we're in AXIS_STATE_IDLE @@ -396,11 +447,10 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; if (config_.startup_encoder_offset_calibration) task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - if (config_.startup_closed_loop_control){ - if(config_.startup_homing) - task_chain_[pos++] = AXIS_STATE_HOMING; + if (config_.startup_homing) + task_chain_[pos++] = AXIS_STATE_HOMING; + if (config_.startup_closed_loop_control) task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; - } else if (config_.startup_sensorless_control) task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL; task_chain_[pos++] = AXIS_STATE_IDLE; @@ -446,9 +496,9 @@ void Axis::run_state_machine_loop() { status = encoder_.run_direction_find(); } break; - case AXIS_STATE_HOMING: - status = controller_.home_axis(); - break; + case AXIS_STATE_HOMING: { + status = run_homing(); + } break; case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: { if (!motor_.is_calibrated_) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 4264b23b..858df43b 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,13 +5,6 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif - -enum HomingState_t { - HOMING_STATE_IDLE, - HOMING_STATE_HOMING, - HOMING_STATE_MOVE_TO_ZERO -}; - class Axis { public: enum Error_t { @@ -26,7 +19,7 @@ public: ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80, ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value ERROR_CONTROLLER_FAILED = 0x200, - ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, + ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, // DEPRECATED ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, ERROR_MIN_ENDSTOP_PRESSED = 0x1000, ERROR_MAX_ENDSTOP_PRESSED = 0x2000, @@ -96,10 +89,7 @@ public: }; struct Homing_t { - HomingState_t homing_state = HOMING_STATE_IDLE; - Controller::ControlMode_t storedControlMode = Controller::CTRL_MODE_POSITION_CONTROL; - Controller::InputMode_t storedInputMode = Controller::INPUT_MODE_PASSTHROUGH; - bool isHomed = false; + bool is_homed = false; }; enum thread_signals { @@ -224,6 +214,7 @@ public: bool run_lockin_spin(const LockinConfig_t &lockin_config); bool run_sensorless_control_loop(); bool run_closed_loop_control_loop(); + bool run_homing(); bool run_idle_loop(); constexpr uint32_t get_watchdog_reset() { @@ -277,8 +268,7 @@ public: make_protocol_property("requested_state", &requested_state_), make_protocol_ro_property("loop_counter", &loop_counter_), make_protocol_ro_property("lockin_state", &lockin_state_), - make_protocol_ro_property("homing_state", &homing_.homing_state), - make_protocol_property("is_homed", &homing_.isHomed), + make_protocol_property("is_homed", &homing_.is_homed), make_protocol_object("config", make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration), make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search), diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 19833c15..aeb09fd5 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -29,6 +29,25 @@ void Controller::input_pos_updated() { input_pos_updated_ = true; } +bool Controller::select_encoder(size_t encoder_num) { + if (encoder_num < AXIS_COUNT) { + Axis* ax = axes[encoder_num]; + if (config_.setpoints_in_cpr) { + pos_estimate_src_ = &ax->encoder_.pos_cpr_; + pos_wrap_src_ = &ax->encoder_.config_.cpr; + } else { + pos_estimate_src_ = &ax->encoder_.pos_estimate_; + pos_wrap_src_ = nullptr; + } + 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_; + return true; + } else { + return set_error(Controller::ERROR_INVALID_LOAD_ENCODER), false; + } +} + void Controller::move_to_pos(float goal_point) { axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_, axis_->trap_.config_.vel_limit, @@ -55,30 +74,6 @@ void Controller::start_anticogging_calibration() { } } -// Slowly drive in the negative direction at homing_speed until the min endstop is pressed -// When pressed, set the linear count to the offset (default 0), and then - -//TODO: This needs to be upgraded to use its own run_control_loop! -bool Controller::home_axis() { - if (axis_->min_endstop_.config_.enabled) { - axis_->homing_.storedControlMode = config_.control_mode; - axis_->homing_.storedInputMode = config_.input_mode; - - config_.control_mode = CTRL_MODE_VELOCITY_CONTROL; - config_.input_mode = INPUT_MODE_VEL_RAMP; - - input_pos_ = 0.0f; - input_pos_updated(); - input_vel_ = -config_.homing_speed; - input_current_ = 0.0f; - - axis_->homing_.isHomed = false; - axis_->homing_.homing_state = HOMING_STATE_HOMING; - } else { - return false; - } - return true; -} /* * This anti-cogging implementation iterates through each encoder position, @@ -88,32 +83,29 @@ bool Controller::home_axis() { * This holding current is added as a feedforward term in the control loop. */ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) { - if (config_.anticogging.calib_anticogging) { - float pos_err = input_pos_ - pos_estimate; - if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold && - std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold) { - config_.anticogging.cogging_map[std::clamp(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_; - } - if (config_.anticogging.index < 3600) { - config_.control_mode = CTRL_MODE_POSITION_CONTROL; - input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio(); - input_vel_ = 0.0f; - input_current_ = 0.0f; - input_pos_updated(); - return false; - } else { - config_.anticogging.index = 0; - config_.control_mode = CTRL_MODE_POSITION_CONTROL; - input_pos_ = 0.0f; // Send the motor home - input_vel_ = 0.0f; - input_current_ = 0.0f; - input_pos_updated(); - anticogging_valid_ = true; - config_.anticogging.calib_anticogging = false; - return true; - } + float pos_err = input_pos_ - pos_estimate; + if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold && + std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold) { + config_.anticogging.cogging_map[std::clamp(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_; + } + if (config_.anticogging.index < 3600) { + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio(); + input_vel_ = 0.0f; + input_current_ = 0.0f; + input_pos_updated(); + return false; + } else { + config_.anticogging.index = 0; + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + input_pos_ = 0.0f; // Send the motor home + input_vel_ = 0.0f; + input_current_ = 0.0f; + input_pos_updated(); + anticogging_valid_ = true; + config_.anticogging.calib_anticogging = false; + return true; } - return false; } void Controller::update_filter_gains() { @@ -129,10 +121,22 @@ float limitVel(const float vel_limit, const float vel_estimate, const float vel_ } } // namespace -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(axis_->encoder_.pos_estimate_, vel_estimate); - float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio(); +bool Controller::update(float* current_setpoint_output) { + float* pos_estimate_src = (pos_estimate_valid_src_ && *pos_estimate_valid_src_) + ? pos_estimate_src_ : nullptr; + float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_) + ? vel_estimate_src_ : nullptr; + + float anticogging_pos = 0.0f; + if (config_.anticogging.calib_anticogging) { + if (!axis_->encoder_.pos_estimate_valid_ || !axis_->encoder_.vel_estimate_valid_) { + set_error(ERROR_INVALID_ESTIMATE); + return false; + } + // non-blocking + anticogging_calibration(axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_); + anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio(); + } // Update inputs switch (config_.input_mode) { @@ -219,18 +223,22 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s float vel_des = vel_setpoint_; if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) { float pos_err; - if (config_.setpoints_in_cpr) { - // TODO this breaks the semantics that estimates come in on the arguments. - // It's probably better to call a get_estimate that will arbitrate (enc vs sensorless) instead. - float cpr = (float)(axis_->encoder_.config_.cpr); + if (!pos_estimate_src) { + set_error(ERROR_INVALID_ESTIMATE); + return false; + } + + if (pos_wrap_src_) { + float cpr = *pos_wrap_src_; // Keep pos setpoint from drifting pos_setpoint_ = fmodf_pos(pos_setpoint_, cpr); // Circular delta - pos_err = pos_setpoint_ - axis_->encoder_.pos_cpr_; + pos_err = pos_setpoint_ - *pos_estimate_src; pos_err = wrap_pm(pos_err, 0.5f * cpr); } else { - pos_err = pos_setpoint_ - pos_estimate; + pos_err = pos_setpoint_ - *pos_estimate_src; } + vel_des += config_.pos_gain * pos_err; // V-shaped gain shedule based on position error float abs_pos_err = std::abs(pos_err); @@ -248,7 +256,11 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // Check for overspeed fault (done in this module (controller) for cohesion with vel_lim) if (config_.enable_overspeed_error) { // 0.0f to disable - if (std::abs(vel_estimate) > config_.vel_limit_tolerance * vel_lim) { + if (!vel_estimate_src) { + set_error(ERROR_INVALID_ESTIMATE); + return false; + } + if (std::abs(*vel_estimate_src) > config_.vel_limit_tolerance * vel_lim) { set_error(ERROR_OVERSPEED); return false; } @@ -264,17 +276,27 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast(anticogging_pos), 3600), 0, 3600)]; } - float v_err = vel_des - vel_estimate; + float v_err = 0.0f; if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += (config_.vel_gain * gain_scheduling_multiplier) * v_err; - } + if (!vel_estimate_src) { + set_error(ERROR_INVALID_ESTIMATE); + return false; + } - // Velocity integral action before limiting - Iq += vel_integrator_current_; + v_err = vel_des - *vel_estimate_src; + Iq += (config_.vel_gain * gain_scheduling_multiplier) * v_err; + + // Velocity integral action before limiting + Iq += vel_integrator_current_; + } // Velocity limiting in current mode if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL && config_.enable_current_vel_limit) { - Iq = limitVel(config_.vel_limit, vel_estimate, config_.vel_gain, Iq); + if (!vel_estimate_src) { + set_error(ERROR_INVALID_ESTIMATE); + return false; + } + Iq = limitVel(config_.vel_limit, *vel_estimate_src, config_.vel_gain, Iq); } // Current limiting diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 6e6565bb..28f3b026 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -14,6 +14,7 @@ public: ERROR_UNSTABLE_GAIN = 0x04, ERROR_INVALID_MIRROR_AXIS = 0x08, ERROR_INVALID_LOAD_ENCODER = 0x10, + ERROR_INVALID_ESTIMATE = 0x20, }; // Note: these should be sorted from lowest level of control to @@ -54,8 +55,8 @@ public: float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] - float vel_limit = 20000.0f; // [counts/s] - float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable + float vel_limit = 20000.0f; // [counts/s] Infinity to disable. + float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable. float vel_ramp_rate = 10000.0f; // [(counts/s) / s] float current_ramp_rate = 1.0f; // A / sec bool setpoints_in_cpr = false; @@ -67,11 +68,10 @@ public: bool enable_gain_scheduling = false; bool enable_vel_limit = true; bool enable_overspeed_error = true; - bool enable_current_vel_limit = true; + bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator) uint8_t axis_to_mirror = -1; float mirror_ratio = 1.0f; - bool use_load_encoder = false; - uint8_t load_encoder_axis = -1; + uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration() float load_encoder_ratio = 1.0f; }; @@ -80,19 +80,18 @@ public: void set_error(Error_t error); void input_pos_updated(); + bool select_encoder(size_t encoder_num); // Trajectory-Planned control void move_to_pos(float goal_point); void move_incremental(float displacement, bool from_goal_point); - - bool home_axis(); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); bool anticogging_calibration(float pos_estimate, float vel_estimate); void update_filter_gains(); - bool update(float pos_estimate, float vel_estimate, float* current_setpoint); + bool update(float* current_setpoint); Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor @@ -105,6 +104,12 @@ public: Error_t error_ = ERROR_NONE; + float* pos_estimate_src_ = nullptr; + bool* pos_estimate_valid_src_ = nullptr; + float* vel_estimate_src_ = nullptr; + bool* vel_estimate_valid_src_ = nullptr; + int32_t* pos_wrap_src_ = nullptr; // enables circular position setpoints if not null. The value pointed to is the maximum position value. + float pos_setpoint_ = 0.0f; float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; @@ -157,7 +162,6 @@ public: 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, @@ -171,8 +175,7 @@ public: make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio), make_protocol_property("anticogging_enabled", &config_.anticogging.enable))), make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"), - make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), - make_protocol_function("home_axis", *this, &Controller::home_axis) + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); } }; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index ac2d8e0c..5fc32687 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -32,6 +32,8 @@ void Encoder::setup() { } void Encoder::set_error(Error_t error) { + vel_estimate_valid_ = false; + pos_estimate_valid_ = false; error_ |= error; axis_->error_ |= Axis::ERROR_ENCODER_FAILED; } @@ -525,5 +527,7 @@ bool Encoder::update() { // ph = fmodf(ph, 2*M_PI); phase_ = wrap_pm_pi(ph); + vel_estimate_valid_ = true; + pos_estimate_valid_ = true; return true; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index d85bcb44..58eea584 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -94,6 +94,9 @@ public: int32_t pos_abs_ = 0; float spi_error_rate_ = 0.0f; + bool pos_estimate_valid_ = false; + bool vel_estimate_valid_ = false; + int16_t tim_cnt_sample_ = 0; // // Updated by low_level pwm_adc_cb uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index 809e52f1..72bcae3a 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -2,19 +2,19 @@ Endstop::Endstop(Endstop::Config_t& config) : config_(config) { - set_endstop_enabled(config_.enabled); + update_config(); } void Endstop::update() { uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); - auto last_pin_state = pin_state_; + bool last_pin_state = pin_state_; pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + float now = axis_->loop_counter_ * current_meas_period; if (pin_state_ != last_pin_state) { - debounce_timer_ = axis_->loop_counter_ * current_meas_period; + debounce_timer_ = now; } if (config_.enabled) { - float now = axis_->loop_counter_ * current_meas_period; if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues @@ -26,15 +26,15 @@ void Endstop::update() { } } -bool Endstop::getEndstopState() { +bool Endstop::get_state() { return endstop_state_; } -void Endstop::update_endstop_config(){ - set_endstop_enabled(config_.enabled); +void Endstop::update_config(){ + set_enabled(config_.enabled); } -void Endstop::set_endstop_enabled(bool enable) { +void Endstop::set_enabled(bool enable) { if (config_.gpio_num != 0) { uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 45195a2f..9b1fffee 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -6,7 +6,7 @@ class Endstop { struct Config_t { float offset = 0; float debounce_ms = 50.0f; - uint16_t gpio_num; + uint16_t gpio_num = 0; bool enabled = false; bool is_active_high = false; }; @@ -16,11 +16,11 @@ class Endstop { Endstop::Config_t& config_; Axis* axis_ = nullptr; - void update_endstop_config(); - void set_endstop_enabled(bool enable); + void update_config(); + void set_enabled(bool enabled); void update(); - bool getEndstopState(); + bool get_state(); bool endstop_state_ = false; @@ -29,9 +29,9 @@ class Endstop { make_protocol_ro_property("endstop_state", &endstop_state_), make_protocol_object("config", make_protocol_property("gpio_num", &config_.gpio_num, - [](void* ctx) { static_cast(ctx)->update_endstop_config(); }, this), + [](void* ctx) { static_cast(ctx)->update_config(); }, this), make_protocol_property("enabled", &config_.enabled, - [](void* ctx) { static_cast(ctx)->update_endstop_config(); }, this), + [](void* ctx) { static_cast(ctx)->update_config(); }, this), make_protocol_property("offset", &config_.offset), make_protocol_property("is_active_high", &config_.is_active_high), make_protocol_property("debounce_ms", &config_.debounce_ms))); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 17707eaa..6c4f6283 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -85,6 +85,7 @@ extern "C" int load_configuration(void) { Axis::load_default_can_id(i, axis_configs[i]); min_endstop_configs[i] = Endstop::Config_t(); max_endstop_configs[i] = Endstop::Config_t(); + controller_configs[i].load_encoder_axis = i; } } else { user_config_loaded_ = true; diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 43191ce3..aebbc09b 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -65,6 +65,7 @@ bool SensorlessEstimator::update() { // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp < 1.0f)) { error_ |= ERROR_UNSTABLE_GAIN; + vel_estimate_valid_ = false; return false; } @@ -77,5 +78,6 @@ bool SensorlessEstimator::update() { // update PLL velocity vel_estimate_ += current_meas_period * pll_ki * delta_phase; + vel_estimate_valid_ = true; return true; }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 719a3227..e47db893 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -26,6 +26,7 @@ public: float phase_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] float vel_estimate_ = 0.0f; // [rad/s] + bool vel_estimate_valid_ = false; // float pll_kp_ = 0.0f; // [rad/s / rad] // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] float flux_state_[2] = {0.0f, 0.0f}; // [Vs]