Auto-adjust centerpoint when idle

firmware/platformio.ini

@@ -20,8 +20,9 @@ monitor_flags =
 lib_deps =
     TFT_eSPI@2.3.59
     fastled/FastLED @ ^3.4.0
-	askuric/Simple FOC @ ^2.2
-	infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
+    askuric/Simple FOC @ ^2.2
+    infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
+    bxparks/AceButton @ ^1.9.1
 
 build_flags =
   -Os

firmware/src/main.cpp

@@ -1,12 +1,15 @@
+#include <AceButton.h>
 #include <Arduino.h>
-#include <TFT_eSPI.h>
 #include <FastLED.h>
 #include <SimpleFOC.h>
+#include <TFT_eSPI.h>
 
 #include "display_task.h"
 #include "motor_task.h"
 #include "tlv_sensor.h"
 
+using namespace ace_button;
+
 DisplayTask display_task = DisplayTask(1);
 MotorTask motor_task = MotorTask(0, display_task);
 

firmware/src/motor_task.cpp

@@ -3,13 +3,20 @@
 #include "motor_task.h"
 #include "tlv_sensor.h"
 
+
 template <typename T> T CLAMP(const T& value, const T& low, const T& high) 
 {
   return value < low ? low : (value > high ? high : value); 
 }
 
 static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
-static const float DEAD_ZONE_RAD = 3 * _PI / 180;
+static const float DEAD_ZONE_RAD = 1 * _PI / 180;
+
+static const float IDLE_VELOCITY_EWMA_ALPHA = 0.001;
+static const float IDLE_VELOCITY_RAD_PER_SEC = 0.05;
+static const uint32_t IDLE_CORRECTION_DELAY_MILLIS = 500;
+static const float IDLE_CORRECTION_MAX_ANGLE_RAD = 5 * PI / 180;
+static const float IDLE_CORRECTION_RATE_ALPHA = 0.0005;
 
 
 MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
@@ -66,7 +73,7 @@ void MotorTask::run() {
     motor.useMonitoring(Serial);
     motor.monitor_downsample = 0; // disable monitor at first - optional
 
-    disableCore0WDT();
+    // disableCore0WDT();
 
     float current_detent_center = motor.shaft_angle;
 
@@ -74,11 +81,35 @@ void MotorTask::run() {
     int max = 255;
     int value = 0;
 
+    float idle_check_velocity_ewma = 0;
+    uint32_t last_idle_start = 0;
+    uint32_t last_debug = 0;
 
     while (1) {
 
         motor.loopFOC();
 
+        idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);
+        if (fabsf(idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC) {
+            last_idle_start = 0;
+        } else {
+            if (last_idle_start == 0) {
+                last_idle_start = millis();
+            }
+        }
+
+        // If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
+        if (last_idle_start > 0 && millis() - last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(motor.shaft_angle - current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD) {
+            current_detent_center = motor.shaft_angle * IDLE_CORRECTION_RATE_ALPHA + current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA);
+            if (millis() - last_debug > 100) {
+                last_debug = millis();
+                Serial.print("Moving detent center. ");
+                Serial.print(current_detent_center);
+                Serial.print(" ");
+                Serial.println(motor.shaft_angle);
+            }
+        }
+
         if (fabs(detents) < 0.01) {
             motor.move(0);
             display_task_.set_angle(motor.shaft_angle);
@@ -86,11 +117,6 @@ void MotorTask::run() {
         } else {
             float detent_width = 2*PI/detents;
 
-            // if (millis() - last > 500) {
-            //     last = millis();
-            //     current_detent_center += detent_width;
-            // }
-
             float angle_to_detent_center = motor.shaft_angle - current_detent_center;
             if (angle_to_detent_center > detent_width * 1.2 && value > min) {
                 current_detent_center += detent_width;
@@ -102,15 +128,16 @@ void MotorTask::run() {
                 value++;
             }
 
-            float dead_zone = CLAMP(
+            float dead_zone_adjustment = CLAMP(
                 angle_to_detent_center,
                 fmaxf(-detent_width*DEAD_ZONE_DETENT_PERCENT, -DEAD_ZONE_RAD),
                 fminf(detent_width*DEAD_ZONE_DETENT_PERCENT, DEAD_ZONE_RAD));
             
             if (fabsf(motor.shaft_velocity) > 20) {
+                // Don't apply torque if velocity is too high (helps avoid feedback loop)
                 motor.move(0);
             } else {
-                motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone));
+                motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment));
             }
 
             display_task_.set_angle(-value * PI / 180);