diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 21dd73ea..a72b05e2 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -348,8 +348,10 @@ bool Axis::run_homing() { Controller::ControlMode_t stored_control_mode = controller_.config_.control_mode; Controller::InputMode_t stored_input_mode = controller_.config_.input_mode; + // TODO: theoretically this check should be inside the update loop, + // otherwise someone could disable the endstop while homing is in progress. if (!min_endstop_.config_.enabled) { - return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; // TODO: define new error code + return error_ |= ERROR_HOMING_WITHOUT_ENDSTOP, false; } controller_.config_.control_mode = Controller::CTRL_MODE_VELOCITY_CONTROL; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 46e3b15b..f2770ee3 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -26,6 +26,7 @@ public: ERROR_ESTOP_REQUESTED = 0x4000, ERROR_DC_BUS_UNDER_CURRENT = 0x8000, // too much current pushed into the power supply ERROR_DC_BUS_OVER_CURRENT = 0x10000, // too much current pulled out of the power supply + ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000, // the min endstop was not enabled during homing }; enum State_t { @@ -83,10 +84,6 @@ public: 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 738b0b9c..f1687494 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -113,13 +113,11 @@ void Controller::update_filter_gains() { input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped } -namespace { -float limitVel(const float vel_limit, const float vel_estimate, const float vel_gain, const float Iq) { +static float limitVel(const float vel_limit, const float vel_estimate, const float vel_gain, const float Iq) { float Imax = (vel_limit - vel_estimate) * vel_gain; float Imin = (-vel_limit - vel_estimate) * vel_gain; return std::clamp(Iq, Imin, Imax); } -} // namespace bool Controller::update(float* current_setpoint_output) { float* pos_estimate_src = (pos_estimate_valid_src_ && *pos_estimate_valid_src_) @@ -169,7 +167,7 @@ bool Controller::update(float* current_setpoint_output) { float delta_vel = input_vel_ - vel_setpoint_; // Vel error float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback current_setpoint_ = accel * config_.inertia; // Accel - vel_setpoint_ += current_meas_period * accel; // delta vel + vel_setpoint_ += std::clamp(current_meas_period * accel, 2.0f * std::abs(delta_vel), -2.0f * std::abs(delta_vel)); // delta vel pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos } break; case INPUT_MODE_MIRROR: { diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 28f3b026..25261568 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -72,7 +72,6 @@ public: uint8_t axis_to_mirror = -1; float mirror_ratio = 1.0f; uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration() - float load_encoder_ratio = 1.0f; }; explicit Controller(Config_t& config); @@ -162,7 +161,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("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), diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 5fc32687..dbcbab4f 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -449,22 +449,23 @@ bool Encoder::update() { case MODE_SPI_ABS_AMS: case MODE_SPI_ABS_CUI:{ - if(abs_spi_pos_updated_ == false && abs_spi_pos_init_once_){ + if (!abs_spi_pos_updated_ && abs_spi_pos_init_once_) { // Low pass filter the error spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_); - // if (spi_error_rate_ > 0.005f) - // set_error(ERROR_ABS_SPI_COM_FAIL); - } - else + if (spi_error_rate_ > 0.005f) + set_error(ERROR_ABS_SPI_COM_FAIL); + } else { // Low pass filter the error spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_); + } abs_spi_pos_updated_ = false; delta_enc = pos_abs_ - count_in_cpr_; delta_enc = mod(delta_enc, config_.cpr); - if (delta_enc > config_.cpr/2) + if (delta_enc > config_.cpr/2) { delta_enc -= config_.cpr; - if(!abs_spi_pos_init_once_ && delta_enc != 0){ + } + if (!abs_spi_pos_init_once_ && delta_enc != 0) { abs_spi_pos_init_once_ = true; } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 6c4f6283..23d6e84a 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -24,7 +24,7 @@ bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; Axis *axes[AXIS_COUNT]; -ODriveCAN *odCAN; +ODriveCAN *odCAN = nullptr; typedef Config< BoardConfig_t, @@ -132,7 +132,7 @@ void vApplicationIdleHook(void) { system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t); system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t); - system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t); + system_stats_.min_stack_space_can = odCAN ? uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t) : 0; } } }