commit 0e86e249deb3421b5fb5dc939f9a4f27405df0c5
author Ravago Jones <ravagojones@gmail.com> Sat Feb 12 18:38:14 2022 -0800
committer Ravago Jones <ravagojones@gmail.com> Sun Feb 27 14:10:37 2022 -0800
tree 6cb124198af48017565cd9ca2eb8977ab9fc8fc6
parent 3dbf1c90692a9aaa38b7f42e73161590d7f249e7

Read heading and velocity pwm in wpilib_interface

Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: Ifeeed94b61ca03972debbba52ae1f36022d901a2

frc971/imu/ADIS16505.cc

diff --git a/frc971/imu/ADIS16505.cc b/frc971/imu/ADIS16505.cc
index 9ed77a9..d3fbf76 100644
--- a/frc971/imu/ADIS16505.cc
+++ b/frc971/imu/ADIS16505.cc
@@ -373,9 +373,7 @@
     yaw += yaw_rate * dt;
 
     // 50% is 0; -2000 deg/sec to 2000 deg/sec
-    uint16_t rate_level =
-        (std::clamp(yaw_rate, -2000.0, 2000.0) / 4000.0 + 0.5) * PWM_TOP;
-    pwm_set_gpio_level(RATE_PWM, rate_level);
+    double scaled_rate = (std::clamp(yaw_rate, -2000.0, 2000.0) / 4000.0 + 0.5);
 
     // 0 to 360
     double wrapped_heading = fmod(yaw, 360);
@@ -383,8 +381,20 @@
       wrapped_heading = wrapped_heading + 360;
     }
 
-    uint16_t heading_level = (int16_t)(wrapped_heading / 360.0 * PWM_TOP);
+    double scaled_heading = wrapped_heading / 360.0;
+
+    constexpr double kScaledRangeLow = 0.1;
+    constexpr double kScaledRangeHigh = 0.9;
+
+    uint16_t heading_level =
+        (scaled_heading * (kScaledRangeHigh - kScaledRangeLow) +
+         kScaledRangeLow) *
+        PWM_TOP;
+    uint16_t rate_level =
+        (scaled_rate * (kScaledRangeHigh - kScaledRangeLow) + kScaledRangeLow) *
+        PWM_TOP;
     pwm_set_gpio_level(HEADING_PWM, heading_level);
+    pwm_set_gpio_level(RATE_PWM, rate_level);
   }
 
   // if 5 or more consecutive checksums are zero, then something weird is going