commit ffff93c3b2c4bf883f6a1ffdd8332761e0cd3168
author Filip Kujawa <filip.j.kujawa@gmail.com> Tue Nov 14 21:33:26 2023 -0800
committer Filip Kujawa <filip.j.kujawa@gmail.com> Wed Nov 15 22:24:15 2023 -0800
tree 2d37a85d0f6d8e675534ae2ac3d9fe1a5676524e
parent 9435b5c9175fafc69a17aa9343a3fbd93ce12da6

Add 3rd Robot joystick reader

Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I7e36aca59b1be268738e5dc1b5d0e0e121b18675

y2023_bot3/joystick_reader.cc

diff --git a/y2023_bot3/joystick_reader.cc b/y2023_bot3/joystick_reader.cc
index 2e874d3..f9f03b7 100644
--- a/y2023_bot3/joystick_reader.cc
+++ b/y2023_bot3/joystick_reader.cc
@@ -31,6 +31,8 @@
 using frc971::input::driver_station::JoystickAxis;
 using frc971::input::driver_station::POVLocation;
 using Side = frc971::control_loops::drivetrain::RobotSide;
+using y2023_bot3::control_loops::superstructure::PivotGoal;
+using y2023_bot3::control_loops::superstructure::RollerGoal;
 
 namespace y2023_bot3 {
 namespace input {
@@ -42,14 +44,24 @@
   ButtonLocation button;
 };
 
+namespace {
+// XBox controller
+const ButtonLocation kSpit(3, 1);          // A
+const ButtonLocation kSpitHigh(3, 4);      // Y
+const ButtonLocation kPickup(3, 8);        // M4
+const ButtonLocation kPickupBack(3, 7);    // M3
+const ButtonLocation kScore(3, 9);         // M1
+const ButtonLocation kScoreBack(3, 10);    // M2
+const ButtonLocation kScoreMid(3, 5);      // LB
+const ButtonLocation kScoreMidBack(3, 6);  // RB
+}  // namespace
 class Reader : public ::frc971::input::ActionJoystickInput {
  public:
   Reader(::aos::EventLoop *event_loop)
       : ::frc971::input::ActionJoystickInput(
             event_loop,
             ::y2023_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
-            ::frc971::input::DrivetrainInputReader::InputType::kPistol,
-            {.use_redundant_joysticks = true}),
+            ::frc971::input::DrivetrainInputReader::InputType::kPistol, {}),
         superstructure_goal_sender_(
             event_loop->MakeSender<superstructure::Goal>("/superstructure")),
         superstructure_status_fetcher_(
@@ -77,6 +89,32 @@
 
       superstructure::Goal::Builder superstructure_goal_builder =
           builder.MakeBuilder<superstructure::Goal>();
+
+      RollerGoal roller_goal = RollerGoal::IDLE;
+      PivotGoal pivot_goal = PivotGoal::NEUTRAL;
+
+      if (data.IsPressed(kSpit)) {
+        roller_goal = RollerGoal::SPIT;
+      }
+
+      if (data.IsPressed(kScore)) {
+        pivot_goal = PivotGoal::SCORE_LOW_FRONT;
+      } else if (data.IsPressed(kScoreBack)) {
+        pivot_goal = PivotGoal::SCORE_LOW_BACK;
+      } else if (data.IsPressed(kScoreMid)) {
+        pivot_goal = PivotGoal::SCORE_MID_FRONT;
+      } else if (data.IsPressed(kScoreMidBack)) {
+        pivot_goal = PivotGoal::SCORE_MID_BACK;
+      } else if (data.IsPressed(kPickup)) {
+        pivot_goal = PivotGoal::PICKUP_FRONT;
+        roller_goal = RollerGoal::INTAKE_CUBE;
+      } else if (data.IsPressed(kPickupBack)) {
+        pivot_goal = PivotGoal::PICKUP_BACK;
+        roller_goal = RollerGoal::INTAKE_CUBE;
+      }
+
+      superstructure_goal_builder.add_roller_goal(roller_goal);
+      superstructure_goal_builder.add_pivot_goal(pivot_goal);
       if (builder.Send(superstructure_goal_builder.Finish()) !=
           aos::RawSender::Error::kOk) {
         AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
@@ -106,4 +144,4 @@
   event_loop.Run();
 
   return 0;
-}
+}
\ No newline at end of file