Added a Catch action based on the SelfCatch.
diff --git a/frc971/actions/catch_action.cc b/frc971/actions/catch_action.cc
new file mode 100644
index 0000000..7057955
--- /dev/null
+++ b/frc971/actions/catch_action.cc
@@ -0,0 +1,133 @@
+#include <functional>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/catch_action.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/queues/othersensors.q.h"
+
+namespace frc971 {
+namespace actions {
+
+CatchAction::CatchAction(actions::CatchActionGroup* s)
+ : actions::ActionBase<actions::CatchActionGroup>(s) {}
+
+void CatchAction::RunAction() {
+
+ // Set claw angle.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ catch_action.goal->catch_angle)
+ .separation_angle(kCatchSeparation).intake(kCatchIntake)
+ .centering(kCatchCentering).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&CatchAction::DoneSetupCatch, this))) return;
+
+ // Set claw angle.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ catch_action.goal->catch_angle).separation_angle(kCatchSeparation)
+ .intake(kCatchIntake).centering(kCatchCentering).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // wait for the sonar to trigger
+ if (WaitUntil(::std::bind(&CatchAction::DoneFoundSonar, this))) return;
+
+ // close the claw
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ kFinishAngle).separation_angle(0.0).intake(kCatchIntake)
+ .centering(kCatchCentering).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // claw now closed
+ if (WaitUntil(::std::bind(&CatchAction::DoneClawWithBall, this))) return;
+ // ball is fully in
+ if (WaitUntil(::std::bind(&CatchAction::DoneBallIn, this))) return;
+
+ // head to a finshed pose
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ kFinishAngle)
+ .separation_angle(kFinishAngle).intake(0.0).centering(0.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // thats it
+ if (WaitUntil(::std::bind(&CatchAction::DoneClawWithBall, this))) return;
+
+ // done with action
+ return;
+}
+
+
+bool CatchAction::DoneBallIn() {
+ if (!sensors::othersensors.FetchLatest()) {
+ sensors::othersensors.FetchNextBlocking();
+ }
+ if (sensors::othersensors->travis_hall_effect_distance > 0.005) {
+ LOG(INFO, "Ball in at %.2f.\n",
+ sensors::othersensors->travis_hall_effect_distance);
+ return true;
+ }
+ return false;
+}
+
+bool CatchAction::DoneClawWithBall() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ if (control_loops::claw_queue_group.status->done_with_ball) {
+ LOG(INFO, "Claw at goal.\n");
+ return true;
+ }
+ return false;
+}
+
+bool CatchAction::DoneFoundSonar() {
+ if (!sensors::othersensors.FetchLatest()) {
+ sensors::othersensors.FetchNextBlocking();
+ }
+ if (sensors::othersensors->sonar_distance > 0.3 &&
+ sensors::othersensors->sonar_distance < kSonarTriggerDist) {
+ LOG(INFO, "Hit Sonar at %.2f.\n", sensors::othersensors->sonar_distance);
+ return true;
+ }
+ return false;
+}
+
+bool CatchAction::DoneSetupCatch() {
+ if (!control_loops::claw_queue_group.status.FetchLatest()) {
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ }
+ if (!control_loops::claw_queue_group.goal.FetchLatest()) {
+ LOG(ERROR, "Failed to fetch claw goal.\n");
+ }
+ // Make sure that the shooter and claw has reached the necessary state.
+ // Check the current positions of the various mechanisms to make sure that we
+ // avoid race conditions where we send it a new goal but it still thinks that
+ // it has the old goal and thinks that it is already done.
+ bool claw_angle_correct =
+ ::std::abs(control_loops::claw_queue_group.status->bottom -
+ control_loops::claw_queue_group.goal->bottom_angle) <
+ 0.005;
+
+ if (control_loops::claw_queue_group.status->done && claw_angle_correct) {
+ LOG(INFO, "Claw ready for catching.\n");
+ return true;
+ }
+
+ return false;
+}
+
+} // namespace actions
+} // namespace frc971
+