Added more generic wait function to actions
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index fc88435..92b5de6 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -1,5 +1,7 @@
#include <stdio.h>
+#include <functional>
+
#include "aos/common/control_loop/Timing.h"
#include "aos/common/logging/logging.h"
#include "aos/common/network/team_number.h"
@@ -9,22 +11,20 @@
namespace frc971 {
namespace actions {
-template <class T>
-class ActionBase {
+template <class T> class ActionBase {
public:
- ActionBase (T* acq)
- : action_q_(acq) {}
+ ActionBase(T* acq) : action_q_(acq) {}
virtual void RunAction() = 0;
- void Run () {
- action_q_->goal.FetchLatest();
+ // runs action while enabled
+ void Run() {
+ action_q_->goal.FetchLatest();
while (!action_q_->goal.get()) {
action_q_->goal.FetchNextBlocking();
}
- if (!action_q_->status.MakeWithBuilder().running(false)
- .Send()) {
+ if (!action_q_->status.MakeWithBuilder().running(false).Send()) {
LOG(ERROR, "Failed to send the status.\n");
}
while (true) {
@@ -33,22 +33,42 @@
action_q_->goal.FetchNextBlocking();
}
LOG(INFO, "Starting action\n");
- if (!action_q_->status.MakeWithBuilder().running(
- true).Send()) {
+ 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()) {
+ 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();
}
}
}
+
+ // will run until the done condition is met.
+ // will return false if successful and true if the action was canceled or
+ // failed.
+ // done condition are defined as functions that return true when done and have
+ // some sort of blocking statement(FetchNextBlocking) to throttle spin rate.
+ bool WaitUntil(::std::function<bool(void)> done_condition) {
+ while (!done_condition()) {
+ if (ShouldCancel() || abort_) {
+ // clear abort bit as we have just aqborted
+ abort_ = false;
+ return true;
+ }
+ }
+ if (ShouldCancel() || abort_) {
+ // clear abort bit as we have just aqborted
+ abort_ = false;
+ return true;
+ } else {
+ return false;
+ }
+ }
+
// Returns true if the action should be canceled.
bool ShouldCancel() {
action_q_->goal.FetchLatest();
@@ -60,9 +80,14 @@
}
protected:
+
+ // boolean to stop on fail
+ bool abort_ = false;
+
+ // queue for this action
T* action_q_;
};
} // namespace actions
} // namespace frc971
-
+
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index 548c5f3..baa5d10 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -1,3 +1,5 @@
+#include <functional>
+
#include "aos/common/control_loop/Timing.h"
#include "aos/common/logging/logging.h"
@@ -10,18 +12,19 @@
namespace frc971 {
namespace actions {
-
ShootAction::ShootAction(actions::ShootActionQueueGroup* s)
: actions::ActionBase<actions::ShootActionQueueGroup>(s) {}
double ShootAction::SpeedToAngleOffset(double speed) {
const frc971::constants::Values& values = frc971::constants::GetValues();
- // scale speed to a [0.0-1.0] on something close to the max
- return (speed/values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
+ // scale speed to a [0.0-1.0] on something close to the max
+ return (speed / values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
}
-
void ShootAction::RunAction() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+
+ // Set shot power
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
shoot_action.goal->shot_power)
.shot_requested(false).unload_requested(false).load_requested(false)
@@ -30,6 +33,7 @@
return;
}
+ // Set claw angle
control_loops::drivetrain.status.FetchLatest();
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
shoot_action.goal->shot_angle +
@@ -39,43 +43,8 @@
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;
- }
-
- // update the claw position to track velocity
- // TODO(ben): the claw may never reach the goal if the velocity is
- // continually changing, we will need testing to see
- control_loops::drivetrain.status.FetchLatest();
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle +
- SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
- .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- } else {
- LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
- SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
- }
-
-
- // 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();
+ // wait for claw to be ready
+ if (WaitUntil(::std::bind(&ShootAction::DoneSetupShot, this))) return;
// Open up the claw in preparation for shooting.
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
@@ -86,23 +55,14 @@
return;
}
- if (ShouldCancel()) return;
+ // wait for the claw to open up a little before we shoot
+ if (WaitUntil(::std::bind(&ShootAction::DonePreShotOpen, this))) 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;
- }
-
+ // 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.
+ previous_shots_ = control_loops::shooter_queue_group.status->shots;
// Shoot!
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
shoot_action.goal->shot_power)
@@ -112,26 +72,61 @@
return;
}
- if (ShouldCancel()) return;
+ // wait for record of shot having been fired
+ if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
+
+ // done with action
+ 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;
+bool ShootAction::DoneSetupShot() {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ // 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.
+ return true;
}
- return;
+ // update the claw position to track velocity
+ // TODO(ben): the claw may never reach the goal if the velocity is
+ // continually changing, we will need testing to see
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ abort_ = true;
+ return true;
+ } else {
+ LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
+ }
+ return false;
+}
+
+bool ShootAction::DonePreShotOpen() {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ control_loops::claw_queue_group.status.FetchNextBlocking();
+ if (control_loops::claw_queue_group.status->separation >
+ values.shooter_action.claw_shooting_separation) {
+ LOG(INFO, "Opened up enough to shoot.\n");
+ return true;
+ }
+ return false;
+}
+
+bool ShootAction::DoneShot() {
+ control_loops::shooter_queue_group.status.FetchNextBlocking();
+ if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
+ LOG(INFO, "Shot succeeded!\n");
+ return true;
+ }
+ return false;
}
} // namespace actions
diff --git a/frc971/actions/shoot_action.h b/frc971/actions/shoot_action.h
index d080760..2522195 100644
--- a/frc971/actions/shoot_action.h
+++ b/frc971/actions/shoot_action.h
@@ -1,13 +1,6 @@
-//#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 {
@@ -26,6 +19,15 @@
static constexpr double kOffsetRadians = 0.2;
protected:
+ // completed shot
+ bool DoneShot();
+ // ready for shot
+ bool DonePreShotOpen();
+ // in the right place
+ bool DoneSetupShot();
+
+ // to track when shot is complete
+ int previous_shots_;
};
} // namespace actions