From 301d68adc6d6f6de2c4669e7bf025cbc56c25a19 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 May 2019 18:56:34 +0200 Subject: [PATCH] Move anticogging_t into config, use a fixed size array and scale --- Firmware/.vscode/c_cpp_properties.json | 4 ++-- Firmware/MotorControl/axis.cpp | 11 --------- Firmware/MotorControl/controller.cpp | 33 +++++++++++++------------- Firmware/MotorControl/controller.hpp | 28 +++++++++------------- Firmware/MotorControl/encoder.cpp | 4 ++++ Firmware/MotorControl/encoder.hpp | 3 ++- Firmware/build.lua | 2 +- 7 files changed, 37 insertions(+), 48 deletions(-) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index c4a2f37c..4a2f1f3b 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -30,10 +30,10 @@ "__packed=\"__attribute__((__packed__))\"", "__GNUC__" ], - "intelliSenseMode": "clang-x64", + "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 80987c00..a83677ab 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -288,17 +288,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 d295246c..5634d18b 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) @@ -69,8 +70,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; } } @@ -82,20 +83,20 @@ void Controller::start_anticogging_calibration() { * 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 - set_pos_setpoint(anticogging_.index, 0.0f, 0.0f); + 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; + config_.anticogging.index = 0; set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home - anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging - anticogging_.calib_anticogging = false; + config_.anticogging.use_anticogging = true; // We're good to go, enable anti-cogging + config_.anticogging.calib_anticogging = false; return true; } } @@ -103,9 +104,9 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } 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; // Trajectory control if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) { @@ -179,8 +180,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 020f34d0..2e2886f8 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -22,6 +22,16 @@ public: CTRL_MODE_TRAJECTORY_CONTROL = 4 }; + 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: Motor_control_mode_t float pos_gain = 20.0f; // [(counts/s) / counts] @@ -32,6 +42,7 @@ public: float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable float vel_ramp_rate = 10000.0f; // [(counts/s) / s] bool setpoints_in_cpr = false; + Anticogging_t anticogging; }; explicit Controller(Config_t& config); @@ -61,23 +72,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; // variables exposed on protocol 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 c2d32841..b6a3b3d8 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), diff --git a/Firmware/build.lua b/Firmware/build.lua index d4c7aad4..016f6d8e 100644 --- a/Firmware/build.lua +++ b/Firmware/build.lua @@ -85,7 +85,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags) end return { compile_c = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'gcc -std=c99', compiler_flags, calculate_stack_usage, src, flags, includes, outputs) end, - compile_cpp = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'g++ -std=c++14', compiler_flags, calculate_stack_usage, src, flags, includes, outputs) end, + compile_cpp = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'g++ -std=c++17 -Wno-register', compiler_flags, calculate_stack_usage, src, flags, includes, outputs) end, compile_asm = function(src, flags, includes, outputs) gcc_generic_compiler(prefix..'gcc -x assembler-with-cpp', compiler_flags, false, src, flags, includes, outputs) end, link = function(objects, output_name) output_name = builddir..'/'..output_name