diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 1886c707..4790c165 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -20,7 +20,7 @@ "intelliSenseMode": "gcc-x64", "compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float", "cStandard": "c11", - "cppStandard": "c++14" + "cppStandard": "c++17" }, { "name": "Linux", diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 895b3432..7305e28e 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -323,16 +323,6 @@ bool Axis::run_idle_loop() { // Infinite loop that does calibration and enters main control loop as appropriate void Axis::run_state_machine_loop() { - // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f - // TODO: Move this somewhere else - // TODO: respect changes of CPR - int encoder_cpr = encoder_.config_.cpr; - controller_.anticogging_.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); - if (controller_.anticogging_.cogging_map != NULL) { - for (int i = 0; i < encoder_cpr; i++) { - controller_.anticogging_.cogging_map[i] = 0.0f; - } - } // arm! motor_.arm(); diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 7063f081..f668bff4 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -1,6 +1,7 @@ #include "odrive_main.h" +#include Controller::Controller(Config_t& config) : config_(config) @@ -50,8 +51,8 @@ void Controller::move_incremental(float displacement, bool from_goal_point = tru void Controller::start_anticogging_calibration() { // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating - if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) { - anticogging_.calib_anticogging = true; + if (axis_->error_ == Axis::ERROR_NONE) { + config_.anticogging.calib_anticogging = true; } } @@ -78,19 +79,20 @@ 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 (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) { - float pos_err = anticogging_.index - pos_estimate; - if (fabsf(pos_err) <= anticogging_.calib_pos_threshold && - fabsf(vel_estimate) < anticogging_.calib_vel_threshold) { - anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_; + if (config_.anticogging.calib_anticogging) { + float pos_err = config_.anticogging.index - pos_estimate; + if (fabsf(pos_err) <= config_.anticogging.calib_pos_threshold && + fabsf(vel_estimate) < config_.anticogging.calib_vel_threshold) { + config_.anticogging.cogging_map[std::clamp(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_; } - if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR - pos_setpoint_ = anticogging_.index; + if (config_.anticogging.index < 3600) { + set_pos_setpoint(config_.anticogging.index * config_.anticogging.cogging_ratio, 0.0f, 0.0f); return false; } else { - anticogging_.index = 0; - anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging - anticogging_.calib_anticogging = false; + config_.anticogging.index = 0; + set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home + config_.anticogging.use_anticogging = true; // We're good to go, enable anti-cogging + config_.anticogging.calib_anticogging = false; return true; } } @@ -103,9 +105,9 @@ void Controller::update_filter_gains() { } bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { - // Only runs if anticogging_.calib_anticogging is true; non-blocking + // Only runs if config_.anticogging.calib_anticogging is true; non-blocking anticogging_calibration(pos_estimate, vel_estimate); - float anticogging_pos = pos_estimate; + float anticogging_pos = pos_estimate / config_.anticogging.cogging_ratio; // Update inputs switch (config_.input_mode) { @@ -207,8 +209,8 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - if (anticogging_.use_anticogging) { - Iq += anticogging_.cogging_map[mod(static_cast(anticogging_pos), axis_->encoder_.config_.cpr)]; + if (config_.anticogging.use_anticogging) { + Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast(anticogging_pos), axis_->encoder_.config_.cpr), 0, 3600)]; } float v_err = vel_des - vel_estimate; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 5673a9d4..9521df57 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -32,6 +32,16 @@ public: INPUT_MODE_TRAP_TRAJ, }; + typedef struct { + int index = 0; + float cogging_map[3600]; + bool use_anticogging = false; + bool calib_anticogging = false; + float calib_pos_threshold = 1.0f; + float calib_vel_threshold = 1.0f; + float cogging_ratio = 1.0f; + } Anticogging_t; + struct Config_t { ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: ControlMode_t InputMode_t input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t @@ -46,6 +56,7 @@ public: float inertia = 0.0f; // [A/(count/s^2)] float input_filter_bandwidth = 2.0f; // [1/s] float homing_speed = 2000.0f; // [counts/s] + Anticogging_t anticogging; }; explicit Controller(Config_t& config); @@ -76,23 +87,6 @@ public: // - use python tools to Fourier transform and write back the smoothed map or Fourier coefficients // - make the calibration persistent - typedef struct { - int index; - float *cogging_map; - bool use_anticogging; - bool calib_anticogging; - float calib_pos_threshold; - float calib_vel_threshold; - } Anticogging_t; - Anticogging_t anticogging_ = { - .index = 0, - .cogging_map = nullptr, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }; - Error_t error_ = ERROR_NONE; float pos_setpoint_ = 0.0f; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 95dc91f9..27679c33 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -99,6 +99,10 @@ void Encoder::set_linear_count(int32_t count) { cpu_exit_critical(prim); } +void Encoder::cpr_changed_callback(){ + axis_->controller_.config_.anticogging.cogging_ratio = config_.cpr / 3600.0f; +} + // Function that sets the CPR circular tracking encoder count to a desired 32-bit value. // Note that this will get mod'ed down to [0, cpr) void Encoder::set_circular_count(int32_t count, bool update_offset) { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 90382fc4..43fb42d8 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -67,6 +67,7 @@ public: void sample_now(); bool update(); + void cpr_changed_callback(); const EncoderHardwareConfig_t& hw_config_; @@ -119,7 +120,7 @@ public: make_protocol_property("pre_calibrated", &config_.pre_calibrated, [](void* ctx) { static_cast(ctx)->check_pre_calibrated(); }, this), make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx), - make_protocol_property("cpr", &config_.cpr), + make_protocol_property("cpr", &config_.cpr, [](void* ctx) { static_cast(ctx)->cpr_changed_callback(); }, this), make_protocol_property("offset", &config_.offset), make_protocol_property("offset_float", &config_.offset_float), make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation),