Add function to remove anticogging bias

Firmware/MotorControl/controller.cpp

@@ -1,6 +1,7 @@
 
 #include "odrive_main.h"
 #include <algorithm>
+#include <numeric>
 
 bool Controller::apply_config() {
     config_.parent = this;
@@ -53,6 +54,20 @@ void Controller::start_anticogging_calibration() {
     }
 }
 
+float Controller::remove_anticogging_bias()
+{
+    auto& cogmap = config_.anticogging.cogging_map;
+
+    auto sum = std::accumulate(std::begin(cogmap), std::end(cogmap), 0.0f);
+    auto average = sum / std::size(cogmap);
+
+    for(auto& val : cogmap) {
+        val -= average;
+    }
+
+    return average;
+}
+
 
 /*
  * This anti-cogging implementation iterates through each encoder position,

Firmware/MotorControl/controller.hpp

@@ -81,6 +81,7 @@ class Controller : public ODriveIntf::ControllerIntf {
 
     // TODO: make this more similar to other calibration loops
     void start_anticogging_calibration();
+    float remove_anticogging_bias();
     bool anticogging_calibration(float pos_estimate, float vel_estimate);
 
     float get_anticogging_value(uint32_t index) {

Firmware/odrive-interface.yaml

@@ -1196,6 +1196,7 @@ interfaces:
             usually corresponds roughly to the current position of the axis.'
           }
       start_anticogging_calibration:
+      remove_anticogging_bias: {out: {val: float32}}
       get_anticogging_value: {in: {index: uint32}, out: {val: float32}}