Add setpoint for testing auto panel from robot
Change-Id: Idde3f2be939441f32eee757a2d0d3fb08a9acc61
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 76ded48..19e1908 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -65,6 +65,12 @@
const ButtonLocation kRelease(2, 4);
+const ButtonLocation kAutoPanel(3, 10);
+const ButtonLocation kAutoPanelIntermediate(4, 6);
+
+const ElevatorWristPosition kAutoPanelPos{0.0, -M_PI / 2.0};
+const ElevatorWristPosition kAutoPanelIntermediatePos{0.34, -M_PI / 2.0};
+
const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.04, M_PI / 2.0};
@@ -175,6 +181,12 @@
stilts_was_above_ = false;
}
+ if (data.IsPressed(kAutoPanel)) {
+ elevator_wrist_pos_ = kAutoPanelPos;
+ } else if (data.IsPressed(kAutoPanelIntermediate)) {
+ elevator_wrist_pos_ = kAutoPanelIntermediatePos;
+ }
+
if (switch_ball_) {
if (superstructure_queue.status->has_piece) {
new_superstructure_goal->wrist.profile_params.max_acceleration = 20;