Add constant-heading localizer reset button
Change-Id: I25afc8985dbce2199f6fc4e7eb91c58b1e9bbc5d
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2af6765..dd6b05c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -243,10 +243,11 @@
// simulation.
if (localizer_control_fetcher_.Fetch()) {
LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
- localizer_->ResetPosition(monotonic_now, localizer_control_fetcher_->x,
- localizer_control_fetcher_->y,
- localizer_control_fetcher_->theta,
- localizer_control_fetcher_->theta_uncertainty);
+ localizer_->ResetPosition(
+ monotonic_now, localizer_control_fetcher_->x,
+ localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
+ localizer_control_fetcher_->theta_uncertainty,
+ !localizer_control_fetcher_->keep_current_theta);
}
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
monotonic_now, position->left_encoder,
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index bd48315..23a978c 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -50,8 +50,8 @@
double gyro_rate, double longitudinal_accelerometer) = 0;
// Reset the absolute position of the estimator.
virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
- double y, double theta,
- double theta_uncertainty) = 0;
+ double y, double theta, double theta_uncertainty,
+ bool reset_theta) = 0;
// There are several subtly different norms floating around for state
// matrices. In order to avoid that mess, we jus tprovide direct accessors for
// the values that most people care about.
@@ -110,7 +110,8 @@
}
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta, double /*theta_override*/) override {
+ double theta, double /*theta_override*/,
+ bool /*reset_theta*/) override {
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
index 8fef686..b1169a9 100644
--- a/frc971/control_loops/drivetrain/localizer.q
+++ b/frc971/control_loops/drivetrain/localizer.q
@@ -7,6 +7,7 @@
float y; // Y position, meters
float theta; // heading, radians
double theta_uncertainty; // Uncertainty in theta.
+ bool keep_current_theta; // Whether to keep the current theta value.
};
queue LocalizerControl localizer_control;
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.