commit 6260c09364620cecec0f169c79704b58b5877db1
author Brian Silverman <bsilver16384@gmail.com> Sun Jan 14 15:21:36 2018 -0800
committer Brian Silverman <bsilver16384@gmail.com> Sat Feb 03 22:33:51 2018 -0500
tree 2335333106884c9a437f72911401c5753050a311
parent 587edcc2c659923c17505e9f4676a415d4f8b4da

Finish the pistol grip code

Change-Id: I95c03a95ac0ec64b4314ec310ad6535176b1d529

motors/motor.cc

diff --git a/motors/motor.cc b/motors/motor.cc
index d966e6a..af7a3c7 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -10,6 +10,7 @@
 #include "motors/peripheral/can.h"
 
 extern "C" float analog_ratio(uint16_t reading);
+extern "C" float absolute_wheel(uint16_t reading);
 
 namespace frc971 {
 namespace salsa {
@@ -194,7 +195,7 @@
   // If we fire phase 2, we should go to -2.0 * PI / 3.0 radians.
   // If we fire phase 3, we should go to 2.0 * PI / 3.0 radians.
   // These numbers were confirmed by the python motor simulation.
-  static int phase_to_fire_count = -500000;
+  static int phase_to_fire_count = -300000;
   static int phase_to_fire = 0;
   ++phase_to_fire_count;
   if (phase_to_fire_count > 200000) {
@@ -217,7 +218,8 @@
   output_registers_[2][0] = 0;
   output_registers_[2][2] = phase_to_fire == 2 ? kPhaseFireWidth : 0;
 #endif
-
+  (void)balanced;
+  (void)captured_wrapped_encoder;
 #if PRINT_READINGS
   static int i = 0;
   if (i == 1000) {
@@ -227,12 +229,19 @@
       DisableInterrupts disable_interrupts;
       readings = AdcReadSmallInit(disable_interrupts);
     }
+    //printf(
+        //"enc %" PRIu32 " %d %d\n", captured_wrapped_encoder,
+        //static_cast<int>((1.0f - analog_ratio(readings.motor1_abs)) * 7000.0f),
+        //static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f));
+    float wheel_position = absolute_wheel(analog_ratio(readings.wheel_abs));
+
     printf(
-        "enc %d %d %d\n", captured_wrapped_encoder,
-        static_cast<int>((1.0f - analog_ratio(readings.motor0_abs)) * 7000.0f),
-        static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f));
+        "ecnt %" PRIu32 " arev:%d erev:%d %d\n", captured_wrapped_encoder,
+        static_cast<int>((analog_ratio(readings.motor1_abs)) * 7000.0f),
+        static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f),
+        static_cast<int>(wheel_position * 1000.0f));
   } else if (i == 200) {
-#ifdef DO_CONTROLS
+#if DO_CONTROLS
     printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", switching_points[0],
            switching_points[1], switching_points[2]);
 #else