Got shoot action stuff building.
Still need to clean some stuff up and get it working and test it and all that fancy stuff.
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 91b1b2f..fe2abe1 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -16,6 +16,21 @@
'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': [
@@ -23,6 +38,7 @@
],
'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',
@@ -36,6 +52,22 @@
],
},
{
+ '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
index 118736b..3fe3b76 100644
--- a/frc971/autonomous/shoot_action.cc
+++ b/frc971/autonomous/shoot_action.cc
@@ -1,10 +1,9 @@
-#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/actions/shoot_action.q.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 {
@@ -13,33 +12,47 @@
class ShootAction {
public:
void Run() {
- ::frc971::actions::shoot_action.FetchLatest();
- while (!::frc971::actions::shoot_action.get()) {
- ::frc971::actions::shoot_action.FetchNextBlocking();
+ ::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->run) {
+ while (!::frc971::actions::shoot_action.goal->run) {
LOG(INFO, "Waiting for an action request.\n");
- ::frc971::actions::shoot_action.FetchNextBlocking();
+ ::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->run) {
+ while (::frc971::actions::shoot_action.goal->run) {
LOG(INFO, "Waiting for the action to be stopped.\n");
- ::frc971::actions::shoot_action.FetchNextBlocking();
+ ::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.FetchLatest();
- bool ans = !::frc971::actions::shoot_action->run;
+ shoot_action.goal.FetchLatest();
+ bool ans = !::frc971::actions::shoot_action.goal->run;
if (ans) {
LOG(INFO, "Time to exit auto mode\n");
}
@@ -49,14 +62,14 @@
void ShootAction::RunAction() {
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
- shoot_action->shot_power).shot_requested(false)
+ 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->shot_angle).separation_angle(0.0).intake(2.0)
+ shoot_action.goal->shot_angle).separation_angle(0.0).intake(2.0)
.centering(1.0).Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
@@ -66,9 +79,12 @@
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;
}
@@ -79,20 +95,23 @@
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->shot_angle).separation_angle(k2).intake(2.0)
- .centering(1.0).Send()) {
+ 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 statuses.
+ // Make sure we have the latest status.
control_loops::claw_queue_group.status.FetchLatest();
while (true) {
- if (control_loops::claw_queue_group.status->separation > k1) {
+ if (control_loops::claw_queue_group.status->separation > values.shooter_action.claw_shooting_separation) {
LOG(INFO, "Opened up enough to shoot.\n");
break;
}
@@ -102,6 +121,35 @@
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
diff --git a/frc971/autonomous/shoot_action.q b/frc971/autonomous/shoot_action.q
index 15073cb..507b12a 100644
--- a/frc971/autonomous/shoot_action.q
+++ b/frc971/autonomous/shoot_action.q
@@ -1,6 +1,6 @@
package frc971.actions;
-queue_group ShootAction {
+queue_group ShootActionQueueGroup {
message Status {
bool running;
};
@@ -18,4 +18,4 @@
queue Status status;
};
-queue_group ShootAction shoot_action;
+queue_group ShootActionQueueGroup shoot_action;