commit c6329344ad8c2b8583c638c72c4b029c7015ef20
author Sabina Davis <sabina1109@gmail.com> Fri Mar 01 20:44:42 2019 -0800
committer Sabina Davis <sabina1109@gmail.com> Wed Mar 06 23:41:32 2019 -0800
tree 460c3c43738da678337fd91a0a2e20d96e51ec0d
parent 35c1eca2f40b6bdfac3845fe12439a3209ccef1d

Added status light and logic.

Change-Id: Idf192f1e6af3cb4f8a8e659fce5269cbd78bd4bd

y2019/joystick_reader.cc

diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9290a2f..887ac5e 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -18,6 +18,7 @@
 
 #include "y2019/control_loops/drivetrain/drivetrain_base.h"
 #include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/status_light.q.h"
 
 using ::y2019::control_loops::superstructure::superstructure_queue;
 using ::aos::input::driver_station::ButtonLocation;
@@ -102,8 +103,7 @@
             ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
     superstructure_queue.goal.FetchLatest();
     if (superstructure_queue.goal.get()) {
-      top_ = superstructure_queue.goal->suction.top;
-      bottom_ = superstructure_queue.goal->suction.bottom;
+      grab_piece_ = superstructure_queue.goal->suction.grab_piece;
     }
   }
 
@@ -119,13 +119,12 @@
     auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
 
     if (data.IsPressed(kSuctionBall)) {
-      Ball();
+      grab_piece_ = true;
     } else if (data.IsPressed(kSuctionHatch)) {
-      Disc();
+      grab_piece_ = true;
     } else if (data.IsPressed(kRelease) ||
                !superstructure_queue.status->has_piece) {
-      top_ = false;
-      bottom_ = false;
+      grab_piece_ = false;
     }
 
     if (data.IsPressed(kRocketBackwardUnpressed)) {
@@ -175,10 +174,10 @@
 
       // Go to intake position and apply vacuum
       if (data.IsPressed(kBallHPIntakeForward)) {
-        Ball();
+        grab_piece_ = true;
         elevator_wrist_pos_ = kBallHPIntakeForwardPos;
       } else if (data.IsPressed(kBallHPIntakeBackward)) {
-        Ball();
+        grab_piece_ = true;
         elevator_wrist_pos_ = kBallHPIntakeBackwardPos;
       }
 
@@ -203,10 +202,10 @@
       }
     } else {
       if (data.IsPressed(kPanelHPIntakeForward)) {
-        Disc();
+        grab_piece_ = true;
         elevator_wrist_pos_ = kPanelHPIntakeForwrdPos;
       } else if (data.IsPressed(kPanelHPIntakeBackward)) {
-        Disc();
+        grab_piece_ = true;
         elevator_wrist_pos_ = kPanelHPIntakeBackwardPos;
       }
 
@@ -236,7 +235,7 @@
       if (kDoBallIntake && !superstructure_queue.status->has_piece) {
         elevator_wrist_pos_ = kBallIntakePos;
         new_superstructure_goal->roller_voltage = 9.0;
-        Ball();
+        grab_piece_ = true;
       } else {
         if (kDoBallOutake) {
           new_superstructure_goal->roller_voltage = -6.0;
@@ -248,12 +247,16 @@
     }
 
     if (data.IsPressed(kRelease)) {
-      top_ = false;
-      bottom_ = false;
+      grab_piece_ = false;
     }
 
-    new_superstructure_goal->suction.top = top_;
-    new_superstructure_goal->suction.bottom = bottom_;
+    if (switch_ball_) {
+      new_superstructure_goal->suction.gamepiece_mode = 0;
+    } else {
+      new_superstructure_goal->suction.gamepiece_mode = 1;
+    }
+
+    new_superstructure_goal->suction.grab_piece = grab_piece_;
 
     new_superstructure_goal->elevator.unsafe_goal =
         elevator_wrist_pos_.elevator;
@@ -265,20 +268,10 @@
     }
   }
 
-  void Disc() {
-    top_ = true;
-    bottom_ = true;
-  }
-  void Ball() {
-    top_ = false;
-    bottom_ = true;
-  }
-
  private:
   // Current goals here.
   ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
-  bool top_ = false;
-  bool bottom_ = false;
+  bool grab_piece_ = false;
 
   bool switch_ball_ = false;
   bool stilts_was_above_ = false;