commit 94596ddee957923a2b5f59b01534fb44fdb6cdb9
author Austin Schuh <austin.linux@gmail.com> Sun Mar 13 21:41:26 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 16 22:08:08 2016 -0700
tree 26bc46b2ad52657a1818941f5b1d90b36a7646a0
parent 2ccf8c58e261654d5f05c06c2528067107eb45c8

Shift into the default when disabled.

Change-Id: Ia33923a9bece64df620c46c74a115c6cd165d191

y2016/joystick_reader.cc

diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index d987268..0bb19dc 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -65,7 +65,7 @@
 class Reader : public ::aos::input::JoystickInput {
  public:
   Reader()
-      : is_high_gear_(false),
+      : is_high_gear_(true),
         intake_goal_(0.0),
         shoulder_goal_(M_PI / 2.0),
         wrist_goal_(0.0) {}
@@ -82,7 +82,14 @@
       }
     }
 
-    if (!data.GetControlBit(ControlBit::kAutonomous)) {
+    if (!data.GetControlBit(ControlBit::kEnabled)) {
+      // If we are not enabled, reset the waiting for zero bit.
+      LOG(DEBUG, "Waiting for zero.\n");
+      waiting_for_zero_ = true;
+      is_high_gear_ = true;
+    }
+
+    if (!auto_running_) {
       HandleDrivetrain(data);
       HandleTeleop(data);
     }
@@ -139,13 +146,6 @@
       LOG(DEBUG, "Canceling\n");
     }
 
-    if (data.PosEdge(ControlBit::kEnabled)) {
-      // If we got enabled, wait for everything to zero.
-      LOG(INFO, "Waiting for zero.\n");
-      waiting_for_zero_ = true;
-      is_high_gear_ = true;
-    }
-
     superstructure_queue.status.FetchLatest();
     if (!superstructure_queue.status.get()) {
       LOG(ERROR, "Got no superstructure status packet.\n");
@@ -154,7 +154,7 @@
     if (superstructure_queue.status.get() &&
         superstructure_queue.status->zeroed) {
       if (waiting_for_zero_) {
-        LOG(INFO, "Zeroed! Starting teleop mode.\n");
+        LOG(DEBUG, "Zeroed! Starting teleop mode.\n");
         waiting_for_zero_ = false;
       }
     } else {