Upgraded the rest of Time.
Change-Id: I0ee083837e51d8f74a798b7ba14a3b6bb3859f35
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 44b21ee..313a818 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -52,8 +52,10 @@
turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
kDrivetrainTurnWidth / 2.0);
+ ::aos::time::PhasedLoop phased_loop(chrono::milliseconds(5),
+ chrono::microseconds(2500));
while (true) {
- ::aos::time::PhasedLoopXMS(5, 2500);
+ phased_loop.SleepUntilNext();
drivetrain_queue.status.FetchLatest();
if (drivetrain_queue.status.get()) {
diff --git a/y2015_bot3/actors/drivetrain_actor_main.cc b/y2015_bot3/actors/drivetrain_actor_main.cc
index a31b233..d12d61a 100644
--- a/y2015_bot3/actors/drivetrain_actor_main.cc
+++ b/y2015_bot3/actors/drivetrain_actor_main.cc
@@ -4,8 +4,6 @@
#include "y2015_bot3/actors/drivetrain_action.q.h"
#include "y2015_bot3/actors/drivetrain_actor.h"
-using ::aos::time::Time;
-
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init(-1);
diff --git a/y2015_bot3/autonomous/auto.cc b/y2015_bot3/autonomous/auto.cc
index 0eaf921..35ad8d2 100644
--- a/y2015_bot3/autonomous/auto.cc
+++ b/y2015_bot3/autonomous/auto.cc
@@ -16,7 +16,6 @@
#include "y2015_bot3/control_loops/intake/intake.q.h"
#include "y2015_bot3/control_loops/drivetrain/drivetrain_base.h"
-using ::aos::time::Time;
using ::frc971::control_loops::drivetrain_queue;
using ::y2015_bot3::control_loops::intake_queue;
using ::y2015_bot3::control_loops::elevator_queue;
@@ -29,8 +28,9 @@
double acceleration;
};
-namespace time = ::aos::time;
+using ::aos::monotonic_clock;
namespace actors = ::frc971::actors;
+namespace chrono = ::std::chrono;
const ProfileParams kFastDrive = {3.0, 3.5};
const ProfileParams kLastDrive = {4.0, 4.0};
@@ -99,9 +99,11 @@
LOG(ERROR, "No action, not waiting\n");
return;
}
+ ::aos::time::PhasedLoop phased_loop(chrono::milliseconds(5),
+ chrono::microseconds(2500));
while (true) {
+ phased_loop.SleepUntilNext();
// Poll the running bit and auto done bits.
- ::aos::time::PhasedLoopXMS(5, 2500);
if (!action->Running() || ShouldExitAuto()) {
return;
}
@@ -126,11 +128,14 @@
}
}
-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 monotonic_now = monotonic_clock::now();
+ monotonic_clock::time_point end_time = monotonic_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_grabber_control.MakeWithBuilder()
.can_grabber_voltage(voltage).can_grabbers(false).Send();
@@ -139,11 +144,11 @@
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();
}
}
@@ -152,8 +157,8 @@
ResetDrivetrain();
// Launch can grabbers.
- //GrabberForTime(12.0, 0.26);
- GrabberForTime(6.0, 0.40);
+ // GrabberForTime(12.0, chrono::milliseconds(260));
+ GrabberForTime(6.0, chrono::milliseconds(400));
if (ShouldExitAuto()) return;
InitializeEncoders();
ResetDrivetrain();
@@ -192,18 +197,18 @@
.right_goal(right_initial_position + kMoveBackDistance)
.right_velocity_goal(0)
.Send();
- GrabberForTime(12.0, 0.02);
+ GrabberForTime(12.0, chrono::milliseconds(20));
if (ShouldExitAuto()) return;
// We shouldn't need as much power at this point, so lower the can grabber
// voltages to avoid damaging the motors due to stalling.
- GrabberForTime(4.0, 0.75);
+ GrabberForTime(4.0, chrono::milliseconds(750));
if (ShouldExitAuto()) return;
- GrabberForTime(-3.0, 0.25);
+ GrabberForTime(-3.0, chrono::milliseconds(250));
if (ShouldExitAuto()) return;
- GrabberForTime(-12.0, 1.0);
+ GrabberForTime(-12.0, chrono::milliseconds(1000));
if (ShouldExitAuto()) return;
- GrabberForTime(-3.0, 12.0);
+ GrabberForTime(-3.0, chrono::milliseconds(12000));
}
const ProfileParams kElevatorProfile = {1.0, 5.0};
@@ -312,7 +317,7 @@
}
void TripleCanAuto() {
- ::aos::time::Time start_time = ::aos::time::Time::Now();
+ monotonic_clock::time_point start_time = monotonic_clock::now();
::std::unique_ptr<::frc971::actors::DrivetrainAction> drive;
InitializeEncoders();
@@ -428,15 +433,17 @@
LOG(INFO, "Got the tote!\n");
break;
}
- if ((::aos::time::Time::Now() - start_time) > time::Time::InSeconds(15.0)) {
+ if (monotonic_clock::now() > chrono::seconds(15) + start_time) {
LOG(INFO, "Out of time\n");
break;
}
}
- ::aos::time::Time end_time = ::aos::time::Time::Now();
+ monotonic_clock::time_point end_time = monotonic_clock::now();
LOG(INFO, "Ended auto with %f to spare\n",
- (time::Time::InSeconds(15.0) - (end_time - start_time)).ToSeconds());
+ chrono::duration_cast<chrono::duration<double>>(chrono::seconds(15) -
+ (end_time - start_time))
+ .count());
if (!intake_queue.goal.MakeWithBuilder().movement(0.0).claw_closed(true)
.Send()) {
LOG(ERROR, "Sending intake goal failed.\n");
@@ -444,8 +451,11 @@
}
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());
// TODO(comran): Add various options for different autos down below.
//CanGrabberAuto();
diff --git a/y2015_bot3/autonomous/auto_main.cc b/y2015_bot3/autonomous/auto_main.cc
index 2c006e0..1357d7c 100644
--- a/y2015_bot3/autonomous/auto_main.cc
+++ b/y2015_bot3/autonomous/auto_main.cc
@@ -6,8 +6,6 @@
#include "y2015_bot3/autonomous/auto.q.h"
#include "y2015_bot3/autonomous/auto.h"
-using ::aos::time::Time;
-
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init(-1);
@@ -24,12 +22,15 @@
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Starting auto mode\n");
- ::aos::time::Time start_time = ::aos::time::Time::Now();
+ const ::aos::monotonic_clock::time_point start_time =
+ ::aos::monotonic_clock::now();
::y2015_bot3::autonomous::HandleAuto();
- ::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
+ const 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 (::y2015_bot3::autonomous::autonomous->run_auto) {
::y2015_bot3::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
diff --git a/y2015_bot3/control_loops/intake/intake_lib_test.cc b/y2015_bot3/control_loops/intake/intake_lib_test.cc
index 1c8b604..d311d5c 100644
--- a/y2015_bot3/control_loops/intake/intake_lib_test.cc
+++ b/y2015_bot3/control_loops/intake/intake_lib_test.cc
@@ -3,17 +3,20 @@
#include <math.h>
#include <unistd.h>
+#include <chrono>
+
#include "gtest/gtest.h"
#include "aos/common/queue.h"
#include "aos/common/controls/control_loop_test.h"
#include "y2015_bot3/control_loops/intake/intake.q.h"
-using ::aos::time::Time;
-
namespace y2015_bot3 {
namespace control_loops {
namespace testing {
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
// Class which simulates the elevator and sends out queue messages with the
// position.
class IntakeSimulation {
@@ -60,9 +63,9 @@
}
// Runs iterations until the specified amount of simulated time has elapsed.
- void RunForTime(double run_for, bool enabled = true) {
- const auto start_time = Time::Now();
- while (Time::Now() < start_time + Time::InSeconds(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);
}
}
@@ -84,7 +87,7 @@
.Send());
// Run for a bit.
- RunForTime(1.0);
+ RunForTime(chrono::seconds(1));
ASSERT_TRUE(queue_.output.FetchLatest());
EXPECT_EQ(queue_.output->intake, 0.0);
@@ -97,7 +100,7 @@
.Send());
// Run for a bit.
- RunForTime(1.0);
+ RunForTime(chrono::seconds(1));
ASSERT_TRUE(queue_.output.FetchLatest());
EXPECT_GT(queue_.output->intake, 0.0);
@@ -110,7 +113,7 @@
.Send());
// Run for a bit.
- RunForTime(1.0);
+ RunForTime(chrono::seconds(1));
ASSERT_TRUE(queue_.output.FetchLatest());
EXPECT_LT(queue_.output->intake, 0.0);
diff --git a/y2015_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index edd2428..b92d02b 100644
--- a/y2015_bot3/wpilib/wpilib_interface.cc
+++ b/y2015_bot3/wpilib/wpilib_interface.cc
@@ -67,6 +67,8 @@
namespace y2015_bot3 {
namespace wpilib {
+namespace chrono = ::std::chrono;
+
double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
::y2015_bot3::control_loops::drivetrain::kDrivetrainEncoderRatio *
@@ -137,8 +139,8 @@
&DriverStation::GetInstance();
#endif
- ::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_) {
@@ -242,8 +244,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_) {
{
@@ -423,7 +425,7 @@
// Writes out can grabber voltages.
class CanGrabberWriter : public LoopOutputHandler {
public:
- CanGrabberWriter() : LoopOutputHandler(::aos::time::Time::InSeconds(0.05)) {}
+ CanGrabberWriter() : LoopOutputHandler(chrono::milliseconds(50)) {}
void set_can_grabber_talon1(::std::unique_ptr<Talon> t) {
can_grabber_talon1_ = ::std::move(t);