Add button for resetting the localizer to HP slot
Change-Id: Ic3d7ea4304c8e732ea62d9b9ce1574b41ba5a87e
diff --git a/y2019/BUILD b/y2019/BUILD
index 7c30dad..25aa3b1 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -128,6 +128,7 @@
"//frc971/autonomous:auto_queue",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:localizer_queue",
"//y2019/control_loops/drivetrain:drivetrain_base",
"//y2019/control_loops/superstructure:superstructure_queue",
],
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 5a4264d..dad667f 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -33,6 +33,7 @@
localizer_(dt_config, &robot_pose_) {
localizer_.ResetInitialState(::aos::monotonic_clock::now(),
Localizer::State::Zero(), localizer_.P());
+ ResetPosition(0.5, 3.4, 0.0);
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 3c2cf4e..aaf377c 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -35,7 +35,16 @@
void Reset(const Localizer::State &state);
void ResetPosition(double x, double y, double theta) override {
- Reset((Localizer::State() << x, y, theta, 0, 0, 0, 0, 0, 0, 0).finished());
+ Localizer::State new_state = localizer_.X_hat();
+ new_state.x() = x;
+ new_state.y() = y;
+ new_state(2, 0) = theta;
+ new_state(4, 0) = 0.0;
+ new_state(6, 0) = 0.0;
+ new_state(7, 0) = 0.0;
+ new_state(8, 0) = 0.0;
+ new_state(9, 0) = 0.0;
+ Reset(new_state);
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 76ded48..7f9fe6c 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -15,12 +15,14 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/localizer.q.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/status_light.q.h"
using ::y2019::control_loops::superstructure::superstructure_queue;
+using ::frc971::control_loops::drivetrain::localizer_control;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -64,6 +66,7 @@
const ButtonLocation kPanelHPIntakeBackward(5, 5);
const ButtonLocation kRelease(2, 4);
+const ButtonLocation kResetLocalizer(4, 3);
const ElevatorWristPosition kStowPos{0.36, 0.0};
@@ -122,6 +125,16 @@
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ if (data.PosEdge(kResetLocalizer)) {
+ auto localizer_resetter = localizer_control.MakeMessage();
+ localizer_resetter->x = 0.4;
+ localizer_resetter->y = 3.4;
+ localizer_resetter->theta = 0.0;
+ if (!localizer_resetter.Send()) {
+ LOG(ERROR, "Failed to reset localizer.\n");
+ }
+ }
+
if (data.IsPressed(kSuctionBall)) {
grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {