StackCancel:
- Added pass through of should cancel from the stack to the profile.
- Split WaitUntilDone into an iterable function.
- Added a test for stack action as this slipped through code reveiw.
Change-Id: I0d4c215b87f2919ff5164e86194ee6992e364190
diff --git a/aos/common/actions/actions.h b/aos/common/actions/actions.h
index 15054d2..776c945 100644
--- a/aos/common/actions/actions.h
+++ b/aos/common/actions/actions.h
@@ -75,6 +75,10 @@
// Waits until the action has finished.
void WaitUntilDone() { DoWaitUntilDone(); }
+ // Run all the checks for one iteration of waiting. Will return true when the
+ // action has completed, successfully or not. This is non-blocking.
+ bool CheckIteration() { return DoCheckIteration(false); }
+
// Retrieves the internal state of the action for testing.
// See comments on the private members of TypedAction<T, S> for details.
void GetState(bool* has_started, bool* sent_started, bool* sent_cancel,
@@ -94,6 +98,8 @@
virtual void DoStart() = 0;
// Blocks until complete.
virtual void DoWaitUntilDone() = 0;
+ // Updates status for one cycle of waiting
+ virtual bool DoCheckIteration(bool blocking) = 0;
// For testing we will need to get the internal state.
// See comments on the private members of TypedAction<T, S> for details.
virtual void DoGetState(bool* has_started, bool* sent_started,
@@ -141,6 +147,8 @@
void DoWaitUntilDone() override;
+ bool DoCheckIteration(bool blocking);
+
// Sets the started flag (also possibly the interrupted flag).
void CheckStarted();
@@ -235,19 +243,34 @@
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_)) {
+ if (DoCheckIteration(true)) {
return;
}
}
}
template <typename T>
+bool TypedAction<T>::DoCheckIteration(bool blocking) {
+ CHECK(sent_started_);
+ if (interrupted_) return true;
+ CheckStarted();
+ if (blocking) {
+ queue_group_->status.FetchAnother();
+ } else {
+ if (!queue_group_->status.FetchNext()) {
+ return false;
+ }
+ }
+ CheckStarted();
+ CheckInterrupted();
+ if (has_started_ && (queue_group_->status.get() &&
+ queue_group_->status->running != run_value_)) {
+ return true;
+ }
+ return false;
+}
+
+template <typename T>
void TypedAction<T>::CheckStarted() {
if (has_started_) return;
if (queue_group_->status.get()) {
diff --git a/frc971/actors/actors.gyp b/frc971/actors/actors.gyp
index 1b89982..6228381 100644
--- a/frc971/actors/actors.gyp
+++ b/frc971/actors/actors.gyp
@@ -183,6 +183,26 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'stack_action_test',
+ 'type': 'executable',
+ 'sources': [
+ 'stack_actor_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/common.gyp:queues',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/actions/actions.gyp:action_lib',
+ '<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:team_number_test_environment',
+ 'stack_action_queue',
+ 'stack_action_lib',
+ ],
+ },
+ {
'target_name': 'stack_action_lib',
'type': 'static_library',
'sources': [
@@ -192,6 +212,7 @@
'fridge_profile_action_lib',
'stack_action_queue',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
'<(AOS)/common/actions/actions.gyp:action_lib',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
diff --git a/frc971/actors/fridge_profile_actor_test.cc b/frc971/actors/fridge_profile_actor_test.cc
index d2d7dd2..b054e8d 100644
--- a/frc971/actors/fridge_profile_actor_test.cc
+++ b/frc971/actors/fridge_profile_actor_test.cc
@@ -356,6 +356,47 @@
}
}
+// Make sure that should cancel gets set by pushing to queue.
+TEST_F(FridgeProfileTest, ProfileShouldCancel) {
+ FridgeProfileActor fridge_profile(&frc971::actors::fridge_profile_action);
+ double next_angle = 0, next_height = 0, next_angle_vel = 0.0,
+ next_height_vel = 0.0;
+ FridgeProfileParams params;
+ params.arm_angle = 5.0;
+ params.arm_max_velocity = 200.0;
+ params.arm_max_acceleration = 20000.0;
+ frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
+ .run(true)
+ .params(params)
+ .Send();
+
+ // tell it the fridge is zeroed
+ control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send();
+
+ fridge_profile.SetTesting();
+
+ fridge_profile.WaitForActionRequest();
+
+ // Angle (0.250000, 0.250000, 0.25) Height (0.250000, 0.250000, 0.25)
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
+ EXPECT_FALSE(fridge_profile.ShouldCancel());
+ frc971::actors::fridge_profile_action.goal.MakeWithBuilder()
+ .run(false)
+ .params(params)
+ .Send();
+
+ EXPECT_TRUE(fridge_profile.ShouldCancel());
+
+ // Angle (0.250000, 0.250000, 0.25) Height (0.250000, 0.250000, 0.25)
+ EXPECT_TRUE(fridge_profile.IterateProfile(5.0, 5.0, &next_angle, &next_height,
+ &next_angle_vel, &next_height_vel));
+}
+
} // namespace testing.
} // namespace actors.
} // namespace frc971.
diff --git a/frc971/actors/intake_actor.cc b/frc971/actors/intake_actor.cc
index 70abe19..07b5170 100644
--- a/frc971/actors/intake_actor.cc
+++ b/frc971/actors/intake_actor.cc
@@ -120,7 +120,9 @@
MoveClaw(kHpIntakeAngle, 0.0);
// Stack the new tote.
- ::std::unique_ptr<StackAction> stack_action = MakeStackAction();
+ StackParams params;
+ params.claw_out_angle = M_PI / 4;
+ ::std::unique_ptr<StackAction> stack_action = MakeStackAction(params);
stack_action->Start();
stack_action->WaitUntilDone();
}
diff --git a/frc971/actors/stack_actor.cc b/frc971/actors/stack_actor.cc
index bf5a3e3..4b31451 100644
--- a/frc971/actors/stack_actor.cc
+++ b/frc971/actors/stack_actor.cc
@@ -1,6 +1,8 @@
#include <math.h>
#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+
#include "frc971/actors/stack_actor.h"
#include "frc971/actors/fridge_profile_actor.h"
#include "frc971/constants.h"
@@ -20,9 +22,7 @@
StackActor::StackActor(StackActionQueueGroup *queues)
: aos::common::actions::ActorBase<StackActionQueueGroup>(queues) {}
-namespace {
-
-void DoProfile(double height, double angle, bool grabbers) {
+void StackActor::DoProfile(double height, double angle, bool grabbers) {
FridgeProfileParams params;
params.elevator_height = height;
@@ -40,19 +40,26 @@
::std::unique_ptr<FridgeAction> profile = MakeFridgeProfileAction(params);
profile->Start();
- profile->WaitUntilDone();
+ while (!profile->CheckIteration()) {
+ // wait until next Xms tick
+ ::aos::time::PhasedLoopXMS(5, 2500);
+ if (ShouldCancel()) {
+ profile->Cancel();
+ return;
+ }
+ }
}
-} // namespace
-
bool StackActor::RunAction(const StackParams ¶ms) {
const auto &values = constants::GetValues();
const double bottom = 0.020;
// Set the current stack down on top of the bottom box.
DoProfile(0.45, 0.0, true);
+ if (ShouldCancel()) return true;
// Move down to enclose bottom box.
DoProfile(bottom + values.tote_height, 0.0, true);
+ if (ShouldCancel()) return true;
// Clamp.
{
auto message = control_loops::claw_queue.goal.MakeMessage();
diff --git a/frc971/actors/stack_actor.h b/frc971/actors/stack_actor.h
index 8e834e7..e45bac1 100644
--- a/frc971/actors/stack_actor.h
+++ b/frc971/actors/stack_actor.h
@@ -17,6 +17,8 @@
public:
explicit StackActor(StackActionQueueGroup *queues);
+ void DoProfile(double height, double angle, bool grabbers);
+
bool RunAction(const StackParams ¶ms) override;
};
diff --git a/frc971/actors/stack_actor_test.cc b/frc971/actors/stack_actor_test.cc
new file mode 100644
index 0000000..e8bc786
--- /dev/null
+++ b/frc971/actors/stack_actor_test.cc
@@ -0,0 +1,71 @@
+#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 "frc971/actors/stack_action.q.h"
+#include "frc971/actors/stack_actor.h"
+#include "frc971/control_loops/fridge/fridge.q.h"
+
+#include "aos/common/controls/control_loop_test.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+
+using ::aos::time::Time;
+
+namespace frc971 {
+namespace actors {
+namespace testing {
+
+class StackActionTest : public ::testing::Test {
+ protected:
+ StackActionTest() {
+ frc971::actors::stack_action.goal.Clear();
+ frc971::actors::stack_action.status.Clear();
+ control_loops::fridge_queue.status.Clear();
+ control_loops::fridge_queue.goal.Clear();
+ }
+
+ virtual ~StackActionTest() {
+ frc971::actors::stack_action.goal.Clear();
+ frc971::actors::stack_action.status.Clear();
+ control_loops::fridge_queue.status.Clear();
+ control_loops::fridge_queue.goal.Clear();
+ }
+
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+};
+
+// Tests that cancel stops not only the stack action, but the underlying profile
+// action.
+TEST_F(StackActionTest, StackCancel) {
+ StackActor stack(&frc971::actors::stack_action);
+
+ frc971::actors::stack_action.goal.MakeWithBuilder().run(true).Send();
+
+ // tell it the fridge is zeroed
+ control_loops::fridge_queue.status.MakeWithBuilder()
+ .zeroed(true)
+ .angle(0.0)
+ .height(0.0)
+ .Send();
+
+ // do the action and it will post to the goal queue
+ stack.WaitForActionRequest();
+
+ // the action has started, so now cancel it and it should cancel
+ // the underlying profile
+ frc971::actors::stack_action.goal.MakeWithBuilder().run(false).Send();
+
+ // let the action start running, if we return from this call it has worked.
+ stack.RunAction(0);
+
+ SUCCEED();
+}
+
+} // namespace testing
+} // namespace actors
+} // namespace frc971
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 51e3f27..5c14fcd 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -20,13 +20,14 @@
'../control_loops/voltage_cap/voltage_cap.gyp:voltage_cap_test',
'../../aos/common/actions/actions.gyp:action_test',
'../actors/actors.gyp:drivetrain_action',
+ '../actors/actors.gyp:claw_action',
'../actors/actors.gyp:fridge_profile_action',
'../actors/actors.gyp:score_action',
'../actors/actors.gyp:stack_action',
- '../actors/actors.gyp:fridge_profile_action_test',
- '../actors/actors.gyp:claw_action',
- '../actors/actors.gyp:claw_action_test',
'../actors/actors.gyp:intake_action',
+ '../actors/actors.gyp:claw_action_test',
+ '../actors/actors.gyp:fridge_profile_action_test',
+ '../actors/actors.gyp:stack_action_test',
],
'copies': [
{