commit 17e484ec90eb95db8b53daa25f37c6ed26b1a6d6
author Austin Schuh <austin.linux@gmail.com> Sun Mar 11 01:11:36 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 21 20:12:23 2018 -0700
tree 868fcde3955fdc34b0bf734bef0a3b5296ab87af
parent 0aae92411895d4e5385ba22097284d012b714523

Add hanger code.

This adds the goal nodes for the hanger, and the ability to hang.

Change-Id: Ib33c19298cd08e98abf27d9789982d78f8df191e

y2018/joystick_reader.cc

diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index bb6c1b1..a5eaf14 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -52,6 +52,11 @@
 const ButtonLocation kArmBackLowBox(4, 12);
 const ButtonLocation kArmBackSwitch(3, 12);
 
+const ButtonLocation kArmAboveHang(3, 7);
+const ButtonLocation kArmBelowHang(3, 2);
+
+const ButtonLocation kWinch(3, 5);
+
 const ButtonLocation kArmNeutral(3, 13);
 const ButtonLocation kArmUp(3, 9);
 
@@ -96,7 +101,6 @@
 
   void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
     drivetrain_input_reader_->HandleDrivetrain(data);
-    robot_velocity_ = drivetrain_input_reader_->robot_velocity();
   }
 
   void HandleTeleop(const ::aos::input::driver_station::Data &data) {
@@ -177,8 +181,33 @@
       arm_goal_position_ = arm::BackLowBoxIndex();
     }  else if (data.IsPressed(kArmBackSwitch)) {
       arm_goal_position_ = arm::BackSwitchIndex();
+    } else if (data.IsPressed(kArmAboveHang)) {
+      if (data.IsPressed(kIntakeIn)) {
+        arm_goal_position_ = arm::SelfHangIndex();
+      } else if (data.IsPressed(kIntakeOut)) {
+        arm_goal_position_ = arm::PartnerHangIndex();
+      } else {
+        arm_goal_position_ = arm::AboveHangIndex();
+      }
+    } else if (data.IsPressed(kArmBelowHang)) {
+      arm_goal_position_ = arm::BelowHangIndex();
     }
 
+    new_superstructure_goal->deploy_fork =
+        data.IsPressed(kArmAboveHang) && data.IsPressed(kClawOpen);
+
+    if (new_superstructure_goal->deploy_fork) {
+      intake_goal_ = -2.0;
+    }
+
+    if (data.IsPressed(kWinch)) {
+      new_superstructure_goal->voltage_winch = 12.0;
+    } else {
+      new_superstructure_goal->voltage_winch = 0.0;
+    }
+
+    new_superstructure_goal->hook_release = data.IsPressed(kArmBelowHang);
+
     new_superstructure_goal->arm_goal_position = arm_goal_position_;
 
     if (data.IsPressed(kClawOpen) || data.PosEdge(kArmPickupBoxFromIntake)) {
@@ -222,13 +251,11 @@
   }
 
   // Current goals to send to the robot.
-  double intake_goal_ = -M_PI * 2.0 / 3.0;
+  double intake_goal_ = 0.0;
 
   bool was_running_ = false;
   bool auto_running_ = false;
 
-  double robot_velocity_ = 0.0;
-
   int arm_goal_position_ = 0;
 
   ::aos::common::actions::ActionQueue action_queue_;