commit 6182f8d0760be10cf5372125d1d29c2e4fb5d1ca
author Austin Schuh <austin@peloton-tech.com> Sat Feb 14 22:15:04 2015 -0800
committer Austin Schuh <austin@peloton-tech.com> Sat Feb 14 22:15:04 2015 -0800
tree 6033bd1bccacb323da26886215ac95f1ce915707
parent dbd6bfa92c5bedb3ce183b972fd38cbdb0402d22

Fixed auto enabled -> teleop transition.

Change-Id: I952658b388ce4e701de4f2389839fa9971d5e5e2

frc971/joystick_reader.cc

diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 5ca4fa1..0f781a9 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -98,30 +98,18 @@
         was_running_(false) {}
 
   virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
-    if (data.GetControlBit(ControlBit::kAutonomous)) {
-      if (data.PosEdge(ControlBit::kEnabled)){
-        LOG(INFO, "Starting auto mode\n");
-        ::frc971::autonomous::autonomous.MakeWithBuilder()
-            .run_auto(true)
-            .Send();
-      } else if (data.NegEdge(ControlBit::kEnabled)) {
-        LOG(INFO, "Stopping auto mode\n");
-        ::frc971::autonomous::autonomous.MakeWithBuilder()
-            .run_auto(false)
-            .Send();
-      } else if (!data.GetControlBit(ControlBit::kEnabled)) {
-        {
-          auto goal = drivetrain_queue.goal.MakeMessage();
-          goal->Zero();
-          goal->control_loop_driving = false;
-          goal->left_goal = goal->right_goal = 0;
-          goal->left_velocity_goal = goal->right_velocity_goal = 0;
-          if (!goal.Send()) {
-            LOG(WARNING, "sending 0 drivetrain goal failed\n");
-          }
-        }
+    bool last_auto_running = auto_running_;
+    auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+                    data.IsPressed(ControlBit::kEnabled);
+    if (auto_running_ != last_auto_running) {
+      if (auto_running_) {
+        StartAuto();
+      } else {
+        StopAuto();
       }
-    } else {
+    }
+
+    if (!data.GetControlBit(ControlBit::kAutonomous)) {
       HandleTeleop(data);
     }
   }
@@ -197,7 +185,7 @@
     HandleDrivetrain(data);
     if (!data.GetControlBit(ControlBit::kEnabled)) {
       action_queue_.CancelAllActions();
-        LOG(DEBUG, "Canceling\n");
+      LOG(DEBUG, "Canceling\n");
     }
 
     action_queue_.Tick();
@@ -205,9 +193,21 @@
   }
 
  private:
+  void StartAuto() {
+    LOG(INFO, "Starting auto mode\n");
+    ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
+  }
+
+  void StopAuto() {
+    LOG(INFO, "Stopping auto mode\n");
+    ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
+  }
+
   bool is_high_gear_;
   bool was_running_;
 
+  bool auto_running_ = false;
+
   ActionQueue action_queue_;
 
   ::aos::util::SimpleLogInterval no_drivetrain_status_ =