Moved the actions out of autonomous. Abstracted some of the boilerplate in shoot action. created a main from which to run the thing.
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
new file mode 100644
index 0000000..fc88435
--- /dev/null
+++ b/frc971/actions/action.h
@@ -0,0 +1,68 @@
+#include <stdio.h>
+
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/network/team_number.h"
+
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace actions {
+
+template <class T>
+class ActionBase {
+ public:
+ ActionBase (T* acq)
+ : action_q_(acq) {}
+
+ virtual void RunAction() = 0;
+
+ void Run () {
+ action_q_->goal.FetchLatest();
+ while (!action_q_->goal.get()) {
+ action_q_->goal.FetchNextBlocking();
+ }
+
+ if (!action_q_->status.MakeWithBuilder().running(false)
+ .Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ while (true) {
+ while (!action_q_->goal->run) {
+ LOG(INFO, "Waiting for an action request.\n");
+ action_q_->goal.FetchNextBlocking();
+ }
+ LOG(INFO, "Starting action\n");
+ if (!action_q_->status.MakeWithBuilder().running(
+ true).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ RunAction();
+ if (!action_q_->status.MakeWithBuilder().running(
+ false).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+
+ while (action_q_->goal->run) {
+ LOG(INFO, "Waiting for the action to be stopped.\n");
+ action_q_->goal.FetchNextBlocking();
+ }
+ }
+ }
+ // Returns true if the action should be canceled.
+ bool ShouldCancel() {
+ action_q_->goal.FetchLatest();
+ bool ans = !action_q_->goal->run;
+ if (ans) {
+ LOG(INFO, "Time to exit auto mode\n");
+ }
+ return ans;
+ }
+
+ protected:
+ T* action_q_;
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
new file mode 100644
index 0000000..931d847
--- /dev/null
+++ b/frc971/actions/actions.gyp
@@ -0,0 +1,62 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'shoot_action_queue',
+ 'type': 'static_library',
+ 'sources': ['shoot_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': [
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ ]
+ },
+ {
+ 'target_name': 'shoot_action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'shoot_action.cc',
+ ],
+ 'dependencies': [
+ 'shoot_action_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ ],
+ },
+ {
+ 'target_name': 'shoot_action',
+ 'type': 'executable',
+ 'sources': [
+ 'shoot_action_main.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ 'shoot_action_queue',
+ 'shoot_action_lib',
+ 'action',
+ ],
+ },
+ ],
+}
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
new file mode 100644
index 0000000..71698a4
--- /dev/null
+++ b/frc971/actions/shoot_action.cc
@@ -0,0 +1,111 @@
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/shoot_action.h"
+#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/claw/claw.q.h"
+#include "frc971/constants.h"
+
+namespace frc971 {
+namespace actions {
+
+ShootAction::ShootAction(actions::ShootActionQueueGroup* s)
+ : actions::ActionBase<actions::ShootActionQueueGroup>(s) {}
+
+void ShootAction::RunAction() {
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ shoot_action.goal->shot_power)
+ .shot_requested(false).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(ERROR, "Failed to send the shoot action\n");
+ return;
+ }
+
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ shoot_action.goal->shot_angle)
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ // Make sure we have the latest statuses.
+ control_loops::shooter_queue_group.status.FetchLatest();
+ control_loops::claw_queue_group.status.FetchLatest();
+ while (true) {
+ // Make sure that both the shooter and claw have reached the necessary
+ // states.
+ if (control_loops::shooter_queue_group.status->ready &&
+ control_loops::claw_queue_group.status->done) {
+ LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ // TODO(james): Get realer numbers for shooter_action.
+ break;
+ }
+
+ // Wait until we have a new status.
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+
+ if (ShouldCancel()) return;
+ }
+
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+
+ // Open up the claw in preparation for shooting.
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ shoot_action.goal->shot_angle)
+ .separation_angle(values.shooter_action.claw_separation_goal)
+ .intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ if (ShouldCancel()) return;
+
+ // Make sure we have the latest status.
+ control_loops::claw_queue_group.status.FetchLatest();
+ while (true) {
+ if (control_loops::claw_queue_group.status->separation >
+ values.shooter_action.claw_shooting_separation) {
+ LOG(INFO, "Opened up enough to shoot.\n");
+ break;
+ }
+
+ // Wait until we have a new status.
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+
+ if (ShouldCancel()) return;
+ }
+
+ // Shoot!
+ if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
+ shoot_action.goal->shot_power)
+ .shot_requested(true).unload_requested(false).load_requested(false)
+ .Send()) {
+ LOG(WARNING, "sending shooter goal failed\n");
+ return;
+ }
+
+ if (ShouldCancel()) 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
+ // again for another few cycles.
+ int previous_shots = control_loops::shooter_queue_group.status->shots;
+ while (true) {
+ if (control_loops::shooter_queue_group.status->shots > previous_shots) {
+ LOG(INFO, "Shot succeeded!\n");
+ break;
+ }
+
+ // Wait until we have a new status.
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+
+ if (ShouldCancel()) return;
+ }
+
+ return;
+}
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/actions/shoot_action.h b/frc971/actions/shoot_action.h
new file mode 100644
index 0000000..b3a20d8
--- /dev/null
+++ b/frc971/actions/shoot_action.h
@@ -0,0 +1,28 @@
+//#include "aos/common/control_loop/Timing.h"
+//#include "aos/common/logging/logging.h"
+
+#include "frc971/actions/shoot_action.q.h"
+#include "frc971/actions/action.h"
+
+//#include "frc971/control_loops/shooter/shooter.q.h"
+//#include "frc971/control_loops/claw/claw.q.h"
+//#include "frc971/constants.h"
+
+namespace frc971 {
+namespace actions {
+
+class ShootAction : public ActionBase <actions::ShootActionQueueGroup> {
+ public:
+
+ explicit ShootAction (actions::ShootActionQueueGroup * s);
+
+ // Actually execute the action of moving the claw and shooter into position
+ // and actually firing them.
+ void RunAction();
+
+ protected:
+};
+
+} // namespace actions
+} // namespace frc971
+
diff --git a/frc971/autonomous/shoot_action.q b/frc971/actions/shoot_action.q
similarity index 100%
rename from frc971/autonomous/shoot_action.q
rename to frc971/actions/shoot_action.q
diff --git a/frc971/actions/shoot_action_main.cc b/frc971/actions/shoot_action_main.cc
new file mode 100644
index 0000000..31cea1f
--- /dev/null
+++ b/frc971/actions/shoot_action_main.cc
@@ -0,0 +1,21 @@
+#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/shoot_action.q.h"
+#include "frc971/actions/shoot_action.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actions::ShootAction shoot(&::frc971::actions::shoot_action);
+ shoot.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
+
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index fe2abe1..91b1b2f 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -16,21 +16,6 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
- 'target_name': 'shoot_action_queue',
- 'type': 'static_library',
- 'sources': ['shoot_action.q'],
- 'variables': {
- 'header_path': 'frc971/autonomous',
- },
- 'dependencies': [
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:queues',
- ],
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
'target_name': 'auto_lib',
'type': 'static_library',
'sources': [
@@ -38,7 +23,6 @@
],
'dependencies': [
'auto_queue',
- 'shoot_action_lib',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
@@ -52,22 +36,6 @@
],
},
{
- 'target_name': 'shoot_action_lib',
- 'type': 'static_library',
- 'sources': [
- 'shoot_action.cc',
- ],
- 'dependencies': [
- 'shoot_action_queue',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:timing',
- '<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
- ],
- },
- {
'target_name': 'auto',
'type': 'executable',
'sources': [
diff --git a/frc971/autonomous/shoot_action.cc b/frc971/autonomous/shoot_action.cc
deleted file mode 100644
index 3fe3b76..0000000
--- a/frc971/autonomous/shoot_action.cc
+++ /dev/null
@@ -1,156 +0,0 @@
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/logging/logging.h"
-
-#include "frc971/autonomous/shoot_action.q.h"
-#include "frc971/control_loops/shooter/shooter.q.h"
-#include "frc971/control_loops/claw/claw.q.h"
-#include "frc971/constants.h"
-
-namespace frc971 {
-namespace actions {
-
-class ShootAction {
- public:
- void Run() {
- ::frc971::actions::shoot_action.goal.FetchLatest();
- while (!::frc971::actions::shoot_action.goal.get()) {
- ::frc971::actions::shoot_action.goal.FetchNextBlocking();
- }
-
- if (!::frc971::actions::shoot_action.status.MakeWithBuilder().running(false)
- .Send()) {
- LOG(ERROR, "Failed to send the status.\n");
- }
- while (true) {
- while (!::frc971::actions::shoot_action.goal->run) {
- LOG(INFO, "Waiting for an action request.\n");
- ::frc971::actions::shoot_action.goal.FetchNextBlocking();
- }
- LOG(INFO, "Starting action\n");
- if (!::frc971::actions::shoot_action.status.MakeWithBuilder().running(
- true).Send()) {
- LOG(ERROR, "Failed to send the status.\n");
- }
- RunAction();
- if (!::frc971::actions::shoot_action.status.MakeWithBuilder().running(
- false).Send()) {
- LOG(ERROR, "Failed to send the status.\n");
- }
-
- while (::frc971::actions::shoot_action.goal->run) {
- LOG(INFO, "Waiting for the action to be stopped.\n");
- ::frc971::actions::shoot_action.goal.FetchNextBlocking();
- }
- }
- }
-
- // Actually execute the action of moving the claw and shooter into position
- // and actually firing them.
- void RunAction();
-
- protected:
- // Returns true if the action should be canceled.
- bool ShouldCancel() {
- shoot_action.goal.FetchLatest();
- bool ans = !::frc971::actions::shoot_action.goal->run;
- if (ans) {
- LOG(INFO, "Time to exit auto mode\n");
- }
- return ans;
- }
-};
-
-void ShootAction::RunAction() {
- if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
- shoot_action.goal->shot_power).shot_requested(false)
- .unload_requested(false).load_requested(false).Send()) {
- LOG(ERROR, "Failed to send the shoot action\n");
- return;
- }
-
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle).separation_angle(0.0).intake(2.0)
- .centering(1.0).Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
-
- // Make sure we have the latest statuses.
- control_loops::shooter_queue_group.status.FetchLatest();
- control_loops::claw_queue_group.status.FetchLatest();
- while (true) {
- // Make sure that both the shooter and claw have reached the necessary
- // states.
- if (control_loops::shooter_queue_group.status->ready &&
- control_loops::claw_queue_group.status->done) {
- LOG(INFO, "Claw and Shooter ready for shooting.\n");
- // TODO(james): Get realer numbers for shooter_action.
- break;
- }
-
- // Wait until we have a new status.
- control_loops::shooter_queue_group.status.FetchNextBlocking();
- control_loops::claw_queue_group.status.FetchNextBlocking();
-
- if (ShouldCancel()) return;
- }
-
- const frc971::constants::Values &values = frc971::constants::GetValues();
-
- // Open up the claw in preparation for shooting.
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle)
- .separation_angle(values.shooter_action.claw_separation_goal)
- .intake(2.0).centering(1.0).Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
-
- if (ShouldCancel()) return;
-
- // Make sure we have the latest status.
- control_loops::claw_queue_group.status.FetchLatest();
- while (true) {
- if (control_loops::claw_queue_group.status->separation > values.shooter_action.claw_shooting_separation) {
- LOG(INFO, "Opened up enough to shoot.\n");
- break;
- }
-
- // Wait until we have a new status.
- control_loops::claw_queue_group.status.FetchNextBlocking();
-
- if (ShouldCancel()) return;
- }
-
- // Shoot!
- if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
- shoot_action.goal->shot_power).shot_requested(true)
- .unload_requested(false).load_requested(false).Send()) {
- LOG(WARNING, "sending shooter goal failed\n");
- return;
- }
-
- if (ShouldCancel()) 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
- // again for another few cycles.
- int previous_shots = control_loops::shooter_queue_group.status->shots;
- while (true) {
- if (control_loops::shooter_queue_group.status->shots > previous_shots) {
- LOG(INFO, "Shot succeeded!\n");
- break;
- }
-
- // Wait until we have a new status.
- control_loops::shooter_queue_group.status.FetchNextBlocking();
-
- if (ShouldCancel()) return;
- }
-
- return;
-}
-
-} // namespace actions
-} // namespace frc971
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 2b5d600..f445b64 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -13,6 +13,7 @@
'../control_loops/shooter/shooter.gyp:shooter',
'../control_loops/shooter/shooter.gyp:shooter_lib_test',
'../autonomous/autonomous.gyp:auto',
+ '../actions/actions.gyp:shoot_action',
'../input/input.gyp:joystick_reader',
'../output/output.gyp:motor_writer',
'../input/input.gyp:sensor_receiver',