Merge branch 'anticogging_saver' into RazorsEdge

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",

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();

Firmware/MotorControl/controller.cpp

@@ -1,6 +1,7 @@
 
 #include "odrive_main.h"
 
+#include <algorithm>
 
 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<int>(anticogging_pos), axis_->encoder_.config_.cpr)];
+    if (config_.anticogging.use_anticogging) {
+        Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast<int>(anticogging_pos), axis_->encoder_.config_.cpr), 0, 3600)];
     }
 
     float v_err = vel_des - vel_estimate;

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;

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) {

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<Encoder*>(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<Encoder*>(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),