Lower elevator when climbing to stay legal
Change-Id: I8479be9ad9368df1715ad51af94fa5dc55336e00
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 69b40f5..7b714be 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -89,6 +89,7 @@
const ButtonLocation kFarCargoHint(4, 2);
const ElevatorWristPosition kStowPos{0.36, 0.0};
+const ElevatorWristPosition kClimbPos{0.0, M_PI / 4.0};
const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.01, M_PI / 2.0};
const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.015, -M_PI / 2.0};
@@ -355,6 +356,10 @@
new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
}
+ if (superstructure_queue.status->stilts.position > 0.3) {
+ elevator_wrist_pos_ = kClimbPos;
+ }
+
if (superstructure_queue.status->stilts.position > kDeployStiltPosition &&
new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;