Move Actions To Common:
- Splits Actions into an ActionQueue and Actions and Actors
- unittested action preemption.
Final cleanup by Brian.
Change-Id: If444ae9902ef6c511898730e042f46f1781f4fb9
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
deleted file mode 100644
index 51da4cf..0000000
--- a/frc971/actions/action.h
+++ /dev/null
@@ -1,141 +0,0 @@
-#ifndef FRC971_ACTIONS_ACTION_H_
-#define FRC971_ACTIONS_ACTION_H_
-
-#include <stdio.h>
-#include <inttypes.h>
-
-#include <functional>
-
-#include "aos/common/logging/logging.h"
-#include "aos/common/logging/queue_logging.h"
-#include "aos/common/time.h"
-
-namespace frc971 {
-namespace actions {
-
-template <class T> class ActionBase {
- public:
- ActionBase(T* acq) : action_q_(acq) {}
-
- virtual void RunAction() = 0;
-
- // runs action while enabled
- void Run() {
- LOG(DEBUG, "Waiting for input to start\n");
-
- action_q_->goal.FetchLatest();
- if (action_q_->goal.get()) {
- LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
- const uint32_t initially_running = action_q_->goal->run;
- if (initially_running != 0) {
- while (action_q_->goal->run == initially_running) {
- LOG(INFO, "run is still %" PRIx32 "\n", initially_running);
- action_q_->goal.FetchNextBlocking();
- LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
- }
- }
- } else {
- action_q_->goal.FetchNextBlocking();
- LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
- }
-
- LOG(DEBUG, "actually starting\n");
-
- if (!action_q_->status.MakeWithBuilder().running(0).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_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
- if (!action_q_->goal->run) {
- if (!action_q_->status.MakeWithBuilder().running(0).Send()) {
- LOG(ERROR, "Failed to send the status.\n");
- }
- }
- }
-
- const uint32_t running_id = action_q_->goal->run;
- LOG(INFO, "Starting action %" PRIx32 "\n", running_id);
- if (!action_q_->status.MakeWithBuilder().running(running_id).Send()) {
- LOG(ERROR, "Failed to send the status.\n");
- }
- RunAction();
- LOG(INFO, "Done with action %" PRIx32 "\n", running_id);
-
- // If we have a new one to run, we shouldn't say we're stopped in between.
- if (action_q_->goal->run == 0 || action_q_->goal->run == running_id) {
- if (!action_q_->status.MakeWithBuilder().running(0).Send()) {
- LOG(ERROR, "Failed to send the status.\n");
- }
- } else {
- LOG(INFO, "skipping sending stopped status for %" PRIx32 "\n",
- running_id);
- }
-
- while (action_q_->goal->run == running_id) {
- LOG(INFO, "Waiting for the action (%" PRIx32 ") to be stopped.\n",
- running_id);
- action_q_->goal.FetchNextBlocking();
- LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
- }
- LOG(DEBUG, "action %" PRIx32 " was stopped\n", running_id);
- }
- }
-
- // will run until the done condition is met or times out.
- // will return false if successful and true if the action was canceled or
- // failed or end_time was reached before it succeeded.
- // done condition are defined as functions that return true when done and have
- // some sort of blocking statement(FetchNextBlocking) to throttle spin rate.
- // end_time is when to stop and return true. Time(0, 0) (the default) means
- // never time out.
- bool WaitUntil(::std::function<bool(void)> done_condition,
- const ::aos::time::Time &end_time = ::aos::time::Time(0, 0)) {
- while (!done_condition()) {
- if (ShouldCancel() || abort_) {
- // Clear abort bit as we have just aborted.
- abort_ = false;
- return true;
- }
- if (end_time != ::aos::time::Time(0, 0) &&
- ::aos::time::Time::Now() >= end_time) {
- LOG(INFO, "WaitUntil timed out\n");
- return true;
- }
- }
- if (ShouldCancel() || abort_) {
- // Clear abort bit as we have just aborted.
- abort_ = false;
- return true;
- } else {
- return false;
- }
- }
-
- // Returns true if the action should be canceled.
- bool ShouldCancel() {
- if (action_q_->goal.FetchNext()) {
- LOG_STRUCT(DEBUG, "goal queue ", *action_q_->goal);
- }
- bool ans = !action_q_->goal->run;
- if (ans) {
- LOG(INFO, "Time to stop action\n");
- }
- return ans;
- }
-
- protected:
-
- // boolean to stop on fail
- bool abort_ = false;
-
- // queue for this action
- T* action_q_;
-};
-
-} // namespace actions
-} // namespace frc971
-
-#endif // FRC971_ACTIONS_ACTION_H_
diff --git a/frc971/actions/action.q b/frc971/actions/action.q
deleted file mode 100644
index a104b72..0000000
--- a/frc971/actions/action.q
+++ /dev/null
@@ -1,24 +0,0 @@
-package frc971.actions;
-
-interface StatusInterface {
- // 0 if the action isn't running or the value from goal.run.
- uint32_t running;
-};
-
-interface GoalInterface {
- // 0 to stop or an arbitrary value to put in status.running.
- uint32_t run;
-};
-
-message Status {
- uint32_t running;
-};
-
-message Goal {
- uint32_t run;
-};
-
-interface ActionQueueGroup {
- queue Status status;
- queue Goal goal;
-};
diff --git a/frc971/actions/action_client.h b/frc971/actions/action_client.h
deleted file mode 100644
index c654fcb..0000000
--- a/frc971/actions/action_client.h
+++ /dev/null
@@ -1,200 +0,0 @@
-#ifndef FRC971_ACTIONS_ACTION_CLIENT_H_
-#define FRC971_ACTIONS_ACTION_CLIENT_H_
-
-#include <inttypes.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <type_traits>
-#include <atomic>
-
-#include "aos/common/logging/logging.h"
-#include "aos/common/queue.h"
-
-namespace frc971 {
-
-class Action {
- public:
- // Cancels the action.
- void Cancel() { DoCancel(); }
- // Returns true if the action is currently running.
- bool Running() { return DoRunning(); }
- // Starts the action.
- void Start() { DoStart(); }
-
- // Waits until the action has finished.
- void WaitUntilDone() { DoWaitUntilDone(); }
-
- virtual ~Action() {}
-
- private:
- virtual void DoCancel() = 0;
- virtual bool DoRunning() = 0;
- virtual void DoStart() = 0;
- virtual void DoWaitUntilDone() = 0;
-};
-
-// Templated subclass to hold the type information.
-template <typename T>
-class TypedAction : public Action {
- public:
- typedef typename std::remove_reference<
- decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
-
- TypedAction(T *queue_group)
- : queue_group_(queue_group),
- goal_(queue_group_->goal.MakeMessage()),
- run_value_(run_counter.fetch_add(1, ::std::memory_order_relaxed) |
- ((getpid() & 0xFFFF) << 16)) {
- LOG(INFO, "Action %" PRIx32 " created on queue %s\n", run_value_,
- queue_group_->goal.name());
- }
-
- // Returns the current goal that will be sent when the action is sent.
- GoalType *GetGoal() { return goal_.get(); }
-
- virtual ~TypedAction() {
- LOG(DEBUG, "Calling destructor of %" PRIx32"\n", run_value_);
- DoCancel();
- }
-
- private:
- // Cancels the action.
- virtual void DoCancel() {
- if (!sent_started_) {
- LOG(INFO, "action %" PRIx32 " was never started\n", run_value_);
- } else {
- if (interrupted_) {
- LOG(INFO, "action %" PRIx32 " was interrupted -> not cancelling\n",
- run_value_);
- } else {
- if (sent_cancel_) {
- LOG(INFO, "action %" PRIx32 " already cancelled\n", run_value_);
- } else {
- LOG(INFO, "Canceling action %" PRIx32 " on queue %s\n", run_value_,
- queue_group_->goal.name());
- queue_group_->goal.MakeWithBuilder().run(0).Send();
- sent_cancel_ = true;
- }
- }
- }
- }
-
- // Returns true if the action is running or we don't have an initial response
- // back from it to signal whether or not it is running.
- virtual bool DoRunning() {
- if (!sent_started_) return false;
- if (has_started_) {
- queue_group_->status.FetchLatest();
- CheckInterrupted();
- } else if (queue_group_->status.FetchLatest()) {
- CheckStarted();
- }
- if (interrupted_) return false;
- if (!has_started_) return true;
- return queue_group_->status.get() &&
- queue_group_->status->running == run_value_;
- }
-
- virtual void DoWaitUntilDone() {
- assert(sent_started_);
- queue_group_->status.FetchLatest();
- CheckInterrupted();
- while (true) {
- if (interrupted_) return;
- CheckStarted();
- queue_group_->status.FetchNextBlocking();
- CheckStarted();
- CheckInterrupted();
- if (has_started_ && (queue_group_->status.get() &&
- queue_group_->status->running != run_value_)) {
- return;
- }
- }
- }
-
- void CheckStarted() {
- if (has_started_) return;
- if (queue_group_->status.get()) {
- if (queue_group_->status->running == run_value_) {
- // It's currently running our instance.
- has_started_ = true;
- LOG(DEBUG, "action %" PRIx32 " has been started\n", run_value_);
- } else if (queue_group_->status->running == old_run_value_) {
- // It's still running an old instance (or still doing nothing).
- } else {
- LOG(WARNING,
- "action %" PRIx32 " interrupted by %" PRIx32 " before starting\n",
- run_value_, queue_group_->status->running);
- has_started_ = true;
- interrupted_ = true;
- }
- }
- }
-
- void CheckInterrupted() {
- if (!interrupted_ && has_started_ && queue_group_->status.get()) {
- if (queue_group_->status->running != 0 &&
- queue_group_->status->running != run_value_) {
- LOG(WARNING,
- "action %" PRIx32 " interrupted by %" PRIx32 " after starting\n",
- run_value_, queue_group_->status->running);
- }
- }
- }
-
- // Starts the action if a goal has been created.
- virtual void DoStart() {
- if (goal_) {
- LOG(INFO, "Starting action %" PRIx32 "\n", run_value_);
- goal_->run = run_value_;
- sent_started_ = true;
- if (!goal_.Send()) {
- LOG(ERROR, "sending goal for action %" PRIx32 " failed\n", run_value_);
- // Don't wait to see a message with it.
- has_started_ = true;
- }
- queue_group_->status.FetchLatest();
- if (queue_group_->status.get()) {
- old_run_value_ = queue_group_->status->running;
- LOG(INFO, "action %" PRIx32 " already running\n", old_run_value_);
- } else {
- old_run_value_ = 0;
- }
- } else {
- LOG(WARNING, "action %" PRIx32 " already started\n", run_value_);
- }
- }
-
- T *const queue_group_;
- ::aos::ScopedMessagePtr<GoalType> goal_;
-
- // Track if we have seen a response to the start message.
- bool has_started_ = false;
- // Track if we have sent an initial start message.
- bool sent_started_ = false;
-
- bool sent_cancel_ = false;
-
- // Gets set to true if we ever see somebody else's value in running.
- bool interrupted_ = false;
-
- // The value we're going to use for goal.run etc.
- const uint32_t run_value_;
-
- // The old value for running that we may have seen. If we see any value other
- // than this or run_value_, somebody else got in the way and we're done. 0 if
- // there was nothing there to start with. Only valid after sent_started_
- // changes to true.
- uint32_t old_run_value_;
-
- static ::std::atomic<uint16_t> run_counter;
-};
-
-template <typename T>
-::std::atomic<uint16_t> TypedAction<T>::run_counter{0};
-
-} // namespace frc971
-
-#endif // FRC971_ACTIONS_ACTION_CLIENT_H_
diff --git a/frc971/actions/actions.gyp b/frc971/actions/actions.gyp
index bcebc2c..4ee8119 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -1,30 +1,6 @@
{
'targets': [
{
- 'target_name': 'action_client',
- 'type': 'static_library',
- 'sources': [
- #'action_client.h',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:queues',
- ],
- },
- {
- 'target_name': 'action_queue',
- 'type': 'static_library',
- 'sources': ['action.q'],
- 'variables': {
- 'header_path': 'frc971/actions',
- },
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
'target_name': 'drivetrain_action_queue',
'type': 'static_library',
'sources': ['drivetrain_action.q'],
@@ -32,10 +8,10 @@
'header_path': 'frc971/actions',
},
'dependencies': [
- 'action_queue',
+ '<(AOS)/common/actions/actions.gyp:action_queue',
],
'export_dependent_settings': [
- 'action_queue',
+ '<(AOS)/common/actions/actions.gyp:action_queue',
],
'includes': ['../../aos/build/queues.gypi'],
},
@@ -43,54 +19,34 @@
'target_name': 'drivetrain_action_lib',
'type': 'static_library',
'sources': [
- 'drivetrain_action.cc',
+ 'drivetrain_actor.cc',
],
'dependencies': [
- 'drivetrain_action_queue',
- '<(DEPTH)/frc971/frc971.gyp:constants',
- '<(AOS)/common/common.gyp:time',
+ '<(EXTERNALS):eigen',
'<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/build/aos.gyp:logging',
- 'action_client',
- 'action',
- '<(EXTERNALS):eigen',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
- ],
- 'export_dependent_settings': [
- 'action',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'drivetrain_action_queue',
- 'action_client',
- ],
- },
- {
- 'target_name': 'action',
- 'type': 'static_library',
- 'sources': [
- #'action.h',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ 'drivetrain_action_queue',
],
},
{
'target_name': 'drivetrain_action',
'type': 'executable',
'sources': [
- 'drivetrain_action_main.cc',
+ 'drivetrain_actor_main.cc',
],
'dependencies': [
'<(AOS)/linux_code/linux_code.gyp:init',
'drivetrain_action_queue',
'drivetrain_action_lib',
- 'action',
],
},
],
diff --git a/frc971/actions/drivetrain_action.h b/frc971/actions/drivetrain_action.h
deleted file mode 100644
index 0a256b8..0000000
--- a/frc971/actions/drivetrain_action.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef FRC971_ACTIONS_DRIVETRAIN_ACTION_H_
-#define FRC971_ACTIONS_DRIVETRAIN_ACTION_H_
-
-#include <memory>
-
-#include "frc971/actions/drivetrain_action.q.h"
-#include "frc971/actions/action.h"
-#include "frc971/actions/action_client.h"
-
-namespace frc971 {
-namespace actions {
-
-class DrivetrainAction : public ActionBase<actions::DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainAction(actions::DrivetrainActionQueueGroup* s);
-
- virtual void RunAction();
-};
-
-// Makes a new DrivetrainAction action.
-::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
-MakeDrivetrainAction();
-
-} // namespace actions
-} // namespace frc971
-
-#endif
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
index 6045da0..5791318 100644
--- a/frc971/actions/drivetrain_action.q
+++ b/frc971/actions/drivetrain_action.q
@@ -1,9 +1,9 @@
package frc971.actions;
-import "frc971/actions/action.q";
+import "aos/common/actions/actions.q";
queue_group DrivetrainActionQueueGroup {
- implements frc971.actions.ActionQueueGroup;
+ implements aos.common.actions.ActionQueueGroup;
message Goal {
uint32_t run;
@@ -16,7 +16,7 @@
};
queue Goal goal;
- queue frc971.actions.Status status;
+ queue aos.common.actions.Status status;
};
queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/frc971/actions/drivetrain_action_main.cc b/frc971/actions/drivetrain_action_main.cc
deleted file mode 100644
index 0251e25..0000000
--- a/frc971/actions/drivetrain_action_main.cc
+++ /dev/null
@@ -1,19 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "aos/common/logging/logging.h"
-#include "frc971/actions/drivetrain_action.q.h"
-#include "frc971/actions/drivetrain_action.h"
-
-using ::aos::time::Time;
-
-int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
-
- frc971::actions::DrivetrainAction drivetrain(&::frc971::actions::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
-
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_actor.cc
similarity index 87%
rename from frc971/actions/drivetrain_action.cc
rename to frc971/actions/drivetrain_actor.cc
index 66e3d3c..71a1be8 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_actor.cc
@@ -1,3 +1,5 @@
+#include "frc971/actions/drivetrain_actor.h"
+
#include <functional>
#include <numeric>
@@ -7,18 +9,19 @@
#include "aos/common/logging/logging.h"
#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/commonmath.h"
+#include "aos/common/time.h"
-#include "frc971/actions/drivetrain_action.h"
+#include "frc971/actions/drivetrain_actor.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace frc971 {
namespace actions {
-DrivetrainAction::DrivetrainAction(actions::DrivetrainActionQueueGroup* s)
- : actions::ActionBase<actions::DrivetrainActionQueueGroup>(s) {}
+DrivetrainActor::DrivetrainActor(actions::DrivetrainActionQueueGroup* s)
+ : aos::common::actions::ActorBase<actions::DrivetrainActionQueueGroup>(s) {}
-void DrivetrainAction::RunAction() {
+void DrivetrainActor::RunAction() {
static const auto K = constants::GetValues().make_drivetrain_loop().K();
const double yoffset = action_q_->goal->y_offset;
@@ -27,6 +30,7 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
+ // TODO(sensors): update this time thing for some reason.
::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
const double goal_velocity = 0.0;
@@ -42,14 +46,14 @@
while (true) {
// wait until next 10ms tick
+ // TODO(sensors): update this time thing for some reason.
::aos::time::PhasedLoop10MS(5000);
control_loops::drivetrain_queue.status.FetchLatest();
if (control_loops::drivetrain_queue.status.get()) {
const auto& status = *control_loops::drivetrain_queue.status;
if (::std::abs(status.uncapped_left_voltage -
- status.uncapped_right_voltage) >
- 24) {
+ status.uncapped_right_voltage) > 24) {
LOG(DEBUG, "spinning in place\n");
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
@@ -151,11 +155,13 @@
LOG(INFO, "Done moving\n");
}
-::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+::std::unique_ptr<aos::common::actions::TypedAction<
+ ::frc971::actions::DrivetrainActionQueueGroup>>
MakeDrivetrainAction() {
- return ::std::unique_ptr<
- TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>(
- new TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>(
+ return ::std::unique_ptr<aos::common::actions::TypedAction<
+ ::frc971::actions::DrivetrainActionQueueGroup>>(
+ new aos::common::actions::TypedAction<
+ ::frc971::actions::DrivetrainActionQueueGroup>(
&::frc971::actions::drivetrain_action));
}
diff --git a/frc971/actions/drivetrain_actor.h b/frc971/actions/drivetrain_actor.h
new file mode 100644
index 0000000..f8e2a6e
--- /dev/null
+++ b/frc971/actions/drivetrain_actor.h
@@ -0,0 +1,28 @@
+#ifndef FRC971_ACTIONS_DRIVETRAIN_ACTION_H_
+#define FRC971_ACTIONS_DRIVETRAIN_ACTION_H_
+
+#include <memory>
+
+#include "frc971/actions/drivetrain_action.q.h"
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+
+namespace frc971 {
+namespace actions {
+
+class DrivetrainActor
+ : public aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
+ public:
+ explicit DrivetrainActor(DrivetrainActionQueueGroup* s);
+
+ void RunAction() override;
+};
+
+// Makes a new DrivetrainActor action.
+::std::unique_ptr<aos::common::actions::TypedAction<DrivetrainActionQueueGroup>>
+ MakeDrivetrainAction();
+
+} // namespace actions
+} // namespace frc971
+
+#endif
diff --git a/frc971/actions/drivetrain_actor_main.cc b/frc971/actions/drivetrain_actor_main.cc
new file mode 100644
index 0000000..9711065
--- /dev/null
+++ b/frc971/actions/drivetrain_actor_main.cc
@@ -0,0 +1,18 @@
+#include <stdio.h>
+
+#include "aos/linux_code/init.h"
+#include "frc971/actions/drivetrain_action.q.h"
+#include "frc971/actions/drivetrain_actor.h"
+
+using ::aos::time::Time;
+
+int main(int /*argc*/, char * /*argv*/[]) {
+ ::aos::Init();
+
+ frc971::actions::DrivetrainActor drivetrain(
+ &::frc971::actions::drivetrain_action);
+ drivetrain.Run();
+
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index aa63eb8..5d9733a 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -11,8 +11,7 @@
#include "frc971/autonomous/auto.q.h"
#include "frc971/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/actions/action_client.h"
-#include "frc971/actions/drivetrain_action.h"
+#include "frc971/actions/drivetrain_actor.h"
using ::aos::time::Time;
@@ -61,6 +60,7 @@
void DriveSpin(double radians) {
LOG(INFO, "going to spin %f\n", radians);
+ //TODO(sensors): update this time thing maybe?
::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
::Eigen::Matrix<double, 2, 1> driveTrainState;
const double goal_velocity = 0.0;
@@ -74,7 +74,7 @@
const double side_offset = kRobotWidth * radians / 2.0;
while (true) {
- ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
+ ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
driveTrainState = profile.Update(side_offset, goal_velocity);
if (::std::abs(driveTrainState(0, 0) - side_offset) < epsilon) break;
@@ -97,7 +97,7 @@
LOG(INFO, "Done moving\n");
}
-void WaitUntilDoneOrCanceled(Action *action) {
+void WaitUntilDoneOrCanceled(aos::common::actions::Action *action) {
while (true) {
// Poll the running bit and auto done bits.
::aos::time::PhasedLoop10MS(5000);
@@ -107,7 +107,8 @@
}
}
-::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
+::std::unique_ptr<aos::common::actions::TypedAction<
+ ::frc971::actions::DrivetrainActionQueueGroup>>
SetDriveGoal(double distance, bool slow_acceleration,
double maximum_velocity = 1.7, double theta = 0) {
LOG(INFO, "Driving to %f\n", distance);
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 4dd0320..966884f 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -24,7 +24,6 @@
'<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
'<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/actions/actions.gyp:action_client',
'<(DEPTH)/frc971/actions/actions.gyp:drivetrain_action_lib',
'<(AOS)/common/logging/logging.gyp:queue_logging',
],
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index cca8066..f6006d0 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -34,7 +34,7 @@
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
- '<(DEPTH)/frc971/actions/actions.gyp:action_client',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
],
},
],
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 0f781a9..d2c5253 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -9,12 +9,12 @@
#include "aos/common/logging/logging.h"
#include "aos/common/util/log_interval.h"
#include "aos/common/time.h"
+#include "aos/common/actions/actions.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
-#include "frc971/actions/action_client.h"
using ::frc971::control_loops::drivetrain_queue;
using ::frc971::sensors::gyro_reading;
@@ -33,63 +33,6 @@
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-// A queue which queues Actions and cancels them.
-class ActionQueue {
- public:
- // Queues up an action for sending.
- void QueueAction(::std::unique_ptr<Action> action) {
- if (current_action_) {
- LOG(INFO, "Queueing action, canceling prior\n");
- current_action_->Cancel();
- next_action_ = ::std::move(action);
- } else {
- LOG(INFO, "Queueing action\n");
- current_action_ = ::std::move(action);
- current_action_->Start();
- }
- }
-
- // Cancels the current action, and runs the next one when the current one has
- // finished.
- void CancelCurrentAction() {
- LOG(INFO, "Canceling current action\n");
- if (current_action_) {
- current_action_->Cancel();
- }
- }
-
- // Cancels all running actions.
- void CancelAllActions() {
- LOG(DEBUG, "Cancelling all actions\n");
- if (current_action_) {
- current_action_->Cancel();
- }
- next_action_.reset();
- }
-
- // Runs the next action when the current one is finished running.
- void Tick() {
- if (current_action_) {
- if (!current_action_->Running()) {
- LOG(INFO, "Action is done.\n");
- current_action_ = ::std::move(next_action_);
- if (current_action_) {
- LOG(INFO, "Running next action\n");
- current_action_->Start();
- }
- }
- }
- }
-
- // Returns true if any action is running or could be running.
- // For a one cycle faster response, call Tick before running this.
- bool Running() { return static_cast<bool>(current_action_); }
-
- private:
- ::std::unique_ptr<Action> current_action_;
- ::std::unique_ptr<Action> next_action_;
-};
-
class Reader : public ::aos::input::JoystickInput {
public:
@@ -208,7 +151,7 @@
bool auto_running_ = false;
- ActionQueue action_queue_;
+ aos::common::actions::ActionQueue action_queue_;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index c96dc3a..e8b0eb5 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -19,6 +19,7 @@
'../frc971.gyp:joystick_reader',
'../zeroing/zeroing.gyp:zeroing_test',
'../control_loops/voltage_cap/voltage_cap.gyp:voltage_cap_test',
+ '../../aos/common/actions/actions.gyp:action_test',
],
'copies': [
{
diff --git a/frc971/prime/start_list.txt b/frc971/prime/start_list.txt
index d13d0d1..9940679 100644
--- a/frc971/prime/start_list.txt
+++ b/frc971/prime/start_list.txt
@@ -4,8 +4,5 @@
drivetrain
auto
claw
-shooter
-shoot_action
+fridge
drivetrain_action
-catch_action
-hot_goal_reader