From 51f43cf7278fcff3553422b30495b4ffd0b9dd55 Mon Sep 17 00:00:00 2001
From: Scott Bezek <scottbez1@gmail.com>
Date: Sat, 23 Oct 2021 20:47:24 -0700
Subject: [PATCH] dead zone and end stops

---
 firmware/src/motor_task.cpp | 53 ++++++++++++++++++++++++++++++++-----
 firmware/src/tlv_sensor.cpp | 12 ++++++---
 firmware/src/tlv_sensor.h   |  1 +
 3 files changed, 55 insertions(+), 11 deletions(-)

diff --git a/firmware/src/motor_task.cpp b/firmware/src/motor_task.cpp
index 4c575b2..b062bc8 100644
--- a/firmware/src/motor_task.cpp
+++ b/firmware/src/motor_task.cpp
@@ -3,6 +3,15 @@
 #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;
+
+
 MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
 }
 
@@ -57,24 +66,54 @@ void MotorTask::run() {
     motor.useMonitoring(Serial);
     motor.monitor_downsample = 0; // disable monitor at first - optional
 
-    float attract_angle = 0;
-
     disableCore0WDT();
 
+    float current_detent_center = motor.shaft_angle;
+
+    int min = 0;
+    int max = 255;
+    int value = 0;
+
+
     while (1) {
+
         motor.loopFOC();
 
         if (fabs(detents) < 0.01) {
             motor.move(0);
             display_task_.set_angle(motor.shaft_angle);
+            current_detent_center = motor.shaft_angle;
         } else {
-            float detent_angle = 2*PI/detents;
+            float detent_width = 2*PI/detents;
 
-            float error = (attract_angle - motor.shaft_angle);
-            motor.move(motor.PID_velocity(error));
+            // if (millis() - last > 500) {
+            //     last = millis();
+            //     current_detent_center += detent_width;
+            // }
 
-            attract_angle = round(motor.shaft_angle/detent_angle)*detent_angle;
-            display_task_.set_angle(attract_angle);
+            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;
+                angle_to_detent_center -= detent_width;
+                value--;
+            } else if (angle_to_detent_center < -detent_width * 1.2 && value < max) {
+                current_detent_center -= detent_width;
+                angle_to_detent_center += detent_width;
+                value++;
+            }
+
+            float dead_zone = 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) {
+                motor.move(0);
+            } else {
+                motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone));
+            }
+
+            display_task_.set_angle(-value * PI / 180);
         }
         motor.monitor();
         command.run();
diff --git a/firmware/src/tlv_sensor.cpp b/firmware/src/tlv_sensor.cpp
index b90fe12..4ece47c 100644
--- a/firmware/src/tlv_sensor.cpp
+++ b/firmware/src/tlv_sensor.cpp
@@ -1,6 +1,6 @@
 #include "tlv_sensor.h"
 
-static const float ALPHA = 0.1;
+static const float ALPHA = 0.05;
 
 TlvSensor::TlvSensor() {}
 
@@ -12,9 +12,13 @@ void TlvSensor::init() {
 }
 
 float TlvSensor::getSensorAngle() {
-    tlv_.updateData();
-    x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
-    y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
+    uint32_t now = micros();
+    if (now - last_update_ > 100) {
+      tlv_.updateData();
+      x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
+      y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
+      last_update_ = now;
+    }
     float rad = atan2f(y_, x_);
     if (rad < 0) {
         rad += 2*PI;
diff --git a/firmware/src/tlv_sensor.h b/firmware/src/tlv_sensor.h
index cbcbb71..c99be1c 100644
--- a/firmware/src/tlv_sensor.h
+++ b/firmware/src/tlv_sensor.h
@@ -20,4 +20,5 @@ class TlvSensor : public Sensor {
         Tlv493d tlv_ = Tlv493d();
         float x_;
         float y_;
+        uint32_t last_update_;
 };