Shoot action stops shooting when the shot has gone out.
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index 5a11582..b1e27b4 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -20,13 +20,46 @@
// scale speed to a [0.0-1.0] on something close to the max
return (speed / values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
}
-
void ShootAction::RunAction() {
+ InnerRunAction();
+
+ // Now do our 'finally' block and make sure that we aren't requesting shots
+ // continually.
+ control_loops::shooter_queue_group.goal.FetchLatest();
+ if (control_loops::shooter_queue_group.goal.get() == nullptr) {
+ return;
+ }
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder()
+ .shot_power(control_loops::shooter_queue_group.goal->shot_power)
+ .shot_requested(false)
+ .unload_requested(false)
+ .load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+}
+
+void ShootAction::InnerRunAction() {
LOG(INFO, "Shooting at the original angle and power.\n");
// wait for claw to be ready
if (WaitUntil(::std::bind(&ShootAction::DoneSetupShot, this))) return;
+ // Turn the intake off.
+ control_loops::claw_queue_group.goal.FetchLatest();
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
+ .intake(0.0)
+ .centering(0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
// Make sure that we have the latest shooter status.
control_loops::shooter_queue_group.status.FetchLatest();
// Get the number of shots fired up to this point. This should not be updated
diff --git a/frc971/actions/shoot_action.h b/frc971/actions/shoot_action.h
index 884c23a..6d29ca7 100644
--- a/frc971/actions/shoot_action.h
+++ b/frc971/actions/shoot_action.h
@@ -15,6 +15,7 @@
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
virtual void RunAction();
+ void InnerRunAction();
// calc an offset to our requested shot based on robot speed
double SpeedToAngleOffset(double speed);