Started work on creating a shooter action with Austin.
diff --git a/frc971/autonomous/shoot_action.cc b/frc971/autonomous/shoot_action.cc
new file mode 100644
index 0000000..118736b
--- /dev/null
+++ b/frc971/autonomous/shoot_action.cc
@@ -0,0 +1,108 @@
+#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/constants.h"
+
+namespace frc971 {
+namespace actions {
+
+class ShootAction {
+ public:
+ void Run() {
+ ::frc971::actions::shoot_action.FetchLatest();
+ while (!::frc971::actions::shoot_action.get()) {
+ ::frc971::actions::shoot_action.FetchNextBlocking();
+ }
+
+ while (true) {
+ while (!::frc971::actions::shoot_action->run) {
+ LOG(INFO, "Waiting for an action request.\n");
+ ::frc971::actions::shoot_action.FetchNextBlocking();
+ }
+ LOG(INFO, "Starting action\n");
+ RunAction();
+
+ while (::frc971::actions::shoot_action->run) {
+ LOG(INFO, "Waiting for the action to be stopped.\n");
+ ::frc971::actions::shoot_action.FetchNextBlocking();
+ }
+ }
+ }
+
+ void RunAction();
+
+ protected:
+ // Returns true if the action should be canceled.
+ bool ShouldCancel() {
+ shoot_action.FetchLatest();
+ bool ans = !::frc971::actions::shoot_action->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->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)
+ .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) {
+ if (control_loops::shooter_queue_group.status->ready &&
+ control_loops::claw_queue_group.status->done) {
+ LOG(INFO, "Claw and Shooter ready for shooting.\n");
+ 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;
+ }
+
+ // 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()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ }
+
+ if (ShouldCancel()) return;
+
+ // Make sure we have the latest statuses.
+ control_loops::claw_queue_group.status.FetchLatest();
+ while (true) {
+ if (control_loops::claw_queue_group.status->separation > k1) {
+ 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;
+ }
+}
+
+} // namespace actions
+} // namespace frc971
diff --git a/frc971/autonomous/shoot_action.q b/frc971/autonomous/shoot_action.q
new file mode 100644
index 0000000..15073cb
--- /dev/null
+++ b/frc971/autonomous/shoot_action.q
@@ -0,0 +1,21 @@
+package frc971.actions;
+
+queue_group ShootAction {
+ message Status {
+ bool running;
+ };
+
+ message Goal {
+ // If true, run this action. If false, cancel the action if it is
+ // currently running.
+ bool run; // Shot power in joules.
+ double shot_power;
+ // Claw angle when shooting.
+ bool shot_angle;
+ };
+
+ queue Goal goal;
+ queue Status status;
+};
+
+queue_group ShootAction shoot_action;
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index e82710b..815210f 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -738,7 +738,19 @@
output->bottom_claw_voltage = -kMaxVoltage;
}
}
- status->done = false;
+
+ bool bottom_done =
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.005;
+ bool separation_done =
+ ::std::abs((top_absolute_position() - bottom_absolute_position()) -
+ goal->separation_angle) <
+ 0.005;
+ status->done = is_ready() && separation_done && bottom_done;
+
+ status->bottom = bottom_absolute_position();
+ status->separation = top_absolute_position() - bottom_absolute_position();
+ status->bottom_velocity = claw_.X_hat(2, 0);
+ status->separation_velocity = claw_.X_hat(3, 0);
was_enabled_ = ::aos::robot_state->enabled;
}
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index f1d2f93..c67ed20 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -51,6 +51,16 @@
double tusk_voltage;
};
+ message Status {
+ // True if zeroed and within tolerance for separation and bottom angle.
+ bool done;
+ // Dump the values of the state matrix.
+ double bottom;
+ double bottom_velocity;
+ double separation;
+ double separation_velocity;
+ };
+
queue Goal goal;
queue Position position;
queue Output output;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 11a11b9..036b466 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -157,6 +157,7 @@
LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
+ status->ready = false;
if (reset()) {
state_ = STATE_INITIALIZE;
@@ -397,6 +398,7 @@
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
if (Time::Now() > shooter_brake_set_time_) {
+ status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
@@ -416,6 +418,7 @@
// TODO(austin): Do we want to set the brake here or after shooting?
// Depends on air usage.
+ status->ready = false;
LOG(DEBUG, "Preparing shot again.\n");
state_ = STATE_PREPARE_SHOT;
}
@@ -570,9 +573,6 @@
output->brake_piston = brake_piston_;
}
- status->done = ::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.004;
-
if (position) {
last_position_ = *position;
LOG(DEBUG, "pos > absolute: %f velocity: %f state= %d l= %d pp= %d, pd= %d "
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index 6310320..fd2ddd4 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -41,6 +41,7 @@
// Whether it's ready to shoot right now.
bool ready;
// Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
bool cocked;
// How many times we've shot.
int32_t shots;