Upgraded the rest of Time.
Change-Id: I0ee083837e51d8f74a798b7ba14a3b6bb3859f35
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();
}