Merge "Zeroed the fridge state on initialization."
diff --git a/aos/common/actions/action_test.cc b/aos/common/actions/action_test.cc
new file mode 100644
index 0000000..df204bc
--- /dev/null
+++ b/aos/common/actions/action_test.cc
@@ -0,0 +1,286 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/actions/actor.h"
+#include "aos/common/actions/actions.h"
+#include "aos/common/actions/actions.q.h"
+#include "aos/common/actions/test_action.q.h"
+
+using ::aos::time::Time;
+
+namespace aos {
+namespace common {
+namespace actions {
+namespace testing {
+
+class TestActorNOP
+ : public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
+ public:
+ explicit TestActorNOP(actions::TestActionQueueGroup* s)
+ : actions::ActorBase<actions::TestActionQueueGroup>(s) {}
+
+ void RunAction() { return; }
+};
+
+::std::unique_ptr<
+ aos::common::actions::TypedAction<actions::TestActionQueueGroup>>
+MakeTestActionNOP() {
+ return ::std::unique_ptr<
+ aos::common::actions::TypedAction<actions::TestActionQueueGroup>>(
+ new aos::common::actions::TypedAction<actions::TestActionQueueGroup>(
+ &actions::test_action));
+}
+
+class TestActorShouldCancel
+ : public aos::common::actions::ActorBase<actions::TestActionQueueGroup> {
+ public:
+ explicit TestActorShouldCancel(actions::TestActionQueueGroup* s)
+ : aos::common::actions::ActorBase<actions::TestActionQueueGroup>(s) {}
+
+ void RunAction() {
+ while (!ShouldCancel()) {
+ LOG(FATAL, "NOT CANCELED!!\n");
+ }
+ return;
+ }
+};
+
+::std::unique_ptr<
+ aos::common::actions::TypedAction<actions::TestActionQueueGroup>>
+MakeTestActionShouldCancel() {
+ return ::std::unique_ptr<
+ aos::common::actions::TypedAction<actions::TestActionQueueGroup>>(
+ new aos::common::actions::TypedAction<actions::TestActionQueueGroup>(
+ &actions::test_action));
+}
+
+class ActionTest : public ::testing::Test {
+ protected:
+ ActionTest() {
+ // Flush the robot state queue so we can use clean shared memory for this.
+ // test.
+ actions::test_action.goal.Clear();
+ actions::test_action.status.Clear();
+ }
+
+ virtual ~ActionTest() {
+ actions::test_action.goal.Clear();
+ actions::test_action.status.Clear();
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+ ::aos::common::actions::ActionQueue action_queue_;
+};
+
+// Tests that the the actions exist in a safe state at startup.
+TEST_F(ActionTest, DoesNothing) {
+ // Tick an empty queue and make sure it was not running.
+ EXPECT_FALSE(action_queue_.Running());
+ action_queue_.Tick();
+ EXPECT_FALSE(action_queue_.Running());
+}
+
+// Tests that the queues are properly configured for testing. Tests that queues
+// work exactly as used in the tests.
+TEST_F(ActionTest, QueueCheck) {
+ actions::TestActionQueueGroup* send_side = &actions::test_action;
+ actions::TestActionQueueGroup* recv_side = &actions::test_action;
+
+ send_side->goal.MakeMessage();
+ send_side->goal.MakeWithBuilder().run(1).Send();
+
+ EXPECT_TRUE(recv_side->goal.FetchLatest());
+ EXPECT_TRUE(recv_side->goal->run);
+
+ send_side->goal.MakeWithBuilder().run(0).Send();
+
+ EXPECT_TRUE(recv_side->goal.FetchLatest());
+ EXPECT_FALSE(recv_side->goal->run);
+
+ send_side->status.MakeMessage();
+ send_side->status.MakeWithBuilder().running(5).last_running(6).Send();
+
+ EXPECT_TRUE(recv_side->status.FetchLatest());
+ EXPECT_EQ(5, static_cast<int>(recv_side->status->running));
+ EXPECT_EQ(6, static_cast<int>(recv_side->status->last_running));
+}
+
+// Tests that an action starts and stops.
+TEST_F(ActionTest, ActionQueueWasRunning) {
+ TestActorNOP nop_act(&actions::test_action);
+
+ // Tick an empty queue and make sure it was not running.
+ action_queue_.Tick();
+ EXPECT_FALSE(action_queue_.Running());
+
+ action_queue_.EnqueueAction(MakeTestActionNOP());
+ nop_act.WaitForActionRequest();
+
+ // We started an action and it should be running.
+ EXPECT_TRUE(action_queue_.Running());
+
+ // Tick it and make sure it is still running.
+ action_queue_.Tick();
+ EXPECT_TRUE(action_queue_.Running());
+
+ // Run the action so it can signal completion.
+ nop_act.RunIteration();
+ action_queue_.Tick();
+
+ // Make sure it stopped.
+ EXPECT_FALSE(action_queue_.Running());
+}
+
+// Tests that we can cancel two actions and have them both stop.
+TEST_F(ActionTest, ActionQueueCancelAll) {
+ TestActorNOP nop_act(&actions::test_action);
+
+ // Tick an empty queue and make sure it was not running.
+ action_queue_.Tick();
+ EXPECT_FALSE(action_queue_.Running());
+
+ // Enqueue two actions to test both cancel. We can have an action and a next
+ // action so we want to test that.
+ action_queue_.EnqueueAction(MakeTestActionNOP());
+ action_queue_.EnqueueAction(MakeTestActionNOP());
+ nop_act.WaitForActionRequest();
+ action_queue_.Tick();
+
+ // Check that current and next exist.
+ EXPECT_TRUE(action_queue_.GetCurrentActionState(nullptr, nullptr, nullptr,
+ nullptr, nullptr, nullptr));
+ EXPECT_TRUE(action_queue_.GetNextActionState(nullptr, nullptr, nullptr,
+ nullptr, nullptr, nullptr));
+
+ action_queue_.CancelAllActions();
+ action_queue_.Tick();
+
+ // It should still be running as the actor could not have signaled.
+ EXPECT_TRUE(action_queue_.Running());
+
+ bool sent_started, sent_cancel, interrupted;
+ EXPECT_TRUE(action_queue_.GetCurrentActionState(
+ nullptr, &sent_started, &sent_cancel, &interrupted, nullptr, nullptr));
+ EXPECT_TRUE(sent_started);
+ EXPECT_TRUE(sent_cancel);
+ EXPECT_FALSE(interrupted);
+
+ EXPECT_FALSE(action_queue_.GetNextActionState(nullptr, nullptr, nullptr,
+ nullptr, nullptr, nullptr));
+
+ // Run the action so it can signal completion.
+ nop_act.RunIteration();
+ action_queue_.Tick();
+
+ // Make sure it stopped.
+ EXPECT_FALSE(action_queue_.Running());
+}
+
+// Tests that an action that would block forever stops when canceled.
+TEST_F(ActionTest, ActionQueueCancelOne) {
+ TestActorShouldCancel cancel_act(&actions::test_action);
+
+ // Enqueue blocking action.
+ action_queue_.EnqueueAction(MakeTestActionShouldCancel());
+
+ cancel_act.WaitForActionRequest();
+ action_queue_.Tick();
+ EXPECT_TRUE(action_queue_.Running());
+
+ // Tell action to cancel.
+ action_queue_.CancelCurrentAction();
+ action_queue_.Tick();
+
+ // This will block forever on failure.
+ // TODO(ben): prolly a bad way to fail
+ cancel_act.RunIteration();
+ action_queue_.Tick();
+
+ // It should still be running as the actor could not have signalled.
+ EXPECT_FALSE(action_queue_.Running());
+}
+
+// Tests that an action starts and stops.
+TEST_F(ActionTest, ActionQueueTwoActions) {
+ TestActorNOP nop_act(&actions::test_action);
+
+ // Tick an empty queue and make sure it was not running.
+ action_queue_.Tick();
+ EXPECT_FALSE(action_queue_.Running());
+
+ // Enqueue action to be canceled.
+ action_queue_.EnqueueAction(MakeTestActionNOP());
+ nop_act.WaitForActionRequest();
+ action_queue_.Tick();
+
+ // Should still be running as the actor could not have signalled.
+ EXPECT_TRUE(action_queue_.Running());
+
+ // id for the first time run.
+ uint32_t nop_act_id = 0;
+ // Check the internal state and write down id for later use.
+ bool sent_started, sent_cancel, interrupted;
+ EXPECT_TRUE(action_queue_.GetCurrentActionState(nullptr, &sent_started,
+ &sent_cancel, &interrupted,
+ &nop_act_id, nullptr));
+ EXPECT_TRUE(sent_started);
+ EXPECT_FALSE(sent_cancel);
+ EXPECT_FALSE(interrupted);
+ ASSERT_NE(0u, nop_act_id);
+
+ // Add the next action which should ensure the first stopped.
+ action_queue_.EnqueueAction(MakeTestActionNOP());
+
+ // id for the second run.
+ uint32_t nop_act2_id = 0;
+ // Check the internal state and write down id for later use.
+ EXPECT_TRUE(action_queue_.GetNextActionState(nullptr, &sent_started,
+ &sent_cancel, &interrupted,
+ &nop_act2_id, nullptr));
+ EXPECT_NE(nop_act_id, nop_act2_id);
+ EXPECT_FALSE(sent_started);
+ EXPECT_FALSE(sent_cancel);
+ EXPECT_FALSE(interrupted);
+ ASSERT_NE(0u, nop_act2_id);
+
+ action_queue_.Tick();
+
+ // Run the action so it can signal completion.
+ nop_act.RunIteration();
+ action_queue_.Tick();
+ // Wait for the first id to finish, needed for the correct number of fetches.
+ nop_act.WaitForStop(nop_act_id);
+
+ // Start the next action on the actor side.
+ nop_act.WaitForActionRequest();
+
+ // Check the new action is the right one.
+ uint32_t test_id = 0;
+ EXPECT_TRUE(action_queue_.GetCurrentActionState(
+ nullptr, &sent_started, &sent_cancel, &interrupted, &test_id, nullptr));
+ EXPECT_TRUE(sent_started);
+ EXPECT_FALSE(sent_cancel);
+ EXPECT_FALSE(interrupted);
+ EXPECT_EQ(nop_act2_id, test_id);
+
+ // Make sure it is still going.
+ EXPECT_TRUE(action_queue_.Running());
+
+ // Run the next action so it can accomplish signal completion.
+ nop_act.RunIteration();
+ action_queue_.Tick();
+ nop_act.WaitForStop(nop_act_id);
+
+ // Make sure it stopped.
+ EXPECT_FALSE(action_queue_.Running());
+}
+
+} // namespace testing.
+} // namespace actions.
+} // namespace common.
+} // namespace aos.
diff --git a/aos/common/actions/actions.cc b/aos/common/actions/actions.cc
new file mode 100644
index 0000000..ff3f386
--- /dev/null
+++ b/aos/common/actions/actions.cc
@@ -0,0 +1,75 @@
+#include "aos/common/actions/actions.h"
+
+namespace aos {
+namespace common {
+namespace actions {
+
+void ActionQueue::EnqueueAction(::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();
+ }
+}
+
+void ActionQueue::CancelCurrentAction() {
+ LOG(INFO, "Canceling current action\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+}
+
+void ActionQueue::CancelAllActions() {
+ LOG(DEBUG, "Cancelling all actions\n");
+ if (current_action_) {
+ current_action_->Cancel();
+ }
+ next_action_.reset();
+}
+
+void ActionQueue::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();
+ }
+ }
+ }
+}
+
+bool ActionQueue::Running() { return static_cast<bool>(current_action_); }
+
+bool ActionQueue::GetCurrentActionState(bool* has_started, bool* sent_started,
+ bool* sent_cancel, bool* interrupted,
+ uint32_t* run_value,
+ uint32_t* old_run_value) {
+ if (current_action_) {
+ current_action_->GetState(has_started, sent_started, sent_cancel,
+ interrupted, run_value, old_run_value);
+ return true;
+ }
+ return false;
+}
+
+bool ActionQueue::GetNextActionState(bool* has_started, bool* sent_started,
+ bool* sent_cancel, bool* interrupted,
+ uint32_t* run_value,
+ uint32_t* old_run_value) {
+ if (next_action_) {
+ next_action_->GetState(has_started, sent_started, sent_cancel, interrupted,
+ run_value, old_run_value);
+ return true;
+ }
+ return false;
+}
+
+} // namespace actions
+} // namespace common
+} // namespace aos
diff --git a/aos/common/actions/actions.gyp b/aos/common/actions/actions.gyp
new file mode 100644
index 0000000..68c52b2
--- /dev/null
+++ b/aos/common/actions/actions.gyp
@@ -0,0 +1,66 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'action_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'actions.cc',
+ 'actor.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/common.gyp:time'
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/common.gyp:time'
+ ],
+ },
+ {
+ 'target_name': 'action_queue',
+ 'type': 'static_library',
+ 'sources': ['actions.q'],
+ 'variables': {
+ 'header_path': 'aos/common/actions',
+ },
+ 'includes': ['../../build/queues.gypi'],
+ },
+ {
+ 'target_name': 'test_action_queue',
+ 'type': 'static_library',
+ 'sources': ['test_action.q'],
+ 'variables': {
+ 'header_path': 'aos/common/actions',
+ },
+ 'dependencies': [
+ 'action_queue',
+ ],
+ 'export_dependent_settings': [
+ 'action_queue',
+ ],
+ 'includes': ['../../build/queues.gypi'],
+ },
+ {
+ 'target_name': 'action_test',
+ 'type': 'executable',
+ 'sources': [
+ 'action_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'action_lib',
+ 'test_action_queue',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ 'action_queue'
+ ],
+ },
+ ],
+}
diff --git a/aos/common/actions/actions.h b/aos/common/actions/actions.h
new file mode 100644
index 0000000..08d94f7
--- /dev/null
+++ b/aos/common/actions/actions.h
@@ -0,0 +1,307 @@
+#ifndef AOS_COMMON_ACTIONS_ACTIONS_H_
+#define AOS_COMMON_ACTIONS_ACTIONS_H_
+
+#include <inttypes.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <type_traits>
+#include <atomic>
+#include <memory>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/queue.h"
+
+namespace aos {
+namespace common {
+namespace actions {
+
+class Action;
+
+// A queue which queues Actions, verifies a single action is running, and
+// cancels Actions.
+class ActionQueue {
+ public:
+ // Queues up an action for sending.
+ void EnqueueAction(::std::unique_ptr<Action> action);
+
+ // Cancels the current action, and runs the next one after the current one has
+ // finished and the action queue ticks again.
+ void CancelCurrentAction();
+
+ // Cancels all running actions.
+ void CancelAllActions();
+
+ // Runs the next action when the current one is finished running and handles
+ // periodically updating various other information.
+ void Tick();
+
+ // Returns true if any action is running or could be running.
+ // For a one cycle faster response, call Tick before running this.
+ // Daniel can think of at least one case (current_action_ is cancelled and
+ // there is no next_action_) in which calling this without running Tick()
+ // first would return a wrong answer.
+ bool Running();
+
+ // Retrieves the internal state of the current action for testing.
+ // See comments on the private members of TypedAction<T> for details.
+ bool GetCurrentActionState(bool* has_started, bool* sent_started,
+ bool* sent_cancel, bool* interrupted,
+ uint32_t* run_value, uint32_t* old_run_value);
+
+ // Retrieves the internal state of the next action for testing.
+ // See comments on the private members of TypedAction<T> for details.
+ bool GetNextActionState(bool* has_started, bool* sent_started,
+ bool* sent_cancel, bool* interrupted,
+ uint32_t* run_value, uint32_t* old_run_value);
+
+ private:
+ ::std::unique_ptr<Action> current_action_;
+ ::std::unique_ptr<Action> next_action_;
+};
+
+// The basic interface an ActionQueue can access.
+class Action {
+ public:
+ virtual ~Action() {}
+
+ // 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(); }
+
+ // Retrieves the internal state of the action for testing.
+ // See comments on the private members of TypedAction<T> for details.
+ void GetState(bool* has_started, bool* sent_started, bool* sent_cancel,
+ bool* interrupted, uint32_t* run_value,
+ uint32_t* old_run_value) {
+ DoGetState(has_started, sent_started, sent_cancel, interrupted, run_value,
+ old_run_value);
+ }
+
+ private:
+ // Cancels the action.
+ virtual void DoCancel() = 0;
+ // 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() = 0;
+ // Starts the action if a goal has been created.
+ virtual void DoStart() = 0;
+ // Blocks until complete.
+ virtual void DoWaitUntilDone() = 0;
+ // For testing we will need to get the internal state.
+ // See comments on the private members of TypedAction<T> for details.
+ virtual void DoGetState(bool* has_started, bool* sent_started,
+ bool* sent_cancel, bool* interrupted,
+ uint32_t* run_value, uint32_t* old_run_value) = 0;
+};
+
+// Templated subclass to hold the type information.
+template <typename T>
+class TypedAction : public Action {
+ public:
+ // A convenient way to refer to the type of our goals.
+ 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()),
+ // This adds 1 to the counter (atomically because it's potentially
+ // shared across threads) and then bitwise-ORs the bottom of the PID to
+ // differentiate it from other processes's values (ie a unique id).
+ 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());
+ // Clear out any old status messages from before now.
+ queue_group_->status.FetchLatest();
+ }
+
+ virtual ~TypedAction() {
+ LOG(DEBUG, "Calling destructor of %" PRIx32 "\n", run_value_);
+ DoCancel();
+ }
+
+ // Returns the current goal that will be sent when the action is sent.
+ GoalType* GetGoal() { return goal_.get(); }
+
+ private:
+ void DoCancel() override;
+
+ bool DoRunning() override;
+
+ void DoWaitUntilDone() override;
+
+ // Sets the started flag (also possibly the interrupted flag).
+ void CheckStarted();
+
+ // Checks for interrupt.
+ void CheckInterrupted();
+
+ void DoStart() override;
+
+ void DoGetState(bool* has_started, bool* sent_started, bool* sent_cancel,
+ bool* interrupted, uint32_t* run_value,
+ uint32_t* old_run_value) override {
+ if (has_started != nullptr) *has_started = has_started_;
+ if (sent_started != nullptr) *sent_started = sent_started_;
+ if (sent_cancel != nullptr) *sent_cancel = sent_cancel_;
+ if (interrupted != nullptr) *interrupted = interrupted_;
+ if (run_value != nullptr) *run_value = run_value_;
+ if (old_run_value != nullptr) *old_run_value = old_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};
+
+template <typename T>
+void TypedAction<T>::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;
+ }
+ }
+ }
+}
+
+template <typename T>
+bool TypedAction<T>::DoRunning() {
+ if (!sent_started_) return false;
+ if (has_started_) {
+ queue_group_->status.FetchNext();
+ CheckInterrupted();
+ } else if (queue_group_->status.FetchLatest()) {
+ CheckStarted();
+ }
+ if (interrupted_) return false;
+ // We've asked it to start but haven't gotten confirmation that it's started
+ // yet.
+ if (!has_started_) return true;
+ return queue_group_->status.get() &&
+ queue_group_->status->running == run_value_;
+}
+
+template <typename T>
+void TypedAction<T>::DoWaitUntilDone() {
+ CHECK(sent_started_);
+ queue_group_->status.FetchNext();
+ 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;
+ }
+ }
+}
+
+template <typename T>
+void TypedAction<T>::CheckStarted() {
+ if (has_started_) return;
+ if (queue_group_->status.get()) {
+ if (queue_group_->status->running == run_value_ ||
+ (queue_group_->status->running == 0 &&
+ queue_group_->status->last_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;
+ }
+ } else {
+ LOG(WARNING, "No status message recieved.\n");
+ }
+}
+
+template <typename T>
+void TypedAction<T>::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);
+ }
+ }
+}
+
+template <typename T>
+void TypedAction<T>::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() && queue_group_->status->running != 0) {
+ 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_);
+ }
+}
+
+} // namespace actions
+} // namespace common
+} // namespace aos
+
+#endif // AOS_COMMON_ACTIONS_ACTIONS_H_
diff --git a/aos/common/actions/actions.q b/aos/common/actions/actions.q
new file mode 100644
index 0000000..fff6326
--- /dev/null
+++ b/aos/common/actions/actions.q
@@ -0,0 +1,29 @@
+package aos.common.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 {
+ // The run value of the instance we're currently running or 0.
+ uint32_t running;
+ // A run value we were previously running or 0.
+ uint32_t last_running;
+};
+
+message Goal {
+ // The unique value to put into status.running while running this instance or
+ // 0 to cancel.
+ uint32_t run;
+};
+
+interface ActionQueueGroup {
+ queue Status status;
+ queue Goal goal;
+};
diff --git a/aos/common/actions/actor.cc b/aos/common/actions/actor.cc
new file mode 100644
index 0000000..6c6bef7
--- /dev/null
+++ b/aos/common/actions/actor.cc
@@ -0,0 +1,9 @@
+#include "aos/common/actions/actor.h"
+
+namespace aos {
+namespace common {
+namespace actions {
+
+} // namespace actions
+} // namespace common
+} // namespace aos
diff --git a/aos/common/actions/actor.h b/aos/common/actions/actor.h
new file mode 100644
index 0000000..958927b
--- /dev/null
+++ b/aos/common/actions/actor.h
@@ -0,0 +1,204 @@
+#ifndef AOS_COMMON_ACTIONS_ACTOR_H_
+#define AOS_COMMON_ACTIONS_ACTOR_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 aos {
+namespace common {
+namespace actions {
+
+template <class T>
+class ActorBase {
+ public:
+ ActorBase(T* acq) : action_q_(acq) {}
+
+ virtual void RunAction() = 0;
+
+ // Runs action while enabled.
+ void Run();
+
+ // Checks if an action was initially running when the thread started.
+ bool CheckInitialRunning();
+
+ // Wait here until someone asks us to go.
+ void WaitForActionRequest();
+
+ // Do work son.
+ uint32_t RunIteration();
+
+ // Wait for stop is signalled.
+ void WaitForStop(uint32_t 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 (such as 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));
+
+ // Returns true if the action should be canceled.
+ bool ShouldCancel();
+
+ protected:
+ // Set to true when we should stop ASAP.
+ bool abort_ = false;
+
+ // The queue for this action.
+ T* action_q_;
+};
+
+template <class T>
+bool ActorBase<T>::CheckInitialRunning() {
+ LOG(DEBUG, "Waiting for input to start\n");
+
+ if (action_q_->goal.FetchLatest()) {
+ 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);
+ }
+ }
+ return true;
+ }
+ return false;
+}
+
+template <class T>
+void ActorBase<T>::WaitForActionRequest() {
+ while (action_q_->goal.get() == nullptr || !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)
+ .last_running(0)
+ .Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+ }
+ }
+}
+
+template <class T>
+uint32_t ActorBase<T>::RunIteration() {
+ CHECK(action_q_->goal.get() != nullptr);
+ 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)
+ .last_running(0)
+ .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)
+ .last_running(running_id)
+ .Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ } else {
+ LOG(INFO, "Sending Done status %" PRIx32 "\n", running_id);
+ }
+ } else {
+ LOG(INFO, "skipping sending stopped status for %" PRIx32 "\n", running_id);
+ }
+
+ return running_id;
+}
+
+template <class T>
+void ActorBase<T>::WaitForStop(uint32_t running_id) {
+ assert(action_q_->goal.get() != nullptr);
+ 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);
+ }
+}
+
+template <class T>
+void ActorBase<T>::Run() {
+
+ // Make sure the last job is done and we have a signal.
+ CheckInitialRunning();
+
+ if (!action_q_->status.MakeWithBuilder().running(0).last_running(0).Send()) {
+ LOG(ERROR, "Failed to send the status.\n");
+ }
+
+ LOG(DEBUG, "action %" PRIx32 " was stopped\n", action_q_->goal->run);
+
+ while (true) {
+ // Wait for a request to come in before starting.
+ WaitForActionRequest();
+
+ // Perform the action once.
+ uint32_t running_id = RunIteration();
+
+ // Don't start again until asked.
+ WaitForStop(running_id);
+ LOG(DEBUG, "action %" PRIx32 " was stopped\n", running_id);
+ }
+}
+
+template <class T>
+bool ActorBase<T>::WaitUntil(::std::function<bool(void)> done_condition,
+ const ::aos::time::Time& end_time) {
+ 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;
+ }
+}
+
+template <class T>
+bool ActorBase<T>::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;
+}
+
+} // namespace actions
+} // namespace common
+} // namespace aos
+
+#endif // AOS_COMMON_ACTIONS_ACTOR_H_
diff --git a/aos/common/actions/test_action.q b/aos/common/actions/test_action.q
new file mode 100644
index 0000000..c46e182
--- /dev/null
+++ b/aos/common/actions/test_action.q
@@ -0,0 +1,17 @@
+package aos.common.actions;
+
+import "aos/common/actions/actions.q";
+
+queue_group TestActionQueueGroup {
+ implements aos.common.actions.ActionQueueGroup;
+
+ message Goal {
+ uint32_t run;
+ double test_value;
+ };
+
+ queue Goal goal;
+ queue aos.common.actions.Status status;
+};
+
+queue_group TestActionQueueGroup test_action;
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 42702c0..bf2eea4 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -11,22 +11,21 @@
// TODO(aschuh): Tests.
-template <class T, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kStaleLogInterval;
-template <class T, bool fail_no_goal>
-constexpr ::aos::time::Time ControlLoop<T, fail_no_goal>::kPwmDisableTime;
+template <class T>
+constexpr ::aos::time::Time ControlLoop<T>::kStaleLogInterval;
+template <class T>
+constexpr ::aos::time::Time ControlLoop<T>::kPwmDisableTime;
-template <class T, bool fail_no_goal>
-void
-ControlLoop<T, fail_no_goal>::ZeroOutputs() {
+template <class T>
+void ControlLoop<T>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool fail_no_goal>
-void ControlLoop<T, fail_no_goal>::Iterate() {
+template <class T>
+void ControlLoop<T>::Iterate() {
no_goal_.Print();
driver_station_old_.Print();
no_sensor_state_.Print();
@@ -49,10 +48,6 @@
LOG_STRUCT(DEBUG, "goal", *goal);
} else {
LOG_INTERVAL(no_goal_);
- if (fail_no_goal) {
- ZeroOutputs();
- return;
- }
}
::aos::robot_state.FetchLatest();
@@ -130,8 +125,8 @@
status.Send();
}
-template <class T, bool fail_no_goal>
-void ControlLoop<T, fail_no_goal>::Run() {
+template <class T>
+void ControlLoop<T>::Run() {
while (true) {
Iterate();
}
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index a176fb6..d48f017 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -44,7 +44,7 @@
// that has a goal, position, status, and output queue.
// It will then call the RunIteration method every cycle that it has enough
// valid data for the control loop to run.
-template <class T, bool fail_no_goal = true>
+template <class T>
class ControlLoop : public SerializableControlLoop {
public:
// Create some convenient typedefs to reference the Goal, Position, Status,
@@ -116,11 +116,12 @@
protected:
// Runs an iteration of the control loop.
- // goal is the last goal that was sent. It might be any number of cycles old.
- // position is the current position, or NULL if we didn't get a position this
- // cycle.
- // output is the values to be sent to the motors. This is NULL if the output
- // is going to be ignored and set to 0.
+ // goal is the last goal that was sent. It might be any number of cycles old
+ // or nullptr if we haven't ever received a goal.
+ // position is the current position, or nullptr if we didn't get a position
+ // this cycle.
+ // output is the values to be sent to the motors. This is nullptr if the
+ // output is going to be ignored and set to 0.
// status is the status of the control loop.
// Both output and status should be filled in by the implementation.
virtual void RunIteration(const GoalType *goal,
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/constants.cc b/frc971/constants.cc
index b0cbc6b..b64f52b 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -116,6 +116,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kClawEncoderIndexDifference, 0.0},
+ 0.0,
},
{
@@ -139,6 +140,7 @@
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ 0.0, 0.0, 0.0, 0.0,
},
// End "sensor" values.
@@ -180,7 +182,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kClawEncoderIndexDifference, 0.0},
-
+ 0.0,
},
{
@@ -203,6 +205,7 @@
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ 0.0, 0.0, 0.0, 0.0,
},
// End "sensor" values.
@@ -244,6 +247,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kClawEncoderIndexDifference, 0.0},
+ 0.0,
},
{
@@ -266,6 +270,7 @@
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ 0.0, 0.0, 0.0, 0.0,
},
// TODO(sensors): End "sensor" values.
diff --git a/frc971/constants.h b/frc971/constants.h
index 883e1b1..5012817 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -11,6 +11,10 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
+// Everything is in SI units (volts, radians, meters, seconds, etc).
+// Some of these values are related to the conversion between raw values
+// (encoder counts, voltage, etc) to scaled units (radians, meters, etc).
+
// This structure contains current values for all of the things that change.
struct Values {
// Drivetrain Values /////
@@ -53,13 +57,14 @@
struct ZeroingConstants {
// The number of samples in the moving average filter.
int average_filter_size;
- // The difference in SI units between two index pulses.
+ // The difference in scaled units between two index pulses.
double index_difference;
- // The absolute position in SI units of one of the index pulses.
+ // The absolute position in scaled units of one of the index pulses.
double measured_index_position;
};
// Defines a range of motion for a subsystem.
+ // These are all absolute positions in scaled units.
struct Range {
double lower_hard_limit;
double upper_hard_limit;
@@ -70,6 +75,10 @@
struct Claw {
Range wrist;
ZeroingConstants zeroing;
+ // The value to add to potentiometer readings after they have been converted
+ // to radians so that the resulting value is 0 when the claw is at absolute
+ // 0 (horizontal straight out the front).
+ double potentiometer_offset;
};
Claw claw;
@@ -81,6 +90,13 @@
ZeroingConstants right_elev_zeroing;
ZeroingConstants left_arm_zeroing;
ZeroingConstants right_arm_zeroing;
+
+ // Values to add to scaled potentiometer readings so 0 lines up with the
+ // physical absolute 0.
+ double left_elevator_potentiometer_offset;
+ double right_elevator_potentiometer_offset;
+ double left_arm_potentiometer_offset;
+ double right_arm_potentiometer_offset;
};
Fridge fridge;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 4f86697..9c0553c 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -124,7 +124,7 @@
// Tests that the loop does nothing when the goal is our lower limit.
TEST_F(ClawTest, DoesNothing) {
- const auto values = constants::GetValues();
+ const auto &values = constants::GetValues();
ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
.angle(values.claw.wrist.lower_limit)
.Send());
@@ -148,7 +148,7 @@
// Tests that it doesn't try to move past the physical range of the mechanism.
TEST_F(ClawTest, RespectsRange) {
- const auto values = constants::GetValues();
+ const auto &values = constants::GetValues();
// Upper limit
ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
.angle(values.claw.wrist.upper_hard_limit + 5.0)
@@ -174,6 +174,17 @@
2.0 * M_PI / 180.0);*/
}
+// Tests that not having a goal doesn't break anything.
+TEST_F(ClawTest, NoGoal) {
+ for (int i = 0; i < 10; ++i) {
+ claw_plant_.SendPositionMessage();
+ claw_.Iterate();
+ claw_plant_.Simulate();
+
+ TickTime();
+ }
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 395f8b9..bcfc6a1 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -689,19 +689,28 @@
}
no_position_.Print();
- double wheel = goal->steering;
- double throttle = goal->throttle;
- bool quickturn = goal->quickturn;
+ bool control_loop_driving = false;
+ if (goal) {
+ double wheel = goal->steering;
+ double throttle = goal->throttle;
+ bool quickturn = goal->quickturn;
#if HAVE_SHIFTERS
- bool highgear = goal->highgear;
+ bool highgear = goal->highgear;
#endif
- bool control_loop_driving = goal->control_loop_driving;
- double left_goal = goal->left_goal;
- double right_goal = goal->right_goal;
+ control_loop_driving = goal->control_loop_driving;
+ double left_goal = goal->left_goal;
+ double right_goal = goal->right_goal;
- dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
- goal->right_velocity_goal);
+ dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+ goal->right_velocity_goal);
+#if HAVE_SHIFTERS
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+#else
+ dt_openloop.SetGoal(wheel, throttle, quickturn, false);
+#endif
+ }
+
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
@@ -714,11 +723,6 @@
}
}
dt_openloop.SetPosition(position);
-#if HAVE_SHIFTERS
- dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
-#else
- dt_openloop.SetGoal(wheel, throttle, quickturn, false);
-#endif
dt_openloop.Update();
if (control_loop_driving) {
@@ -732,7 +736,7 @@
}
dt_closedloop.Update(output == NULL, false);
}
-
+
// set the output status of the control loop state
if (status) {
bool done = false;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 96583da..7e14cf0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -170,6 +170,26 @@
VerifyNearGoal();
}
+// Tests that never having a goal doesn't break.
+TEST_F(DrivetrainTest, NoGoalStart) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ }
+}
+
+// Tests that never having a goal, but having driver's station messages, doesn't
+// break.
+TEST_F(DrivetrainTest, NoGoalWithRobotState) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ }
+}
+
::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 9fbd87c..3347da9 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -56,7 +56,7 @@
}
Fridge::Fridge(control_loops::FridgeQueue *fridge)
- : aos::controls::ControlLoop<control_loops::FridgeQueue, false>(fridge),
+ : aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
arm_loop_(new CappedStateFeedbackLoop(
StateFeedbackLoop<4, 2, 2>(MakeArmLoop()))),
elevator_loop_(new CappedStateFeedbackLoop(
@@ -245,7 +245,7 @@
// Get a reference to the constants struct since we use it so often in this
// code.
- auto &values = constants::GetValues();
+ const auto &values = constants::GetValues();
// Bool to track if we should turn the motors on or not.
bool disable = output == nullptr;
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index eecfe1b..87dbfa4 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -40,7 +40,7 @@
};
class Fridge
- : public aos::controls::ControlLoop<control_loops::FridgeQueue, false> {
+ : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
public:
explicit Fridge(
control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 2af5e2c..657c07e 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -248,7 +248,7 @@
TEST_F(FridgeTest, DoesNothing) {
// Set the goals to the starting values. This should theoretically guarantee
// that the controller does nothing.
- const constants::Values values = constants::GetValues();
+ const auto &values = constants::GetValues();
ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
.angle(0.0)
.height(values.fridge.elevator.lower_limit)
@@ -263,7 +263,7 @@
// Tests that the loop can reach a goal.
TEST_F(FridgeTest, ReachesGoal) {
// Set a reasonable goal.
- const auto values = constants::GetValues();
+ const auto &values = constants::GetValues();
const double soft_limit = values.fridge.elevator.lower_limit;
ASSERT_TRUE(fridge_queue_.goal.MakeWithBuilder()
.angle(M_PI / 4.0)
@@ -291,7 +291,7 @@
// Check that we are near our soft limit.
fridge_queue_.status.FetchLatest();
- const auto values = constants::GetValues();
+ const auto &values = constants::GetValues();
EXPECT_NEAR(values.fridge.elevator.lower_limit, fridge_queue_.status->height,
0.001);
EXPECT_NEAR(values.fridge.arm.upper_limit, fridge_queue_.status->angle,
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 0a9fbce..e306553 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
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index b6a08de..c60feec 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -68,7 +68,7 @@
(2 * M_PI /*radians*/);
}
-double arm_pot_translate(double voltage) {
+double arm_potentiometer_translate(double voltage) {
return voltage /
constants::GetValues().arm_pot_ratio *
(5.0 /*volts*/ / 5.0 /*turns*/) *
@@ -83,7 +83,7 @@
constants::GetValues().elev_distance_per_radian;
}
-double elevator_pot_translate(double voltage) {
+double elevator_potentiometer_translate(double voltage) {
return voltage /
constants::GetValues().elev_pot_ratio *
(2 * M_PI /*radians*/) *
@@ -98,7 +98,7 @@
(2 * M_PI /*radians*/);
}
-double claw_pot_translate(double voltage) {
+double claw_potentiometer_translate(double voltage) {
return voltage /
constants::GetValues().claw_pot_ratio *
(5.0 /*volts*/ / 5.0 /*turns*/) *
@@ -254,25 +254,33 @@
dma_synchronizer_->RunIteration();
+ const auto &values = constants::GetValues();
+
{
auto fridge_message = fridge_queue.position.MakeMessage();
CopyPotAndIndexPosition(arm_left_encoder_, &fridge_message->arm.left,
- arm_translate, arm_pot_translate, false);
- CopyPotAndIndexPosition(arm_right_encoder_, &fridge_message->arm.right,
- arm_translate, arm_pot_translate, true);
+ arm_translate, arm_potentiometer_translate, false,
+ values.fridge.left_elevator_potentiometer_offset);
+ CopyPotAndIndexPosition(
+ arm_right_encoder_, &fridge_message->arm.right, arm_translate,
+ arm_potentiometer_translate, true,
+ values.fridge.right_elevator_potentiometer_offset);
CopyPotAndIndexPosition(
elevator_left_encoder_, &fridge_message->elevator.left,
- elevator_translate, elevator_pot_translate, false);
- CopyPotAndIndexPosition(elevator_right_encoder_,
- &fridge_message->elevator.right,
- elevator_translate, elevator_pot_translate, true);
+ elevator_translate, elevator_potentiometer_translate, false,
+ values.fridge.left_arm_potentiometer_offset);
+ CopyPotAndIndexPosition(
+ elevator_right_encoder_, &fridge_message->elevator.right,
+ elevator_translate, elevator_potentiometer_translate, true,
+ values.fridge.right_arm_potentiometer_offset);
fridge_message.Send();
}
{
auto claw_message = claw_queue.position.MakeMessage();
CopyPotAndIndexPosition(wrist_encoder_, &claw_message->joint,
- claw_translate, claw_pot_translate, false);
+ claw_translate, claw_potentiometer_translate,
+ false, values.claw.potentiometer_offset);
claw_message.Send();
}
}
@@ -289,16 +297,20 @@
void CopyPotAndIndexPosition(
const DMAEncoderAndPotentiometer &encoder, PotAndIndexPosition *position,
::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> pot_translate, bool reverse) {
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double potentiometer_offset) {
const double multiplier = reverse ? -1.0 : 1.0;
position->encoder =
multiplier * encoder_translate(encoder.polled_encoder_value());
- position->pot =
- multiplier * pot_translate(encoder.polled_potentiometer_voltage());
+ position->pot = multiplier * potentiometer_translate(
+ encoder.polled_potentiometer_voltage()) +
+ potentiometer_offset;
position->latched_encoder =
multiplier * encoder_translate(encoder.last_encoder_value());
position->latched_pot =
- multiplier * pot_translate(encoder.last_potentiometer_voltage());
+ multiplier *
+ potentiometer_translate(encoder.last_potentiometer_voltage()) +
+ potentiometer_offset;
position->index_pulses = encoder.index_posedge_count();
}
@@ -306,20 +318,23 @@
const InterruptEncoderAndPotentiometer &encoder,
PotAndIndexPosition *position,
::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> pot_translate, bool reverse) {
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double potentiometer_offset) {
const double multiplier = reverse ? -1.0 : 1.0;
position->encoder =
multiplier * encoder_translate(encoder.encoder()->GetRaw());
- position->pot =
- multiplier * pot_translate(encoder.potentiometer()->GetVoltage());
+ position->pot = multiplier * potentiometer_translate(
+ encoder.potentiometer()->GetVoltage()) +
+ potentiometer_offset;
position->latched_encoder =
multiplier * encoder_translate(encoder.last_encoder_value());
position->latched_pot =
- multiplier * pot_translate(encoder.last_potentiometer_voltage());
+ multiplier *
+ potentiometer_translate(encoder.last_potentiometer_voltage()) +
+ potentiometer_offset;
position->index_pulses = encoder.index_posedge_count();
}
-
::std::unique_ptr<DMASynchronizer> dma_synchronizer_;
DMAEncoderAndPotentiometer arm_left_encoder_, arm_right_encoder_,
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 9228331..d45c49f 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -10,7 +10,6 @@
const constants::Values::ZeroingConstants& constants) {
index_diff_ = constants.index_difference;
max_sample_count_ = constants.average_filter_size;
- index_pulse_count_after_reset_ = 0;
known_index_pos_ = constants.measured_index_position;
start_pos_samples_.reserve(max_sample_count_);
@@ -24,6 +23,27 @@
start_pos_samples_.clear();
zeroed_ = false;
wait_for_index_pulse_ = true;
+ last_used_index_pulse_count_ = 0;
+ error_ = false;
+}
+
+void ZeroingEstimator::TriggerError() {
+ if (!error_) {
+ LOG(ERROR, "Manually triggered zeroing error.\n");
+ error_ = true;
+ }
+}
+
+double ZeroingEstimator::CalculateStartPosition(double start_average,
+ double latched_encoder) const {
+ // We calculate an aproximation of the value of the last index position.
+ // Also account for index pulses not lining up with integer multiples of the
+ // index_diff.
+ double index_pos = start_average + latched_encoder - known_index_pos_;
+ // We round index_pos to the closest valid value of the index.
+ double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
+ // Now we reverse the first calculation to get the accurate start position.
+ return accurate_index_pos - latched_encoder + known_index_pos_;
}
void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
@@ -32,7 +52,7 @@
// reset and wait for that count to change before we consider ourselves
// zeroed.
if (wait_for_index_pulse_) {
- index_pulse_count_after_reset_ = info.index_pulses;
+ last_used_index_pulse_count_ = info.index_pulses;
wait_for_index_pulse_ = false;
}
@@ -57,18 +77,15 @@
// If there are no index pulses to use or we don't have enough samples yet to
// have a well-filtered starting position then we use the filtered value as
// our best guess.
- if (info.index_pulses == index_pulse_count_after_reset_ ||
- offset_ratio_ready() < 1.0) {
+ if (!zeroed_ && (info.index_pulses == last_used_index_pulse_count_ ||
+ offset_ratio_ready() < 1.0)) {
start_pos_ = start_average;
- } else {
- // We calculate an aproximation of the value of the last index position.
- // Also account for index pulses not lining up with integer multiples of
- // the index_diff.
- double index_pos = start_average + info.latched_encoder - known_index_pos_;
- // We round index_pos to the closest valid value of the index.
- double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
- // Now we reverse the first calculation to get the accurate start position.
- start_pos_ = accurate_index_pos - info.latched_encoder + known_index_pos_;
+ } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses) {
+ // Note the accurate start position and the current index pulse count so
+ // that we only run this logic once per index pulse. That should be more
+ // resilient to corrupted intermediate data.
+ start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
+ last_used_index_pulse_count_ = info.index_pulses;
// Now that we have an accurate starting position we can consider ourselves
// zeroed.
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 5566533..9f9329a 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -7,8 +7,6 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/constants.h"
-// TODO(pschrader): Create an error API to flag faults/errors etc..
-//
// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
// away from the last one (i.e. got extra counts from noise, etc..)
//
@@ -33,6 +31,14 @@
// Reset the internal logic so it needs to be re-zeroed.
void Reset();
+ // Manually trigger an internal error. This is used for testing the error
+ // logic.
+ void TriggerError();
+
+ // Returns true if an error has occurred, false otherwise. This gets reset to
+ // false when the Reset() function is called.
+ bool error() const { return error_; }
+
// Returns true if the logic considers the corresponding mechanism to be
// zeroed. It return false otherwise. For example, right after a call to
// Reset() this returns false.
@@ -58,6 +64,11 @@
}
private:
+ // This function calculates the start position given the internal state and
+ // the provided `latched_encoder' value.
+ double CalculateStartPosition(double start_average,
+ double latched_encoder) const;
+
// The estimated position.
double pos_;
// The distance between two consecutive index positions.
@@ -77,14 +88,20 @@
// account for the various ways the encoders get mounted into the robot.
double known_index_pos_;
// Flag for triggering logic that takes note of the current index pulse count
- // after a reset. See `index_pulse_count_after_reset_'.
+ // after a reset. See `last_used_index_pulse_count_'.
bool wait_for_index_pulse_;
// After a reset we keep track of the index pulse count with this. Only after
// the index pulse count changes (i.e. increments at least once or wraps
- // around) will we consider the mechanism zeroed.
- uint32_t index_pulse_count_after_reset_;
+ // around) will we consider the mechanism zeroed. We also use this to store
+ // the most recent `PotAndIndexPosition::index_pulses' value when the start
+ // position was calculated. It helps us calculate the start position only on
+ // index pulses to reject corrupted intermediate data.
+ uint32_t last_used_index_pulse_count_;
// Marker to track whether we're fully zeroed yet or not.
bool zeroed_;
+ // Marker to track whether an error has occurred. This gets reset to false
+ // whenever Reset() is called.
+ bool error_;
};
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 6798123..2ef6865 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -237,5 +237,28 @@
ASSERT_DOUBLE_EQ(4.7 * index_diff, estimator.position());
}
+TEST_F(ZeroingTest, BasicErrorAPITest) {
+ const double index_diff = 1.0;
+ ZeroingEstimator estimator(
+ Values::ZeroingConstants{kSampleSize, index_diff, 0.0});
+ PositionSensorSimulator sim(index_diff);
+ sim.Initialize(1.5 * index_diff, index_diff / 3.0, 0.0);
+
+ // Perform a simple move and make sure that no error occured.
+ MoveTo(&sim, &estimator, 3.5 * index_diff);
+ ASSERT_FALSE(estimator.error());
+
+ // Trigger an error and make sure it's reported.
+ estimator.TriggerError();
+ ASSERT_TRUE(estimator.error());
+
+ // Make sure that it can recover after a reset.
+ estimator.Reset();
+ ASSERT_FALSE(estimator.error());
+ MoveTo(&sim, &estimator, 4.5 * index_diff);
+ MoveTo(&sim, &estimator, 5.5 * index_diff);
+ ASSERT_FALSE(estimator.error());
+}
+
} // namespace zeroing
} // namespace frc971