Merge "Add setpoint for testing auto panel from robot"
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 11b8a77..b08d6b6 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -75,6 +75,12 @@
const ButtonLocation kRelease(2, 4);
const ButtonLocation kResetLocalizer(4, 3);
+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};
@@ -198,6 +204,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;