Add constant-heading localizer reset button
Change-Id: I25afc8985dbce2199f6fc4e7eb91c58b1e9bbc5d
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 282ca9e..889133f 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -34,7 +34,7 @@
target_selector_(event_loop) {
localizer_.ResetInitialState(::aos::monotonic_clock::now(),
Localizer::State::Zero(), localizer_.P());
- ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0, 0.0);
+ ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0, 0.0, true);
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 9255109..183e817 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -36,13 +36,16 @@
void Reset(::aos::monotonic_clock::time_point t,
const Localizer::State &state, double theta_uncertainty);
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta, double theta_uncertainty) override {
+ double theta, double theta_uncertainty,
+ bool reset_theta) override {
// When we reset the state, we want to keep the encoder positions intact, so
// we copy from the original state and reset everything else.
Localizer::State new_state = localizer_.X_hat();
new_state.x() = x;
new_state.y() = y;
- new_state(2, 0) = theta;
+ if (reset_theta) {
+ new_state(2, 0) = theta;
+ }
// Velocity terms.
new_state(4, 0) = 0.0;
new_state(6, 0) = 0.0;
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.