Added a Catch action based on the SelfCatch.
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index 41972a9..c38c65b 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -31,6 +31,21 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'catch_action_queue',
+ 'type': 'static_library',
+ 'sources': ['catch_action.q'],
+ 'variables': {
+ 'header_path': 'frc971/actions',
+ },
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:queues',
+ ],
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'action',
'type': 'static_library',
'dependencies': [
@@ -80,6 +95,22 @@
],
},
{
+ 'target_name': 'catch_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'catch_action.cc',
+ ],
+ 'dependencies': [
+ 'catch_action_queue',
+ 'action',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ ],
+ },
+ {
'target_name': 'shoot_action',
'type': 'executable',
'sources': [
@@ -105,5 +136,18 @@
'action',
],
},
+ {
+ 'target_name': 'catch_action',
+ 'type': 'executable',
+ 'sources': [
+ 'catch_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'catch_action_queue',
+ 'catch_action_lib',
+ 'action',
+ ],
+ },
],
}
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
+
diff --git a/frc971/actions/catch_action.h b/frc971/actions/catch_action.h
new file mode 100644
index 0000000..f6e793f
--- /dev/null
+++ b/frc971/actions/catch_action.h
@@ -0,0 +1,38 @@
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/action.h"
+#include "aos/common/time.h"
+
+namespace frc971 {
+namespace actions {
+
+class CatchAction : public ActionBase<actions::CatchActionGroup> {
+ public:
+
+ explicit CatchAction(actions::CatchActionGroup* s);
+
+ // Actually executes the action of moving the claw into position and closing
+ // it.
+ void RunAction();
+
+ static constexpr double kCatchSeparation = 1.0;
+ static constexpr double kCatchIntake = 12.0;
+ static constexpr double kSonarTriggerDist = 0.8;
+ static constexpr double kCatchCentering = 12.0;
+ static constexpr double kFinishAngle = 0.2;
+
+ protected:
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupCatch();
+ // sonar is in valid range to close
+ bool DoneFoundSonar();
+ // Claw reports it is done
+ bool DoneClawWithBall();
+ // hall effect reports the ball is in
+ bool DoneBallIn();
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/catch_action.q b/frc971/actions/catch_action.q
new file mode 100644
index 0000000..33b21f0
--- /dev/null
+++ b/frc971/actions/catch_action.q
@@ -0,0 +1,19 @@
+package frc971.actions;
+
+queue_group CatchActionGroup {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run;
+ double catch_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group CatchActionGroup catch_action;
diff --git a/frc971/actions/catch_action_main.cc b/frc971/actions/catch_action_main.cc
new file mode 100644
index 0000000..29fc744
--- /dev/null
+++ b/frc971/actions/catch_action_main.cc
@@ -0,0 +1,22 @@
+#include "stdio.h"
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/actions/catch_action.q.h"
+#include "frc971/actions/catch_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/ []) {
+ ::aos::Init();
+
+ frc971::actions::CatchAction action_catch(
+ &::frc971::actions::catch_action);
+ action_catch.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index d5ca0d1..7fe4c2f 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -15,6 +15,7 @@
'../autonomous/autonomous.gyp:auto',
'../actions/actions.gyp:shoot_action',
'../actions/actions.gyp:selfcatch_action',
+ '../actions/actions.gyp:catch_action',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',