Upgraded the rest of Time.
Change-Id: I0ee083837e51d8f74a798b7ba14a3b6bb3859f35
diff --git a/y2015/actors/BUILD b/y2015/actors/BUILD
index 9452202..e28cbfc 100644
--- a/y2015/actors/BUILD
+++ b/y2015/actors/BUILD
@@ -123,26 +123,6 @@
],
)
-cc_test(
- name = 'score_action_test',
- srcs = [
- 'score_actor_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- '//aos/testing:test_shm',
- '//aos/common/logging:queue_logging',
- '//aos/common:queues',
- '//aos/common:time',
- '//aos/linux_code:init',
- '//aos/common/actions:action_lib',
- '//y2015/control_loops/fridge:fridge_queue',
- '//frc971/control_loops:team_number_test_environment',
- ':score_action_queue',
- ':score_action_lib',
- ],
-)
-
queue_library(
name = 'pickup_action_queue',
srcs = [
@@ -424,27 +404,6 @@
],
)
-cc_test(
- name = 'stack_action_test',
- srcs = [
- 'stack_actor_test.cc',
- ],
- deps = [
- '//aos/testing:googletest',
- '//aos/testing:test_shm',
- '//aos/common/logging:queue_logging',
- '//aos/common:queues',
- '//aos/common:time',
- '//aos/linux_code:init',
- '//aos/common/actions:action_lib',
- '//aos/common/controls:control_loop_test',
- '//y2015/control_loops/fridge:fridge_queue',
- '//frc971/control_loops:team_number_test_environment',
- ':stack_action_queue',
- ':stack_action_lib',
- ],
-)
-
cc_library(
name = 'stack_action_lib',
srcs = [
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 945d923..8772af7 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -47,8 +47,10 @@
turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
constants::GetValues().turn_width / 2.0);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
while (true) {
- ::aos::time::PhasedLoopXMS(5, 2500);
+ phased_loop.SleepUntilNext();
drivetrain_queue.status.FetchLatest();
if (drivetrain_queue.status.get()) {
diff --git a/y2015/actors/drivetrain_actor_main.cc b/y2015/actors/drivetrain_actor_main.cc
index 76519f2..a51cb48 100644
--- a/y2015/actors/drivetrain_actor_main.cc
+++ b/y2015/actors/drivetrain_actor_main.cc
@@ -4,8 +4,6 @@
#include "y2015/actors/drivetrain_action.q.h"
#include "y2015/actors/drivetrain_actor.h"
-using ::aos::time::Time;
-
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init(-1);
diff --git a/y2015/actors/held_to_lift_actor.cc b/y2015/actors/held_to_lift_actor.cc
index 942488d..913d172 100644
--- a/y2015/actors/held_to_lift_actor.cc
+++ b/y2015/actors/held_to_lift_actor.cc
@@ -90,13 +90,13 @@
}
{
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ ::std::chrono::milliseconds(5) / 2);
::std::unique_ptr<LiftAction> lift_action =
MakeLiftAction(params.lift_params);
lift_action->Start();
while (lift_action->Running()) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
+ phased_loop.SleepUntilNext();
if (ShouldCancel()) {
lift_action->Cancel();
diff --git a/y2015/actors/pickup_actor.cc b/y2015/actors/pickup_actor.cc
index 3aa0c18..0aa1c58 100644
--- a/y2015/actors/pickup_actor.cc
+++ b/y2015/actors/pickup_actor.cc
@@ -1,11 +1,12 @@
#include "y2015/actors/pickup_actor.h"
+#include <chrono>
#include <cmath>
-#include "aos/common/logging/logging.h"
#include "aos/common/controls/control_loop.h"
-#include "aos/common/util/phased_loop.h"
+#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
#include "y2015/control_loops/claw/claw.q.h"
namespace y2015 {
@@ -19,6 +20,8 @@
constexpr double kClawMoveUpAcceleration = 25.0;
} // namespace
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
using ::y2015::control_loops::claw_queue;
PickupActor::PickupActor(PickupActionQueueGroup* queues)
@@ -91,13 +94,14 @@
}
// Pull in for params.intake_time.
- ::aos::time::Time end_time =
- ::aos::time::Time::Now() + aos::time::Time::InSeconds(params.intake_time);
- while ( ::aos::time::Time::Now() <= end_time) {
- ::aos::time::PhasedLoopXMS(
- ::std::chrono::duration_cast<::std::chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
+ monotonic_clock::time_point end_time =
+ monotonic_clock::now() +
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(params.intake_time));
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ ::std::chrono::milliseconds(5) / 2);
+ while (monotonic_clock::now() <= end_time) {
+ phased_loop.SleepUntilNext();
if (ShouldCancel()) return true;
}
diff --git a/y2015/actors/score_actor.cc b/y2015/actors/score_actor.cc
index 92ddfac..d485af5 100644
--- a/y2015/actors/score_actor.cc
+++ b/y2015/actors/score_actor.cc
@@ -68,10 +68,10 @@
return false;
}
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ ::std::chrono::milliseconds(5) / 2);
while (true) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
+ phased_loop.SleepUntilNext();
if (ShouldCancel()) {
return true;
}
@@ -94,29 +94,31 @@
bool started_lowering = false;
- while (true) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
- if (ShouldCancel()) {
- return true;
- }
- // Round the moving out corner and start setting down.
- if (params.place_the_stack && !started_lowering) {
- if (CurrentGoalX() < params.horizontal_start_lowering) {
- if (!SendGoal(params.horizontal_move_target, params.place_height, true,
- kSlowMaxXVelocity, kFastMaxYVelocity,
- kSlowMaxXAcceleration, kMaxYAcceleration)) {
- LOG(ERROR, "Sending fridge message failed.\n");
- return false;
- }
- started_lowering = true;
+ {
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ phased_loop.SleepUntilNext();
+ if (ShouldCancel()) {
+ return true;
}
- }
+ // Round the moving out corner and start setting down.
+ if (params.place_the_stack && !started_lowering) {
+ if (CurrentGoalX() < params.horizontal_start_lowering) {
+ if (!SendGoal(params.horizontal_move_target, params.place_height,
+ true, kSlowMaxXVelocity, kFastMaxYVelocity,
+ kSlowMaxXAcceleration, kMaxYAcceleration)) {
+ LOG(ERROR, "Sending fridge message failed.\n");
+ return false;
+ }
+ started_lowering = true;
+ }
+ }
- if (NearHorizontalGoal(params.horizontal_move_target)) {
- LOG(INFO, "reached goal\n");
- break;
+ if (NearHorizontalGoal(params.horizontal_move_target)) {
+ LOG(INFO, "reached goal\n");
+ break;
+ }
}
}
@@ -134,16 +136,18 @@
return false;
}
- while (true) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
- if (ShouldCancel()) {
- return true;
- }
+ {
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ chrono::microseconds(2500));
+ while (true) {
+ phased_loop.SleepUntilNext();
+ if (ShouldCancel()) {
+ return true;
+ }
- if (NearGoal(params.horizontal_move_target, params.place_height)) {
- break;
+ if (NearGoal(params.horizontal_move_target, params.place_height)) {
+ break;
+ }
}
}
@@ -165,10 +169,10 @@
}
bool has_lifted = false;
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ chrono::microseconds(2500));
while (true) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
+ phased_loop.SleepUntilNext();
if (ShouldCancel()) {
return true;
}
diff --git a/y2015/actors/score_actor_test.cc b/y2015/actors/score_actor_test.cc
deleted file mode 100644
index 25d1bc2..0000000
--- a/y2015/actors/score_actor_test.cc
+++ /dev/null
@@ -1,100 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/testing/test_shm.h"
-#include "aos/common/actions/actor.h"
-#include "y2015/actors/score_action.q.h"
-#include "y2015/actors/score_actor.h"
-#include "y2015/control_loops/fridge/fridge.q.h"
-#include "frc971/control_loops/team_number_test_environment.h"
-
-using ::aos::time::Time;
-
-namespace y2015 {
-namespace actors {
-namespace testing {
-
-using ::y2015::control_loops::fridge::fridge_queue;
-
-class ScoreActionTest : public ::testing::Test {
- protected:
- ScoreActionTest() {
- y2015::actors::score_action.goal.Clear();
- y2015::actors::score_action.status.Clear();
- fridge_queue.status.Clear();
- fridge_queue.goal.Clear();
- }
-
- virtual ~ScoreActionTest() {
- y2015::actors::score_action.goal.Clear();
- y2015::actors::score_action.status.Clear();
- fridge_queue.status.Clear();
- fridge_queue.goal.Clear();
- }
-
- // Bring up and down Core.
- ::aos::testing::TestSharedMemory my_shm_;
-};
-
-// Tests that cancel stops not only the score action, but also the underlying
-// profile action.
-TEST_F(ScoreActionTest, PlaceTheStackCancel) {
- ScoreActor score(&y2015::actors::score_action);
-
- y2015::actors::score_action.goal.MakeWithBuilder().run(true).Send();
-
- // Tell it the fridge is zeroed.
- ASSERT_TRUE(fridge_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .height(0.0)
- .Send());
-
- // do the action and it will post to the goal queue
- score.WaitForActionRequest();
-
- // the action has started, so now cancel it and it should cancel
- // the underlying profile
- y2015::actors::score_action.goal.MakeWithBuilder().run(false).Send();
-
- // let the action start running, if we return from this call it has worked.
- const ScoreParams params = {true, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
- score.RunAction(params);
-
- SUCCEED();
-}
-
-// Tests that cancel stops not only the score action, but also the underlying
-// profile action.
-TEST_F(ScoreActionTest, MoveStackIntoPositionCancel) {
- ScoreActor score(&y2015::actors::score_action);
-
- y2015::actors::score_action.goal.MakeWithBuilder().run(true).Send();
-
- // Tell it the fridge is zeroed.
- ASSERT_TRUE(fridge_queue.status.MakeWithBuilder()
- .zeroed(true)
- .angle(0.0)
- .height(0.0)
- .Send());
-
- // do the action and it will post to the goal queue
- score.WaitForActionRequest();
-
- // the action has started, so now cancel it and it should cancel
- // the underlying profile
- y2015::actors::score_action.goal.MakeWithBuilder().run(false).Send();
-
- // let the action start running, if we return from this call it has worked.
- const ScoreParams params = {false, true, 0.14, 0.13, -0.7, -0.7, -0.10, -0.5, 0.1};
- score.RunAction(params);
-
- SUCCEED();
-}
-
-} // namespace testing
-} // namespace actors
-} // namespace y2015
diff --git a/y2015/actors/stack_actor_test.cc b/y2015/actors/stack_actor_test.cc
deleted file mode 100644
index c62b6ce..0000000
--- a/y2015/actors/stack_actor_test.cc
+++ /dev/null
@@ -1,74 +0,0 @@
-#include <unistd.h>
-
-#include <memory>
-
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/testing/test_shm.h"
-#include "aos/common/actions/actor.h"
-#include "y2015/actors/stack_action.q.h"
-#include "y2015/actors/stack_actor.h"
-#include "y2015/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 y2015 {
-namespace actors {
-namespace testing {
-
-using ::y2015::control_loops::fridge::fridge_queue;
-
-class StackActionTest : public ::testing::Test {
- protected:
- StackActionTest() {
- y2015::actors::stack_action.goal.Clear();
- y2015::actors::stack_action.status.Clear();
- fridge_queue.status.Clear();
- fridge_queue.goal.Clear();
- }
-
- virtual ~StackActionTest() {
- y2015::actors::stack_action.goal.Clear();
- y2015::actors::stack_action.status.Clear();
- fridge_queue.status.Clear();
- fridge_queue.goal.Clear();
- }
-
- // Bring up and down Core.
- ::aos::testing::TestSharedMemory my_shm_;
-};
-
-// Tests that cancel stops not only the stack action, but the underlying profile
-// action.
-TEST_F(StackActionTest, StackCancel) {
- StackActor stack(&y2015::actors::stack_action);
-
- y2015::actors::stack_action.goal.MakeWithBuilder().run(true).Send();
-
- // tell it the fridge is zeroed
- 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
- y2015::actors::stack_action.goal.MakeWithBuilder().run(false).Send();
-
- // let the action start running, if we return from this call it has worked.
- StackParams s;
- stack.RunAction(s);
-
- SUCCEED();
-}
-
-} // namespace testing
-} // namespace actors
-} // namespace y2015
diff --git a/y2015/actors/stack_and_hold_actor.cc b/y2015/actors/stack_and_hold_actor.cc
index e12b54c..1b94703 100644
--- a/y2015/actors/stack_and_hold_actor.cc
+++ b/y2015/actors/stack_and_hold_actor.cc
@@ -79,10 +79,10 @@
::std::unique_ptr<StackAction> stack_action =
MakeStackAction(stack_params);
stack_action->Start();
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ ::std::chrono::milliseconds(5) / 2);
while (stack_action->Running()) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
+ phased_loop.SleepUntilNext();
if (ShouldCancel()) {
stack_action->Cancel();
diff --git a/y2015/actors/stack_and_lift_actor.cc b/y2015/actors/stack_and_lift_actor.cc
index 24eb11e..8bf3633 100644
--- a/y2015/actors/stack_and_lift_actor.cc
+++ b/y2015/actors/stack_and_lift_actor.cc
@@ -39,10 +39,10 @@
::std::unique_ptr<StackAction> stack_action =
MakeStackAction(stack_params);
stack_action->Start();
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ ::std::chrono::milliseconds(5) / 2);
while (stack_action->Running()) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
+ phased_loop.SleepUntilNext();
if (ShouldCancel()) {
stack_action->Cancel();
@@ -80,10 +80,10 @@
lift_params.pack_claw_angle = claw_goal_start;
::std::unique_ptr<LiftAction> lift_action = MakeLiftAction(lift_params);
lift_action->Start();
+ ::aos::time::PhasedLoop phased_loop(::aos::controls::kLoopFrequency,
+ ::std::chrono::milliseconds(5) / 2);
while (lift_action->Running()) {
- ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
- ::aos::controls::kLoopFrequency).count(),
- 2500);
+ phased_loop.SleepUntilNext();
if (ShouldCancel()) {
lift_action->Cancel();