Add cube pickup path
Turns out the link lengths in python didn't match C++... Sigh. I
updated all the trajectories to be equivilent to what they were before
the constant switch.
Change-Id: I9d29c51b6c6dc4ecc95b7182ac8bdd1949806daa
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 92f79d8..5cb148c 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -115,6 +115,12 @@
.game_piece = GamePiece::CONE_UP,
.button = kMidBackTipConeScoreLeft,
},
+ {
+ .index = arm::GroundPickupBackCubeIndex(),
+ .wrist_goal = 0.6,
+ .game_piece = GamePiece::CUBE,
+ .button = kGroundPickupCube,
+ },
};
class Reader : public ::frc971::input::ActionJoystickInput {
@@ -163,6 +169,10 @@
current_game_piece_ = GamePiece::CUBE;
}
+ if (current_game_piece_ == GamePiece::CUBE) {
+ wrist_goal = 0.6;
+ }
+
// Search for the active setpoint.
for (const ArmSetpoint &setpoint : setpoints) {
if (data.IsPressed(setpoint.button)) {