Actually shoot when auto tracking
We were waiting for the velocity goal to be nonzero. This wasn't
correctly handling autotrack, so we couldn't fire unless the shooter
button was pressed too.
Change-Id: Ib25a213ed95a0bb3990402eb693135c0ded951a5
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index 5fdac39..4eafe47 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -29,6 +29,7 @@
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
+ double goal() const { return last_goal_; }
// Sets the current encoder position in radians
void set_position(double current_position,
const aos::monotonic_clock::time_point position_timestamp);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index f72eeeb..1a46949 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -26,6 +26,9 @@
bool ready() { return ready_; }
+ float finisher_goal() const { return finisher_.goal(); }
+ float accelerator_goal() const { return accelerator_left_.goal(); }
+
private:
FlywheelController finisher_, accelerator_left_, accelerator_right_;
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 91e782e..9a48095 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -204,9 +204,8 @@
if (unsafe_goal) {
output_struct.washing_machine_spinner_voltage = 0.0;
if (unsafe_goal->shooting()) {
- if (shooter_.ready() &&
- unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
- unsafe_goal->shooter()->velocity_finisher() > 10.0) {
+ if (shooter_.ready() && shooter_.finisher_goal() > 10.0 &&
+ shooter_.accelerator_goal() > 10.0) {
output_struct.feeder_voltage = 12.0;
} else {
output_struct.feeder_voltage = 0.0;