Upgraded the rest of Time.
Change-Id: I0ee083837e51d8f74a798b7ba14a3b6bb3859f35
diff --git a/y2015/BUILD b/y2015/BUILD
index 84d9e14..748fde6 100644
--- a/y2015/BUILD
+++ b/y2015/BUILD
@@ -12,6 +12,7 @@
deps = [
'//aos/common/logging',
'//aos/common:once',
+ '//aos/common:time',
'//aos/common/network:team_number',
'//frc971/control_loops:state_feedback_loop',
'//frc971:shifter_hall_effect',
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();
diff --git a/y2015/autonomous/auto.cc b/y2015/autonomous/auto.cc
index 198cc97..4df613d 100644
--- a/y2015/autonomous/auto.cc
+++ b/y2015/autonomous/auto.cc
@@ -1,29 +1,32 @@
#include <stdio.h>
+#include <chrono>
#include <memory>
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/time.h"
-#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-
+#include "aos/common/time.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
#include "frc971/autonomous/auto.q.h"
-#include "y2015/autonomous/auto.q.h"
-#include "y2015/constants.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2015/actors/drivetrain_actor.h"
-#include "y2015/control_loops/claw/claw.q.h"
-#include "y2015/control_loops/fridge/fridge.q.h"
+#include "y2015/actors/held_to_lift_actor.h"
#include "y2015/actors/pickup_actor.h"
#include "y2015/actors/stack_actor.h"
-#include "y2015/actors/held_to_lift_actor.h"
+#include "y2015/autonomous/auto.q.h"
+#include "y2015/constants.h"
+#include "y2015/control_loops/claw/claw.q.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
-using ::aos::time::Time;
using ::frc971::control_loops::drivetrain_queue;
using ::y2015::control_loops::claw_queue;
using ::y2015::control_loops::fridge::fridge_queue;
+namespace chrono = ::std::chrono;
+namespace this_thread = ::std::this_thread;
+using ::aos::monotonic_clock;
+
namespace y2015 {
namespace autonomous {
@@ -86,9 +89,11 @@
LOG(ERROR, "No action, not waiting\n");
return;
}
+ ::aos::time::PhasedLoop phased_loop(chrono::milliseconds(5),
+ chrono::microseconds(2500));
while (true) {
// Poll the running bit and auto done bits.
- ::aos::time::PhasedLoopXMS(5, 2500);
+ phased_loop.SleepUntilNext();
if (!action->Running() || ShouldExitAuto()) {
return;
}
@@ -338,14 +343,14 @@
LOG(INFO, "Sucking in tote.\n");
SetClawState(0.0, 6.0, true, kInstantaneousClaw);
- time::SleepFor(time::Time::InSeconds(0.7));
+ this_thread::sleep_for(chrono::milliseconds(700));
LOG(INFO, "Done sucking in tote\n");
// Now pick it up
pickup = actors::MakePickupAction(pickup_params);
pickup->Start();
- time::SleepFor(time::Time::InSeconds(0.9));
+ this_thread::sleep_for(chrono::milliseconds(900));
// Start turning.
LOG(INFO, "Turning in place\n");
drive = SetDriveGoal(0.0, kFastDrive, -0.23, kStackingFirstTurn);
@@ -379,7 +384,7 @@
LOG(INFO, "Lowering the claw to knock the tote\n");
SetClawStateNoWait(0.0, 0.0, true, kFastClaw);
- time::SleepFor(time::Time::InSeconds(0.1));
+ this_thread::sleep_for(chrono::milliseconds(100));
if (ShouldExitAuto()) return;
WaitUntilDoneOrCanceled(::std::move(drive));
@@ -421,12 +426,12 @@
if (ShouldExitAuto()) return;
if (ShouldExitAuto()) return;
- time::SleepFor(time::Time::InSeconds(0.30));
+ this_thread::sleep_for(chrono::milliseconds(300));
if (ShouldExitAuto()) return;
SetClawStateNoWait(0.0, 4.0, true, kFastClaw);
if (ShouldExitAuto()) return;
- time::SleepFor(time::Time::InSeconds(0.10));
+ this_thread::sleep_for(chrono::milliseconds(100));
WaitUntilDoneOrCanceled(::std::move(lift));
if (ShouldExitAuto()) return;
@@ -439,7 +444,7 @@
pickup = actors::MakePickupAction(pickup_params);
pickup->Start();
- time::SleepFor(time::Time::InSeconds(1.0));
+ this_thread::sleep_for(chrono::seconds(1));
if (ShouldExitAuto()) return;
// Start turning.
@@ -478,7 +483,7 @@
// Lift the fridge.
MoveFridge(0.0, 0.3, true, kFridgeXProfile, kFridgeYProfile);
- time::SleepFor(time::Time::InSeconds(0.1));
+ this_thread::sleep_for(chrono::milliseconds(100));
if (ShouldExitAuto()) return;
WaitUntilDoneOrCanceled(::std::move(drive));
@@ -503,7 +508,7 @@
SetClawState(0.0, 7.0, true, kFastClaw);
if (ShouldExitAuto()) return;
- time::SleepFor(time::Time::InSeconds(0.2));
+ this_thread::sleep_for(chrono::milliseconds(200));
if (ShouldExitAuto()) return;
WaitUntilDoneOrCanceled(::std::move(drive));
@@ -514,7 +519,7 @@
//StepDrive(2.5, -1.4);
drive = SetDriveGoal(2.5, kRaceDrive, -1.4, kRaceTurn);
- time::SleepFor(time::Time::InSeconds(0.5));
+ this_thread::sleep_for(chrono::milliseconds(500));
LOG(INFO, "Moving totes out\n");
MoveFridge(0.6, 0.32, true, kFridgeXProfile, kFridgeYProfile);
@@ -528,7 +533,7 @@
WaitForFridge();
if (ShouldExitAuto()) return;
- time::SleepFor(time::Time::InSeconds(0.1));
+ this_thread::sleep_for(chrono::milliseconds(100));
if (ShouldExitAuto()) return;
@@ -550,30 +555,33 @@
if (ShouldExitAuto()) return;
}
-void GrabberForTime(double voltage, double wait_time) {
- ::aos::time::Time now = ::aos::time::Time::Now();
- ::aos::time::Time end_time = now + time::Time::InSeconds(wait_time);
- LOG(INFO, "Starting to grab at %f for %f seconds\n", voltage, wait_time);
+void GrabberForTime(double voltage, monotonic_clock::duration wait_time) {
+ monotonic_clock::time_point now = monotonic_clock::now();
+ monotonic_clock::time_point end_time = now + wait_time;
+ LOG(INFO, "Starting to grab at %f for %f seconds\n", voltage,
+ chrono::duration_cast<chrono::duration<double>>(wait_time).count());
+ ::aos::time::PhasedLoop phased_loop(chrono::milliseconds(5),
+ chrono::microseconds(2500));
while (true) {
autonomous::can_control.MakeWithBuilder().can_voltage(voltage).Send();
// Poll the running bit and auto done bits.
if (ShouldExitAuto()) {
return;
}
- if (::aos::time::Time::Now() > end_time) {
+ if (monotonic_clock::now() > end_time) {
LOG(INFO, "Done grabbing\n");
return;
}
- ::aos::time::PhasedLoopXMS(5, 2500);
+ phased_loop.SleepUntilNext();
}
}
void CanGrabberAuto() {
ResetDrivetrain();
- GrabberForTime(12.0, 0.18);
+ GrabberForTime(12.0, chrono::milliseconds(180));
if (ShouldExitAuto()) return;
- //GrabberForTime(4.0, 0.10);
+ //GrabberForTime(4.0, chrono::milliseconds(100));
if (ShouldExitAuto()) return;
InitializeEncoders();
ResetDrivetrain();
@@ -588,15 +596,17 @@
.right_goal(right_initial_position + 1.5)
.right_velocity_goal(0)
.Send();
- GrabberForTime(12.0, 0.02);
+ GrabberForTime(12.0, chrono::milliseconds(20));
- GrabberForTime(4.0, 14.0);
+ GrabberForTime(4.0, chrono::seconds(14));
if (ShouldExitAuto()) return;
}
void HandleAuto() {
- ::aos::time::Time start_time = ::aos::time::Time::Now();
- LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
+ monotonic_clock::time_point start_time = monotonic_clock::now();
+ LOG(INFO, "Starting auto mode at %f\n",
+ chrono::duration_cast<chrono::duration<double>>(
+ start_time.time_since_epoch()).count());
//TripleCanAuto();
CanGrabberAuto();
}
diff --git a/y2015/autonomous/auto_main.cc b/y2015/autonomous/auto_main.cc
index d9437d1..c5e07d6 100644
--- a/y2015/autonomous/auto_main.cc
+++ b/y2015/autonomous/auto_main.cc
@@ -1,13 +1,12 @@
#include <stdio.h>
+#include <chrono>
+#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
-#include "aos/common/logging/logging.h"
#include "frc971/autonomous/auto.q.h"
#include "y2015/autonomous/auto.h"
-using ::aos::time::Time;
-
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init(-1);
@@ -24,12 +23,15 @@
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Starting auto mode\n");
- ::aos::time::Time start_time = ::aos::time::Time::Now();
+ ::aos::monotonic_clock::time_point start_time =
+ ::aos::monotonic_clock::now();
::y2015::autonomous::HandleAuto();
- ::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
+ auto elapsed_time = ::aos::monotonic_clock::now() - start_time;
LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
- elapsed_time.ToSeconds());
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ elapsed_time)
+ .count());
while (::frc971::autonomous::autonomous->run_auto) {
::frc971::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
diff --git a/y2015/constants.cc b/y2015/constants.cc
index 7ccb020..5b02a25 100644
--- a/y2015/constants.cc
+++ b/y2015/constants.cc
@@ -127,7 +127,8 @@
const double kArmLength = 0.7366;
// TODO(danielp): All these values might need to change.
-const double kClawPistonSwitchTime = 0.4;
+constexpr ::aos::monotonic_clock::duration kClawPistonSwitchTime =
+ ::std::chrono::milliseconds(400);
const double kClawZeroingRange = 0.3;
const Values *DoGetValuesForTeam(uint16_t team) {
diff --git a/y2015/constants.h b/y2015/constants.h
index eed5616..8cefe7e 100644
--- a/y2015/constants.h
+++ b/y2015/constants.h
@@ -3,9 +3,10 @@
#include <stdint.h>
+#include "aos/common/time.h"
+#include "frc971/constants.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/shifter_hall_effect.h"
-#include "frc971/constants.h"
namespace y2015 {
namespace constants {
@@ -76,7 +77,7 @@
// Time between sending commands to claw opening pistons and them reaching
// the new state.
- double piston_switch_time;
+ ::aos::monotonic_clock::duration piston_switch_time;
// How far on either side we look for the index pulse before we give up.
double zeroing_range;
};
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index b4a5a7b..987bb5e 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -1,18 +1,19 @@
#include "y2015/control_loops/claw/claw.h"
#include <algorithm>
+#include <chrono>
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
-
+#include "aos/common/time.h"
+#include "aos/common/util/trapezoid_profile.h"
#include "y2015/constants.h"
#include "y2015/control_loops/claw/claw_motor_plant.h"
-#include "aos/common/util/trapezoid_profile.h"
namespace y2015 {
namespace control_loops {
-using ::aos::time::Time;
+using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
constexpr double kZeroingVoltage = 4.0;
@@ -33,7 +34,7 @@
Claw::Claw(control_loops::ClawQueue *claw)
: aos::controls::ControlLoop<control_loops::ClawQueue>(claw),
- last_piston_edge_(Time::Now()),
+ last_piston_edge_(monotonic_clock::min_time),
claw_loop_(new ClawCappedStateFeedbackLoop(
::y2015::control_loops::claw::MakeClawLoop())),
claw_estimator_(constants::GetValues().claw.zeroing),
@@ -273,7 +274,7 @@
}
}
if (output->rollers_closed != last_rollers_closed_) {
- last_piston_edge_ = Time::Now();
+ last_piston_edge_ = monotonic_clock::now();
}
}
@@ -296,11 +297,12 @@
if (output) {
status->rollers_open = !output->rollers_closed &&
- ((Time::Now() - last_piston_edge_).ToSeconds() >=
- values.claw.piston_switch_time);
- status->rollers_closed = output->rollers_closed &&
- ((Time::Now() - last_piston_edge_).ToSeconds() >=
- values.claw.piston_switch_time);
+ (monotonic_clock::now() >=
+ values.claw.piston_switch_time + last_piston_edge_);
+ status->rollers_closed =
+ output->rollers_closed &&
+ (monotonic_clock::now() >=
+ values.claw.piston_switch_time + last_piston_edge_);
} else {
status->rollers_open = false;
status->rollers_closed = false;
diff --git a/y2015/control_loops/claw/claw.h b/y2015/control_loops/claw/claw.h
index 58c3cc1..324f9f3 100644
--- a/y2015/control_loops/claw/claw.h
+++ b/y2015/control_loops/claw/claw.h
@@ -87,7 +87,7 @@
State state_ = UNINITIALIZED;
// The time when we last changed the claw piston state.
- ::aos::time::Time last_piston_edge_;
+ ::aos::monotonic_clock::time_point last_piston_edge_;
// The state feedback control loop to talk to.
::std::unique_ptr<ClawCappedStateFeedbackLoop> claw_loop_;
diff --git a/y2015/control_loops/claw/claw_lib_test.cc b/y2015/control_loops/claw/claw_lib_test.cc
index b38a2ee..ed82322 100644
--- a/y2015/control_loops/claw/claw_lib_test.cc
+++ b/y2015/control_loops/claw/claw_lib_test.cc
@@ -1,24 +1,26 @@
#include <math.h>
#include <unistd.h>
+#include <chrono>
#include <memory>
-#include "gtest/gtest.h"
+#include "aos/common/controls/control_loop_test.h"
#include "aos/common/queue.h"
#include "aos/common/time.h"
-#include "aos/common/controls/control_loop_test.h"
-#include "y2015/control_loops/claw/claw.q.h"
-#include "y2015/control_loops/claw/claw.h"
#include "frc971/control_loops/position_sensor_sim.h"
-#include "y2015/constants.h"
#include "frc971/control_loops/team_number_test_environment.h"
-
-using ::aos::time::Time;
+#include "gtest/gtest.h"
+#include "y2015/constants.h"
+#include "y2015/control_loops/claw/claw.h"
+#include "y2015/control_loops/claw/claw.q.h"
namespace y2015 {
namespace control_loops {
namespace testing {
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
// Class which simulates the claw and sends out queue messages with the
// position.
class ClawSimulation {
@@ -37,21 +39,20 @@
}
void InitializePosition(double start_pos) {
- InitializePosition(start_pos,
- constants::GetValues().claw.zeroing.measured_index_position);
+ InitializePosition(
+ start_pos, constants::GetValues().claw.zeroing.measured_index_position);
}
void InitializePosition(double start_pos, double index_pos) {
- InitializePosition(start_pos,
+ InitializePosition(
+ start_pos,
// This gives us a standard deviation of ~9 degrees on the pot noise.
- constants::GetValues().claw.zeroing.index_difference / 10.0,
- index_pos);
+ constants::GetValues().claw.zeroing.index_difference / 10.0, index_pos);
}
// Do specific initialization for the sensors.
- void InitializePosition(double start_pos,
- double pot_noise_stddev,
- double index_pos) {
+ void InitializePosition(double start_pos, double pot_noise_stddev,
+ double index_pos) {
// Update internal state.
claw_plant_->mutable_X(0, 0) = start_pos;
// Zero velocity.
@@ -63,7 +64,7 @@
// Sends a queue message with the position.
void SendPositionMessage() {
::aos::ScopedMessagePtr<control_loops::ClawQueue::Position> position =
- claw_queue_.position.MakeMessage();
+ claw_queue_.position.MakeMessage();
pot_and_encoder_.GetSensorValues(&position->joint);
position.Send();
}
@@ -106,9 +107,7 @@
void VerifyNearGoal() {
claw_queue_.goal.FetchLatest();
claw_queue_.status.FetchLatest();
- EXPECT_NEAR(claw_queue_.goal->angle,
- claw_queue_.status->angle,
- 10.0);
+ EXPECT_NEAR(claw_queue_.goal->angle, claw_queue_.status->angle, 10.0);
EXPECT_TRUE(claw_queue_.status->zeroed);
EXPECT_FALSE(claw_queue_.status->estopped);
@@ -126,9 +125,9 @@
}
// Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const Time &run_for, bool enabled = true) {
- const auto start_time = Time::Now();
- while (Time::Now() < start_time + run_for) {
+ void RunForTime(monotonic_clock::duration run_for, bool enabled = true) {
+ const auto start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < start_time + run_for) {
RunIteration(enabled);
}
}
@@ -147,10 +146,10 @@
TEST_F(ClawTest, DoesNothing) {
const auto &values = constants::GetValues();
ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(values.claw.wrist.lower_limit)
- .Send());
+ .angle(values.claw.wrist.lower_limit)
+ .Send());
- RunForTime(Time::InSeconds(6));
+ RunForTime(chrono::seconds(6));
// We should not have moved.
VerifyNearGoal();
@@ -162,11 +161,9 @@
// Tests that the loop zeroing works with normal values.
TEST_F(ClawTest, Zeroes) {
// It should zero itself if we run it for awhile.
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
- RunForTime(Time::InSeconds(6));
+ RunForTime(chrono::seconds(6));
ASSERT_TRUE(claw_queue_.status.FetchLatest());
EXPECT_TRUE(claw_queue_.status->zeroed);
@@ -180,13 +177,11 @@
claw_plant_.InitializePosition(values.claw.wrist.lower_limit,
values.claw.wrist.upper_limit);
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
// The zeroing is going to take its sweet time on this one, so we had better
// run it for longer.
- RunForTime(Time::InMS(12000));
+ RunForTime(chrono::milliseconds(12000));
ASSERT_TRUE(claw_queue_.status.FetchLatest());
EXPECT_FALSE(claw_queue_.status->zeroed);
@@ -195,11 +190,9 @@
// Tests that we can reach a reasonable goal.
TEST_F(ClawTest, ReachesGoal) {
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
- RunForTime(Time::InSeconds(6));
+ RunForTime(chrono::seconds(6));
VerifyNearGoal();
}
@@ -209,26 +202,24 @@
const auto &values = constants::GetValues();
// Upper limit
ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(values.claw.wrist.upper_hard_limit + 5.0)
- .Send());
+ .angle(values.claw.wrist.upper_hard_limit + 5.0)
+ .Send());
- RunForTime(Time::InSeconds(7));
+ RunForTime(chrono::seconds(7));
claw_queue_.status.FetchLatest();
- EXPECT_NEAR(values.claw.wrist.upper_limit,
- claw_queue_.status->angle,
+ EXPECT_NEAR(values.claw.wrist.upper_limit, claw_queue_.status->angle,
2.0 * M_PI / 180.0);
// Lower limit.
ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(values.claw.wrist.lower_hard_limit - 5.0)
- .Send());
+ .angle(values.claw.wrist.lower_hard_limit - 5.0)
+ .Send());
- RunForTime(Time::InMS(6000));
+ RunForTime(chrono::milliseconds(6000));
claw_queue_.status.FetchLatest();
- EXPECT_NEAR(values.claw.wrist.lower_limit,
- claw_queue_.status->angle,
+ EXPECT_NEAR(values.claw.wrist.lower_limit, claw_queue_.status->angle,
2.0 * M_PI / 180.0);
}
@@ -236,10 +227,8 @@
TEST_F(ClawTest, LowerHardstopStartup) {
claw_plant_.InitializePosition(
constants::GetValues().claw.wrist.lower_hard_limit);
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
- RunForTime(Time::InSeconds(6));
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
+ RunForTime(chrono::seconds(6));
VerifyNearGoal();
}
@@ -248,67 +237,56 @@
TEST_F(ClawTest, UpperHardstopStartup) {
claw_plant_.InitializePosition(
constants::GetValues().claw.wrist.upper_hard_limit);
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
// Zeroing will take a long time here.
- RunForTime(Time::InSeconds(15));
+ RunForTime(chrono::seconds(15));
VerifyNearGoal();
}
-
// Tests that not having a goal doesn't break anything.
-TEST_F(ClawTest, NoGoal) {
- RunForTime(Time::InMS(50));
-}
+TEST_F(ClawTest, NoGoal) { RunForTime(chrono::milliseconds(50)); }
// Tests that a WPILib reset results in rezeroing.
TEST_F(ClawTest, WpilibReset) {
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
- RunForTime(Time::InSeconds(6));
+ RunForTime(chrono::seconds(6));
VerifyNearGoal();
SimulateSensorReset();
- RunForTime(Time::InMS(100));
+ RunForTime(chrono::milliseconds(100));
ASSERT_TRUE(claw_queue_.status.FetchLatest());
EXPECT_NE(Claw::RUNNING, claw_queue_.status->state);
// Once again, it's going to take us awhile to rezero since we moved away from
// our index pulse.
- RunForTime(Time::InSeconds(6));
+ RunForTime(chrono::seconds(6));
VerifyNearGoal();
}
// Tests that internal goals don't change while disabled.
TEST_F(ClawTest, DisabledGoal) {
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
- RunForTime(Time::InMS(100), false);
+ RunForTime(chrono::milliseconds(100), false);
EXPECT_EQ(0.0, claw_.claw_goal_);
// Now make sure they move correctly.
- RunForTime(Time::InMS(1000), true);
+ RunForTime(chrono::milliseconds(1000), true);
EXPECT_NE(0.0, claw_.claw_goal_);
}
// Tests that the claw zeroing goals don't wind up too far.
TEST_F(ClawTest, GoalPositiveWindup) {
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
while (claw_.state() != Claw::ZEROING) {
RunIteration();
}
// Kick it.
- RunForTime(Time::InMS(50));
+ RunForTime(chrono::milliseconds(50));
const double orig_claw_goal = claw_.claw_goal_;
claw_.claw_goal_ += 100.0;
@@ -322,16 +300,14 @@
// Tests that the claw zeroing goals don't wind up too far.
TEST_F(ClawTest, GoalNegativeWindup) {
- ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder()
- .angle(M_PI / 4.0)
- .Send());
+ ASSERT_TRUE(claw_queue_.goal.MakeWithBuilder().angle(M_PI / 4.0).Send());
while (claw_.state() != Claw::ZEROING) {
RunIteration();
}
// Kick it.
- RunForTime(Time::InMS(50));
+ RunForTime(chrono::milliseconds(50));
double orig_claw_goal = claw_.claw_goal_;
claw_.claw_goal_ -= 100.0;
diff --git a/y2015/control_loops/fridge/fridge_lib_test.cc b/y2015/control_loops/fridge/fridge_lib_test.cc
index 2378622..6acc983 100644
--- a/y2015/control_loops/fridge/fridge_lib_test.cc
+++ b/y2015/control_loops/fridge/fridge_lib_test.cc
@@ -3,27 +3,30 @@
#include <math.h>
#include <unistd.h>
+#include <chrono>
#include <memory>
-#include "gtest/gtest.h"
-#include "aos/common/queue.h"
-#include "aos/common/time.h"
#include "aos/common/commonmath.h"
#include "aos/common/controls/control_loop_test.h"
-#include "y2015/util/kinematics.h"
+#include "aos/common/queue.h"
+#include "aos/common/time.h"
#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2015/constants.h"
#include "y2015/control_loops/fridge/arm_motor_plant.h"
#include "y2015/control_loops/fridge/elevator_motor_plant.h"
#include "y2015/control_loops/fridge/fridge.q.h"
-#include "y2015/constants.h"
-#include "frc971/control_loops/team_number_test_environment.h"
-
-using ::aos::time::Time;
+#include "y2015/util/kinematics.h"
namespace y2015 {
namespace control_loops {
namespace fridge {
namespace testing {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
// Class which simulates the fridge and sends out queue messages with the
// position.
class FridgeSimulation {
@@ -263,9 +266,10 @@
}
// Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(const Time &run_for, bool enabled = true) {
- const auto start_time = Time::Now();
- while (Time::Now() < start_time + run_for) {
+ void RunForTime(const monotonic_clock::duration run_for,
+ bool enabled = true) {
+ const auto start_time = monotonic_clock::now();
+ while (monotonic_clock::now() < start_time + run_for) {
RunIteration(enabled);
}
}
@@ -297,7 +301,7 @@
.Send());
// Run a few iterations.
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
}
@@ -324,7 +328,7 @@
.Send());
// Give it a lot of time to get there.
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
}
@@ -344,7 +348,7 @@
.Send());
// Give it a lot of time to get there.
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
}
@@ -364,7 +368,7 @@
.max_angular_acceleration(20)
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
fridge_queue_.status.FetchLatest();
@@ -385,7 +389,7 @@
.max_angular_acceleration(20)
.Send());
- RunForTime(Time::InSeconds(10));
+ RunForTime(chrono::seconds(10));
// Check that we are near our soft limit.
fridge_queue_.status.FetchLatest();
@@ -405,7 +409,7 @@
.max_angular_velocity(20)
.max_angular_acceleration(20)
.Send();
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
}
@@ -420,7 +424,7 @@
constants::GetValues().fridge.arm.lower_hard_limit);
fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
// We have to wait for it to put the elevator in a safe position as well.
- RunForTime(Time::InMS(8000));
+ RunForTime(chrono::milliseconds(8000));
VerifyNearGoal();
}
@@ -434,7 +438,7 @@
constants::GetValues().fridge.arm.upper_hard_limit,
constants::GetValues().fridge.arm.upper_hard_limit);
fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
- RunForTime(Time::InMS(5000));
+ RunForTime(chrono::milliseconds(5000));
VerifyNearGoal();
}
@@ -525,7 +529,7 @@
EXPECT_EQ(Fridge::ZEROING_ELEVATOR, fridge_.state());
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
}
@@ -551,7 +555,7 @@
EXPECT_EQ(Fridge::ZEROING_ELEVATOR, fridge_.state());
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
VerifyNearGoal();
}
@@ -564,14 +568,14 @@
constants::GetValues().fridge.arm.upper_hard_limit,
constants::GetValues().fridge.arm.upper_hard_limit);
fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
- RunForTime(Time::InMS(5000));
+ RunForTime(chrono::milliseconds(5000));
EXPECT_EQ(Fridge::RUNNING, fridge_.state());
VerifyNearGoal();
SimulateSensorReset();
- RunForTime(Time::InMS(100));
+ RunForTime(chrono::milliseconds(100));
EXPECT_NE(Fridge::RUNNING, fridge_.state());
- RunForTime(Time::InMS(6000));
+ RunForTime(chrono::milliseconds(6000));
EXPECT_EQ(Fridge::RUNNING, fridge_.state());
VerifyNearGoal();
}
@@ -580,12 +584,12 @@
TEST_F(FridgeTest, DisabledGoalTest) {
fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
- RunForTime(Time::InMS(100), false);
+ RunForTime(chrono::milliseconds(100), false);
EXPECT_EQ(0.0, fridge_.elevator_goal_);
EXPECT_EQ(0.0, fridge_.arm_goal_);
// Now make sure they move correctly
- RunForTime(Time::InMS(4000), true);
+ RunForTime(chrono::milliseconds(4000), true);
EXPECT_NE(0.0, fridge_.elevator_goal_);
EXPECT_NE(0.0, fridge_.arm_goal_);
}
@@ -599,7 +603,7 @@
}
// Kick it.
- RunForTime(Time::InMS(50));
+ RunForTime(chrono::milliseconds(50));
double orig_fridge_goal = fridge_.elevator_goal_;
fridge_.elevator_goal_ += 100.0;
@@ -623,7 +627,7 @@
}
// Kick it.
- RunForTime(Time::InMS(50));
+ RunForTime(chrono::milliseconds(50));
double orig_fridge_goal = fridge_.arm_goal_;
fridge_.arm_goal_ += 100.0;
@@ -644,7 +648,7 @@
}
// Kick it.
- RunForTime(Time::InMS(50));
+ RunForTime(chrono::milliseconds(50));
double orig_fridge_goal = fridge_.elevator_goal_;
fridge_.elevator_goal_ -= 100.0;
@@ -665,7 +669,7 @@
}
// Kick it.
- RunForTime(Time::InMS(50));
+ RunForTime(chrono::milliseconds(50));
double orig_fridge_goal = fridge_.arm_goal_;
fridge_.arm_goal_ -= 100.0;
@@ -679,7 +683,7 @@
// Tests that the loop zeroes when run for a while.
TEST_F(FridgeTest, ZeroNoGoal) {
- RunForTime(Time::InSeconds(5));
+ RunForTime(chrono::seconds(5));
EXPECT_EQ(Fridge::RUNNING, fridge_.state());
}
@@ -692,9 +696,9 @@
values.fridge.elevator.lower_hard_limit);
fridge_plant_.InitializeArmPosition(M_PI / 4.0);
- const auto start_time = Time::Now();
+ const auto start_time = monotonic_clock::now();
double last_arm_goal = fridge_.arm_goal_;
- while (Time::Now() < start_time + Time::InMS(4000)) {
+ while (monotonic_clock::now() < start_time + chrono::milliseconds(4000)) {
RunIteration();
if (fridge_.state() != Fridge::ZEROING_ELEVATOR) {
@@ -721,7 +725,7 @@
fridge_plant_.set_arm_power_error(1.0);
fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
- RunForTime(Time::InMS(8000));
+ RunForTime(chrono::milliseconds(8000));
VerifyNearGoal();
}
diff --git a/y2015/http_status/http_status.cc b/y2015/http_status/http_status.cc
index 7d5093a..1b79555 100644
--- a/y2015/http_status/http_status.cc
+++ b/y2015/http_status/http_status.cc
@@ -1,5 +1,6 @@
#include "y2015/http_status/http_status.h"
+#include <chrono>
#include <iostream>
#include <sstream>
#include <string>
@@ -22,6 +23,9 @@
namespace y2015 {
namespace http_status {
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+
// TODO(comran): Make some of these separate libraries & document them better.
HTTPStatusMessage::HTTPStatusMessage()
@@ -33,13 +37,13 @@
void HTTPStatusMessage::NextSample() {
int32_t adjusted_index = GetIndex(sample_id_);
- ::aos::time::Time time_now = ::aos::time::Time::Now();
+ const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
if (sample_id_ < overflow_id_) {
- sample_times_.emplace_back(time_now);
+ sample_times_.emplace_back(monotonic_now);
data_values_.emplace_back(::std::vector<double>());
} else {
- sample_times_[adjusted_index] = time_now;
+ sample_times_[adjusted_index] = monotonic_now;
}
sample_id_++;
@@ -122,7 +126,10 @@
int32_t adjusted_index = GetIndex(cur_sample);
- message << cur_sample << "%" << sample_times_.at(adjusted_index).ToSeconds()
+ message << cur_sample << "%"
+ << chrono::duration_cast<chrono::duration<double>>(
+ sample_times_.at(adjusted_index).time_since_epoch())
+ .count()
<< "%";
for (int32_t cur_measure = 0;
cur_measure < static_cast<int32_t>(data_names_.size());
@@ -216,8 +223,10 @@
void DataCollector::operator()() {
::aos::SetCurrentThreadName("HTTPStatusData");
+ ::aos::time::PhasedLoop phased_loop(chrono::milliseconds(5),
+ chrono::microseconds(0));
while (run_) {
- ::aos::time::PhasedLoopXMS(5, 0);
+ phased_loop.SleepUntilNext();
RunIteration();
}
}
diff --git a/y2015/http_status/http_status.h b/y2015/http_status/http_status.h
index b7b3b4c..7e6383f 100644
--- a/y2015/http_status/http_status.h
+++ b/y2015/http_status/http_status.h
@@ -55,7 +55,7 @@
// Vectors of vectors to store samples at indexes determined by GetIndex.
::std::vector<::std::string> data_names_;
::std::vector<::std::vector<double>> data_values_;
- ::std::vector<::aos::time::Time> sample_times_;
+ ::std::vector<::aos::monotonic_clock::time_point> sample_times_;
int32_t sample_id_; // Last sample id used.
int32_t measure_index_; // Last measure index used.
diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index a6dab4d..0c643b4 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -68,6 +68,7 @@
using ::frc971::wpilib::PneumaticsToLog;
using ::y2015::control_loops::claw_queue;
using ::y2015::control_loops::fridge::fridge_queue;
+namespace chrono = ::std::chrono;
namespace y2015 {
namespace wpilib {
@@ -251,8 +252,8 @@
wrist_encoder_.Start();
dma_synchronizer_->Start();
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
- ::std::chrono::milliseconds(4));
+ ::aos::time::PhasedLoop phased_loop(chrono::milliseconds(5),
+ chrono::milliseconds(4));
::aos::SetCurrentThreadRealtimePriority(40);
while (run_) {
@@ -428,8 +429,8 @@
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(27);
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
- ::std::chrono::milliseconds(1));
+ ::aos::time::PhasedLoop phased_loop(chrono::milliseconds(20),
+ chrono::milliseconds(1));
while (run_) {
{
@@ -505,7 +506,7 @@
class CanWriter : public LoopOutputHandler {
public:
- CanWriter() : LoopOutputHandler(::aos::time::Time::InSeconds(0.10)) {}
+ CanWriter() : LoopOutputHandler(chrono::milliseconds(100)) {}
void set_can_talon(::std::unique_ptr<Talon> t) {
can_talon_ = ::std::move(t);