Add constant-heading localizer reset button
Change-Id: I25afc8985dbce2199f6fc4e7eb91c58b1e9bbc5d
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 766de6e..816a1df 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -87,10 +87,16 @@
const ButtonLocation kResetLocalizerRightForwards(3, 8);
const ButtonLocation kResetLocalizerRightBackwards(3, 7);
+const ButtonLocation kResetLocalizerLeft(3, 11);
+const ButtonLocation kResetLocalizerRight(3, 13);
+
const ButtonLocation kNearCargoHint(3, 3);
const ButtonLocation kMidCargoHint(3, 5);
const ButtonLocation kFarCargoHint(3, 6);
+const JoystickAxis kCargoSelectorY(5, 6);
+const JoystickAxis kCargoSelectorX(5, 5);
+
const ButtonLocation kCameraLog(3, 14);
const ElevatorWristPosition kStowPos{0.36, 0.0};
@@ -178,13 +184,47 @@
} else if (data.IsPressed(kFarCargoHint)) {
target_hint->suggested_target = 3;
} else {
- target_hint->suggested_target = 0;
+ const double cargo_joy_y = data.GetAxis(kCargoSelectorY);
+ const double cargo_joy_x = data.GetAxis(kCargoSelectorX);
+ if (cargo_joy_y > 0.5) {
+ target_hint->suggested_target = 3;
+ } else if (cargo_joy_y < -0.5) {
+ target_hint->suggested_target = 1;
+ } else if (::std::abs(cargo_joy_x) > 0.5) {
+ target_hint->suggested_target = 2;
+ } else {
+ target_hint->suggested_target = 0;
+ }
}
if (!target_hint.Send()) {
LOG(ERROR, "Failed to send target selector hint.\n");
}
}
+ if (data.PosEdge(kResetLocalizerLeft)) {
+ auto localizer_resetter = localizer_control.MakeMessage();
+ // Start at the left feeder station.
+ localizer_resetter->x = 0.6;
+ localizer_resetter->y = 3.4;
+ localizer_resetter->keep_current_theta = true;
+
+ if (!localizer_resetter.Send()) {
+ LOG(ERROR, "Failed to reset localizer.\n");
+ }
+ }
+
+ if (data.PosEdge(kResetLocalizerRight)) {
+ auto localizer_resetter = localizer_control.MakeMessage();
+ // Start at the left feeder station.
+ localizer_resetter->x = 0.6;
+ localizer_resetter->y = -3.4;
+ localizer_resetter->keep_current_theta = true;
+
+ if (!localizer_resetter.Send()) {
+ LOG(ERROR, "Failed to reset localizer.\n");
+ }
+ }
+
if (data.PosEdge(kResetLocalizerLeftForwards)) {
auto localizer_resetter = localizer_control.MakeMessage();
// Start at the left feeder station.