Upgraded the rest of Time.
Change-Id: I0ee083837e51d8f74a798b7ba14a3b6bb3859f35
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d3ea2f0..21a968a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -1,5 +1,6 @@
#include <unistd.h>
+#include <chrono>
#include <memory>
#include "aos/common/controls/control_loop_test.h"
@@ -24,8 +25,8 @@
namespace drivetrain {
namespace testing {
-using ::aos::time::Time;
-
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
using ::y2016::control_loops::drivetrain::MakeDrivetrainPlant;
// TODO(Comran): Make one that doesn't depend on the actual values for a
@@ -235,9 +236,9 @@
SimulateTimestep(true);
}
- void RunForTime(const Time run_for) {
- const auto end_time = Time::Now() + run_for;
- while (Time::Now() < end_time) {
+ void RunForTime(monotonic_clock::duration run_for) {
+ const auto end_time = monotonic_clock::now() + run_for;
+ while (monotonic_clock::now() < end_time) {
RunIteration();
}
}
@@ -261,7 +262,7 @@
.left_goal(-1.0)
.right_goal(1.0)
.Send();
- RunForTime(Time::InSeconds(2.0));
+ RunForTime(chrono::seconds(2));
VerifyNearGoal();
}
@@ -275,7 +276,7 @@
.Send();
drivetrain_motor_plant_.set_left_voltage_offset(1.0);
drivetrain_motor_plant_.set_right_voltage_offset(1.0);
- RunForTime(Time::InSeconds(1.5));
+ RunForTime(chrono::milliseconds(1500));
VerifyNearGoal();
}
@@ -311,7 +312,7 @@
// Tests that never having a goal, but having driver's station messages, doesn't
// break.
TEST_F(DrivetrainTest, NoGoalWithRobotState) {
- RunForTime(Time::InSeconds(0.1));
+ RunForTime(chrono::milliseconds(100));
}
// Tests that the robot successfully drives straight forward.
@@ -393,7 +394,8 @@
goal.Send();
}
- while (Time::Now() < Time::InSeconds(6)) {
+ while (monotonic_clock::now() <
+ monotonic_clock::time_point(chrono::seconds(6))) {
RunIteration();
ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
@@ -423,7 +425,8 @@
goal.Send();
}
- while (Time::Now() < Time::InSeconds(6)) {
+ while (monotonic_clock::now() <
+ monotonic_clock::time_point(chrono::seconds(6))) {
RunIteration();
ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
EXPECT_NEAR(my_drivetrain_queue_.output->left_voltage,
@@ -453,7 +456,8 @@
goal.Send();
}
- while (Time::Now() < Time::InSeconds(3)) {
+ while (monotonic_clock::now() <
+ monotonic_clock::time_point(chrono::seconds(3))) {
RunIteration();
ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
}
@@ -471,7 +475,7 @@
.quickturn(false)
.Send();
- RunForTime(Time::InSeconds(1.0));
+ RunForTime(chrono::seconds(1));
my_drivetrain_queue_.goal.MakeWithBuilder()
.control_loop_driving(false)
@@ -481,7 +485,7 @@
.quickturn(false)
.Send();
- RunForTime(Time::InSeconds(1.0));
+ RunForTime(chrono::seconds(1));
my_drivetrain_queue_.goal.MakeWithBuilder()
.control_loop_driving(false)
@@ -491,7 +495,7 @@
.quickturn(false)
.Send();
- RunForTime(Time::InSeconds(10.0));
+ RunForTime(chrono::seconds(10));
{
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
@@ -508,8 +512,8 @@
goal.Send();
}
- const auto end_time = Time::Now() + Time::InSeconds(4);
- while (Time::Now() < end_time) {
+ const auto end_time = monotonic_clock::now() + chrono::seconds(4);
+ while (monotonic_clock::now() < end_time) {
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
diff --git a/frc971/control_loops/voltage_cap/voltage_cap_test.cc b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
index 4bee640..23e2f34 100644
--- a/frc971/control_loops/voltage_cap/voltage_cap_test.cc
+++ b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
@@ -7,8 +7,6 @@
#include "aos/common/queue.h"
#include "aos/testing/test_shm.h"
-using ::aos::time::Time;
-
namespace frc971 {
namespace control_loops {
namespace testing {
diff --git a/frc971/control_loops/zeroed_joint.h b/frc971/control_loops/zeroed_joint.h
index f25434f..fcfd29f 100644
--- a/frc971/control_loops/zeroed_joint.h
+++ b/frc971/control_loops/zeroed_joint.h
@@ -135,12 +135,11 @@
ZeroedJoint(StateFeedbackLoop<3, 1, 1> loop)
: loop_(new ZeroedStateFeedbackLoop<kNumZeroSensors>(loop, this)),
- last_good_time_(0, 0),
+ last_good_time_(::aos::monotonic_clock::min_time()),
state_(UNINITIALIZED),
error_count_(0),
zero_offset_(0.0),
- capped_goal_(false) {
- }
+ capped_goal_(false) {}
// Copies the provided configuration data locally.
void set_config_data(const ConfigurationData &config_data) {
@@ -191,14 +190,14 @@
friend class testing::WristTest_NoWindupPositive_Test;
friend class testing::WristTest_NoWindupNegative_Test;
- static const ::aos::time::Time kRezeroTime;
+ static constexpr ::aos::monotonic_clock::duration kRezeroTime;
// The state feedback control loop to talk to.
::std::unique_ptr<ZeroedStateFeedbackLoop<kNumZeroSensors>> loop_;
ConfigurationData config_data_;
- ::aos::time::Time last_good_time_;
+ ::aos::monotonic_clock::time_point last_good_time_;
// Returns the index of the first active sensor, or -1 if none are active.
int ActiveSensorIndex(
@@ -262,8 +261,8 @@
};
template <int kNumZeroSensors>
-const ::aos::time::Time ZeroedJoint<kNumZeroSensors>::kRezeroTime =
- ::aos::time::Time::InSeconds(2);
+constexpr ::aos::monotonic_clock::duration
+ ZeroedJoint<kNumZeroSensors>::kRezeroTime = ::std::chrono::seconds(2);
template <int kNumZeroSensors>
/*static*/ const double ZeroedJoint<kNumZeroSensors>::dt = 0.01;
@@ -283,7 +282,7 @@
output_enabled = false;
LOG(WARNING, "err_count is %d so disabling\n", error_count_);
- if ((::aos::time::Time::Now() - last_good_time_) > kRezeroTime) {
+ if (::aos::monotonic_clock::now() > kRezeroTime + last_good_time_) {
LOG(WARNING, "err_count is %d (or 1st time) so forcing a re-zero\n",
error_count_);
state_ = UNINITIALIZED;
@@ -291,7 +290,7 @@
}
}
if (position != NULL) {
- last_good_time_ = ::aos::time::Time::Now();
+ last_good_time_ = ::aos::monotonic_clock::now();
error_count_ = 0;
}
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index cf1a6f8..ec219cc 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -5,18 +5,21 @@
#include <inttypes.h>
#include <math.h>
+#include <chrono>
#include "aos/common/logging/queue_logging.h"
#include "aos/common/messages/robot_state.q.h"
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
-
#include "frc971/wpilib/imu.q.h"
#include "frc971/zeroing/averager.h"
namespace frc971 {
namespace wpilib {
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+
template <uint8_t size>
bool ADIS16448::DoTransaction(uint8_t to_send[size], uint8_t to_receive[size]) {
switch (spi_->Transaction(to_send, to_receive, size)) {
@@ -128,7 +131,7 @@
// Try to initialize repeatedly as long as we're supposed to be running.
while (run_ && !Initialize()) {
- ::aos::time::SleepFor(::aos::time::Time::InMS(50));
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
}
LOG(INFO, "IMU initialized successfully\n");
@@ -150,7 +153,7 @@
}
}
got_an_interrupt = true;
- const ::aos::time::Time read_time = ::aos::time::Time::Now();
+ const monotonic_clock::time_point read_time = monotonic_clock::now();
uint8_t to_send[2 * 14], to_receive[2 * 14];
memset(&to_send[0], 0, sizeof(to_send));
@@ -188,7 +191,9 @@
auto message = imu_values.MakeMessage();
message->fpga_timestamp = dio1_->ReadRisingTimestamp();
- message->monotonic_timestamp_ns = read_time.ToNSec();
+ message->monotonic_timestamp_ns =
+ chrono::duration_cast<chrono::nanoseconds>(read_time.time_since_epoch())
+ .count();
message->gyro_x =
ConvertValue(&to_receive[4], kGyroLsbDegreeSecond * M_PI / 180.0);
@@ -364,7 +369,7 @@
{
uint16_t value;
do {
- ::aos::time::SleepFor(::aos::time::Time::InMS(10));
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(10));
if (!ReadRegister(kMscCtrlAddress, &value)) return false;
} while ((value & (1 << 10)) != 0);
}
diff --git a/frc971/wpilib/gyro_interface.cc b/frc971/wpilib/gyro_interface.cc
index 6b3ab40..9b7bb8b 100644
--- a/frc971/wpilib/gyro_interface.cc
+++ b/frc971/wpilib/gyro_interface.cc
@@ -1,6 +1,7 @@
#include "frc971/wpilib/gyro_interface.h"
#include <inttypes.h>
+#include <chrono>
#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
@@ -35,7 +36,7 @@
}
// Wait for it to assert the fault conditions before reading them.
- ::aos::time::SleepFor(::aos::time::Time::InMS(50));
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
if (!DoTransaction(0x20000000, &result)) {
LOG(WARNING, "failed to clear latched non-fault data\n");
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 8cfdc39..9acb6fc 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -1,6 +1,11 @@
#include "frc971/wpilib/gyro_sender.h"
+#include <fcntl.h>
#include <inttypes.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <chrono>
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
@@ -16,13 +21,15 @@
namespace wpilib {
GyroSender::GyroSender() {}
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
void GyroSender::operator()() {
::aos::SetCurrentThreadName("Gyro");
// Try to initialize repeatedly as long as we're supposed to be running.
while (run_ && !gyro_.InitializeGyro()) {
- ::aos::time::SleepFor(::aos::time::Time::InMS(50));
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
}
LOG(INFO, "gyro initialized successfully\n");
diff --git a/frc971/wpilib/interrupt_edge_counting.cc b/frc971/wpilib/interrupt_edge_counting.cc
index 4bdb2d2..5c10b89 100644
--- a/frc971/wpilib/interrupt_edge_counting.cc
+++ b/frc971/wpilib/interrupt_edge_counting.cc
@@ -1,5 +1,7 @@
#include "frc971/wpilib/interrupt_edge_counting.h"
+#include <chrono>
+
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
@@ -64,7 +66,7 @@
// Wait more than the amount of time it takes for a digital input change
// to go from visible to software to having triggered an interrupt.
- ::aos::time::SleepFor(::aos::time::Time::InUS(120));
+ ::std::this_thread::sleep_for(::std::chrono::microseconds(120));
if (TryFinishingIteration()) return;
}
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
index 310b3e3..59770be 100644
--- a/frc971/wpilib/loop_output_handler.cc
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -2,8 +2,9 @@
#include <sys/timerfd.h>
-#include <thread>
+#include <chrono>
#include <functional>
+#include <thread>
#include "aos/linux_code/init.h"
#include "aos/common/messages/robot_state.q.h"
@@ -11,7 +12,9 @@
namespace frc971 {
namespace wpilib {
-LoopOutputHandler::LoopOutputHandler(const ::aos::time::Time &timeout)
+namespace chrono = ::std::chrono;
+
+LoopOutputHandler::LoopOutputHandler(::std::chrono::nanoseconds timeout)
: watchdog_(this, timeout) {}
void LoopOutputHandler::operator()() {
@@ -44,12 +47,12 @@
}
LoopOutputHandler::Watchdog::Watchdog(LoopOutputHandler *handler,
- const ::aos::time::Time &timeout)
+ ::std::chrono::nanoseconds timeout)
: handler_(handler),
timeout_(timeout),
- timerfd_(timerfd_create(::aos::time::Time::kDefaultClock, 0)) {
+ timerfd_(timerfd_create(CLOCK_MONOTONIC, 0)) {
if (timerfd_.get() == -1) {
- PLOG(FATAL, "timerfd_create(Time::kDefaultClock, 0)");
+ PLOG(FATAL, "timerfd_create(CLOCK_MONOTONIC, 0)");
}
}
@@ -65,7 +68,11 @@
void LoopOutputHandler::Watchdog::Reset() {
itimerspec value = itimerspec();
- value.it_value = timeout_.ToTimespec();
+ value.it_value.tv_sec = chrono::duration_cast<chrono::seconds>(timeout_).count();
+ value.it_value.tv_nsec =
+ chrono::duration_cast<chrono::nanoseconds>(
+ timeout_ - chrono::seconds(value.it_value.tv_sec))
+ .count();
PCHECK(timerfd_settime(timerfd_.get(), 0, &value, nullptr));
}
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index 144093d..14a5cc5 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -22,7 +22,7 @@
class LoopOutputHandler {
public:
LoopOutputHandler(
- const ::aos::time::Time &timeout = ::aos::time::Time::InSeconds(0.10));
+ ::std::chrono::nanoseconds timeout = ::std::chrono::milliseconds(100));
void Quit() { run_ = false; }
@@ -54,7 +54,7 @@
// LoopOutputHandler whenever the timerfd expires.
class Watchdog {
public:
- Watchdog(LoopOutputHandler *handler, const ::aos::time::Time &timeout);
+ Watchdog(LoopOutputHandler *handler, ::std::chrono::nanoseconds timeout);
void operator()();
@@ -65,7 +65,7 @@
private:
LoopOutputHandler *const handler_;
- const ::aos::time::Time timeout_;
+ const ::std::chrono::nanoseconds timeout_;
::aos::ScopedFD timerfd_;