commit 4a8d492db67a93ff25b26e6367628f25768b3dee
author Austin Schuh <austin.linux@gmail.com> Sun Apr 07 15:31:30 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Mon Apr 08 21:17:36 2019 -0700
tree 0493c38e8951295e17a5277f02efd93b68fb5800
parent fbbf0f0c711ef998708bdb38cb83f61396f92c2e

Calibrate based on pistol grip id

Convert a lot of the calibration to more meaningful floats as well so
it's easier to calibrate.

Change-Id: If0076f3020c03603b51c60c54da29000410f3d58

motors/pistol_grip/controller.cc

diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index c0ef677..52bba6d 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -197,10 +197,15 @@
 }
 
 float absolute_wheel(float wheel_position) {
-  if (wheel_position < 0.43f) {
+  const float kCenterOffset = (ProcessorIndex() == 1) ? -0.683f : -0.935f;
+
+  wheel_position += kCenterOffset;
+
+  if (wheel_position > 0.5f) {
+    wheel_position -= 1.0f;
+  } else if (wheel_position < -0.5f) {
     wheel_position += 1.0f;
   }
-  wheel_position -= 0.462f + 0.473f;
   return wheel_position;
 }
 
@@ -1006,15 +1011,34 @@
   const float motor1_offset_scaled = analog_ratio(motor1_offset);
   // Good for the initial trigger.
   {
-    constexpr float kZeroOffset0 = 0.27f;
+    // Calibrate winding phase error here.
+    const float kZeroOffset0 = ProcessorIndex() == 1 ? 0.275f : 0.27f;
     const int motor0_starting_point = static_cast<int>(
         (motor0_offset_scaled + (kZeroOffset0 / 7.0f)) * 4096.0f);
     printf("Motor 0 starting at %d\n", motor0_starting_point);
     motor0.set_encoder_calibration_offset(motor0_starting_point);
     motor0.set_encoder_multiplier(-1);
 
-    // Calibrate neutral here.
-    motor0.set_encoder_offset(motor0.encoder_offset() - 2065 + 20);
+    // Calibrate output coordinate neutral here.
+    motor0.set_encoder_offset(
+        motor0.encoder_offset() +
+        (ProcessorIndex() == 1 ? (-3096 - 430 - 30 - 6) : (-2065 + 20)));
+
+    while (true) {
+      const uint32_t encoder =
+          global_motor0.load(::std::memory_order_relaxed)->wrapped_encoder();
+      const int32_t absolute_encoder =
+          global_motor0.load(::std::memory_order_relaxed)
+              ->absolute_encoder(encoder);
+
+      if (absolute_encoder > 2047) {
+        motor0.set_encoder_offset(motor0.encoder_offset() - 4096);
+      } else if (absolute_encoder < -2047) {
+        motor0.set_encoder_offset(motor0.encoder_offset() + 4096);
+      } else {
+        break;
+      }
+    }
 
     uint32_t new_encoder = motor0.wrapped_encoder();
     int32_t absolute_encoder = motor0.absolute_encoder(new_encoder);
@@ -1023,7 +1047,7 @@
   }
 
   {
-    constexpr float kZeroOffset1 = 0.26f;
+    const float kZeroOffset1 = ProcessorIndex() == 1 ? 0.420f : 0.26f;
     const int motor1_starting_point = static_cast<int>(
         (motor1_offset_scaled + (kZeroOffset1 / 7.0f)) * 4096.0f);
     printf("Motor 1 starting at %d\n", motor1_starting_point);
@@ -1038,7 +1062,8 @@
            static_cast<int>(wheel_position * 1000.0f), encoder);
 
     constexpr float kWheelGearRatio = (1.25f + 0.02f) / 0.35f;
-    constexpr float kWrappedWheelAtZero = 0.6586310546875f;
+    const float kWrappedWheelAtZero =
+        ProcessorIndex() == 1 ? (0.934630859375f) : 0.6586310546875f;
 
     const int encoder_wraps =
         static_cast<int>(lround(wheel_position * kWheelGearRatio -