Converted trapezoid_profile over to monotonic_clock
This also involves updating all the callers, and updating
control_loop's frequency variable.
Change-Id: Ic88d2715db30efcc25721da2dd8c89910ede7788
diff --git a/aos/common/actions/actor.h b/aos/common/actions/actor.h
index b3a0220..e591084 100644
--- a/aos/common/actions/actor.h
+++ b/aos/common/actions/actor.h
@@ -65,11 +65,12 @@
// succeeded.
bool WaitOrCancel(const ::aos::time::Time& duration) {
return !WaitUntil([]() {
- ::aos::time::PhasedLoopXMS(
- ::aos::controls::kLoopFrequency.ToMSec(), 2500);
- return false;
- },
- ::aos::time::Time::Now() + duration);
+ ::aos::time::PhasedLoopXMS(
+ ::std::chrono::duration_cast<::std::chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
+ return false;
+ }, ::aos::time::Time::Now() + duration);
}
// Returns true if the action should be canceled.
diff --git a/aos/common/controls/control_loop.h b/aos/common/controls/control_loop.h
index d56e5d8..f589ed7 100644
--- a/aos/common/controls/control_loop.h
+++ b/aos/common/controls/control_loop.h
@@ -37,7 +37,8 @@
};
// Control loops run this often, "starting" at time 0.
-constexpr time::Time kLoopFrequency = time::Time::InSeconds(0.005);
+constexpr ::std::chrono::nanoseconds kLoopFrequency =
+ ::std::chrono::milliseconds(5);
// Provides helper methods to assist in writing control loops.
// This template expects to be constructed with a queue group as an argument
diff --git a/aos/common/util/trapezoid_profile.cc b/aos/common/util/trapezoid_profile.cc
index f441fbe..4e4546e 100644
--- a/aos/common/util/trapezoid_profile.cc
+++ b/aos/common/util/trapezoid_profile.cc
@@ -7,10 +7,8 @@
namespace aos {
namespace util {
-TrapezoidProfile::TrapezoidProfile(const time::Time &delta_time)
- : maximum_acceleration_(0),
- maximum_velocity_(0),
- timestep_(delta_time) {
+TrapezoidProfile::TrapezoidProfile(::std::chrono::nanoseconds delta_time)
+ : maximum_acceleration_(0), maximum_velocity_(0), timestep_(delta_time) {
output_.setZero();
}
@@ -26,7 +24,9 @@
double goal_velocity) {
CalculateTimes(goal_position - output_(0), goal_velocity);
- double next_timestep = timestep_.ToSeconds();
+ double next_timestep =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(timestep_)
+ .count();
if (acceleration_time_ > next_timestep) {
UpdateVals(acceleration_, next_timestep);
diff --git a/aos/common/util/trapezoid_profile.h b/aos/common/util/trapezoid_profile.h
index fe63352..cc91db1 100644
--- a/aos/common/util/trapezoid_profile.h
+++ b/aos/common/util/trapezoid_profile.h
@@ -17,7 +17,7 @@
class TrapezoidProfile {
public:
// delta_time is how long between each call to Update.
- TrapezoidProfile(const time::Time &delta_time);
+ TrapezoidProfile(::std::chrono::nanoseconds delta_time);
// Updates the state.
const Eigen::Matrix<double, 2, 1> &Update(double goal_position,
@@ -58,7 +58,7 @@
double maximum_velocity_;
// How long between calls to Update.
- const time::Time timestep_;
+ ::std::chrono::nanoseconds timestep_;
DISALLOW_COPY_AND_ASSIGN(TrapezoidProfile);
};
diff --git a/aos/common/util/trapezoid_profile_test.cc b/aos/common/util/trapezoid_profile_test.cc
index 1dfeff3..c6e811c 100644
--- a/aos/common/util/trapezoid_profile_test.cc
+++ b/aos/common/util/trapezoid_profile_test.cc
@@ -45,11 +45,13 @@
}
private:
- static const time::Time delta_time;
+ static constexpr ::std::chrono::nanoseconds delta_time =
+ ::std::chrono::milliseconds(10);
Eigen::Matrix<double, 2, 1> position_;
};
-const time::Time TrapezoidProfileTest::delta_time = time::Time::InSeconds(0.01);
+
+constexpr ::std::chrono::nanoseconds TrapezoidProfileTest::delta_time;
TEST_F(TrapezoidProfileTest, ReachesGoal) {
for (int i = 0; i < 450; ++i) {
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 037fab9..c0ee47b 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -19,6 +19,8 @@
namespace y2014 {
namespace actors {
+namespace chrono = ::std::chrono;
+
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
loop_(constants::GetValues().make_drivetrain_loop()) {
@@ -34,8 +36,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(5));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(5));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index faeeaf0..af310f0 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -4,6 +4,7 @@
#include <algorithm>
#include <limits>
+#include <chrono>
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -15,11 +16,13 @@
namespace y2014 {
namespace control_loops {
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
using ::y2014::control_loops::shooter::kSpringConstant;
using ::y2014::control_loops::shooter::kMaxExtension;
using ::y2014::control_loops::shooter::kDt;
using ::y2014::control_loops::shooter::MakeShooterLoop;
-using ::aos::time::Time;
void ZeroedStateFeedbackLoop::CapU() {
const double old_voltage = voltage_;
@@ -115,11 +118,6 @@
my_shooter),
shooter_(MakeShooterLoop()),
state_(STATE_INITIALIZE),
- loading_problem_end_time_(0, 0),
- load_timeout_(0, 0),
- shooter_brake_set_time_(0, 0),
- unload_timeout_(0, 0),
- shot_end_time_(0, 0),
cycles_not_moved_(0),
shot_count_(0),
zeroed_(false),
@@ -404,16 +402,18 @@
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
state_ = STATE_LOADING_PROBLEM;
- loading_problem_end_time_ = Time::Now() + kLoadProblemEndTimeout;
+ loading_problem_end_time_ =
+ monotonic_clock::now() + kLoadProblemEndTimeout;
}
}
- if (load_timeout_ < Time::Now()) {
+ if (load_timeout_ < monotonic_clock::now()) {
if (position) {
// If none of the sensors is triggered, estop.
// Otherwise, trigger anyways if it has been 0.5 seconds more.
if (!(position->pusher_distal.current ||
position->pusher_proximal.current) ||
- (load_timeout_ + Time::InSeconds(0.5) < Time::Now())) {
+ (load_timeout_ + chrono::milliseconds(500) <
+ monotonic_clock::now())) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -428,7 +428,7 @@
}
// We got to the goal, but the latch hasn't registered as down. It might
// be stuck, or on it's way but not there yet.
- if (Time::Now() > loading_problem_end_time_) {
+ if (monotonic_clock::now() > loading_problem_end_time_) {
// Timeout by unloading.
Unload();
} else if (position && position->plunger && position->latch) {
@@ -468,7 +468,7 @@
// We are there, set the brake and move on.
latch_piston_ = true;
brake_piston_ = true;
- shooter_brake_set_time_ = Time::Now() + kShooterBrakeSetTime;
+ shooter_brake_set_time_ = monotonic_clock::now() + kShooterBrakeSetTime;
state_ = STATE_READY;
} else {
latch_piston_ = true;
@@ -482,7 +482,7 @@
LOG(DEBUG, "In ready\n");
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
- if (Time::Now() > shooter_brake_set_time_) {
+ if (monotonic_clock::now() > shooter_brake_set_time_) {
status->ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
@@ -491,7 +491,7 @@
if (goal && goal->shot_requested && !disabled) {
LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
- shot_end_time_ = Time::Now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
firing_starting_position_ = shooter_.absolute_position();
state_ = STATE_FIRE;
}
@@ -526,7 +526,7 @@
if (position->plunger) {
// If disabled and the plunger is still back there, reset the
// timeout.
- shot_end_time_ = Time::Now() + kShotEndTimeout;
+ shot_end_time_ = monotonic_clock::now() + kShotEndTimeout;
}
}
}
@@ -545,7 +545,7 @@
if (((::std::abs(firing_starting_position_ -
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
- Time::Now() > shot_end_time_) &&
+ monotonic_clock::now() > shot_end_time_) &&
::aos::robot_state->voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
@@ -585,10 +585,10 @@
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
latch_piston_ = false;
state_ = STATE_UNLOAD_MOVE;
- unload_timeout_ = Time::Now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
}
- if (Time::Now() > unload_timeout_) {
+ if (monotonic_clock::now() > unload_timeout_) {
// We have been stuck trying to unload for way too long, give up and
// turn everything off.
state_ = STATE_ESTOP;
@@ -599,7 +599,7 @@
break;
case STATE_UNLOAD_MOVE: {
if (disabled) {
- unload_timeout_ = Time::Now() + kUnloadTimeout;
+ unload_timeout_ = monotonic_clock::now() + kUnloadTimeout;
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
}
cap_goal = true;
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index cccb953..fb991e3 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -19,8 +19,6 @@
class ShooterTest_RezeroWhileUnloading_Test;
};
-using ::aos::time::Time;
-
// Note: Everything in this file assumes that there is a 1 cycle delay between
// power being requested and it showing up at the motor. It assumes that
// X_hat(2, 1) is the voltage being applied as well. It will go unstable if
@@ -113,13 +111,19 @@
bool capped_goal_;
};
-const Time kUnloadTimeout = Time::InSeconds(10);
-const Time kLoadTimeout = Time::InSeconds(2.0);
-const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
-const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
+static constexpr ::std::chrono::nanoseconds kUnloadTimeout =
+ ::std::chrono::seconds(10);
+static constexpr ::std::chrono::nanoseconds kLoadTimeout =
+ ::std::chrono::seconds(2);
+static constexpr ::std::chrono::nanoseconds kLoadProblemEndTimeout =
+ ::std::chrono::seconds(1);
+static constexpr ::std::chrono::nanoseconds kShooterBrakeSetTime =
+ ::std::chrono::milliseconds(50);
// Time to wait after releasing the latch piston before winching back again.
-const Time kShotEndTimeout = Time::InSeconds(0.2);
-const Time kPrepareFireEndTime = Time::InMS(40);
+static constexpr ::std::chrono::nanoseconds kShotEndTimeout =
+ ::std::chrono::milliseconds(200);
+static constexpr ::std::chrono::nanoseconds kPrepareFireEndTime =
+ ::std::chrono::milliseconds(40);
class ShooterMotor
: public aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue> {
@@ -171,12 +175,12 @@
// Enter state STATE_UNLOAD
void Unload() {
state_ = STATE_UNLOAD;
- unload_timeout_ = Time::Now() + kUnloadTimeout;
+ unload_timeout_ = ::aos::monotonic_clock::now() + kUnloadTimeout;
}
// Enter state STATE_LOAD
void Load() {
state_ = STATE_LOAD;
- load_timeout_ = Time::Now() + kLoadTimeout;
+ load_timeout_ = ::aos::monotonic_clock::now() + kLoadTimeout;
}
::y2014::control_loops::ShooterQueue::Position last_position_;
@@ -187,19 +191,24 @@
State state_;
// time to giving up on loading problem
- Time loading_problem_end_time_;
+ ::aos::monotonic_clock::time_point loading_problem_end_time_ =
+ ::aos::monotonic_clock::min_time;
// The end time when loading for it to timeout.
- Time load_timeout_;
+ ::aos::monotonic_clock::time_point load_timeout_ =
+ ::aos::monotonic_clock::min_time;
// wait for brake to set
- Time shooter_brake_set_time_;
+ ::aos::monotonic_clock::time_point shooter_brake_set_time_ =
+ ::aos::monotonic_clock::min_time;
// The timeout for unloading.
- Time unload_timeout_;
+ ::aos::monotonic_clock::time_point unload_timeout_ =
+ ::aos::monotonic_clock::min_time;
// time that shot must have completed
- Time shot_end_time_;
+ ::aos::monotonic_clock::time_point shot_end_time_ =
+ ::aos::monotonic_clock::min_time;
// track cycles that we are stuck to detect errors
int cycles_not_moved_;
diff --git a/y2014_bot3/actions/drivetrain_action.cc b/y2014_bot3/actions/drivetrain_action.cc
index bef5bd6..d7660b3 100644
--- a/y2014_bot3/actions/drivetrain_action.cc
+++ b/y2014_bot3/actions/drivetrain_action.cc
@@ -16,6 +16,8 @@
namespace bot3 {
namespace actions {
+namespace chrono = ::std::chrono;
+
DrivetrainAction::DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s)
: ::frc971::actions::ActionBase
<::frc971::actions::DrivetrainActionQueueGroup>(s) {}
@@ -29,8 +31,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(10));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(10));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 5ace60d..f998fff 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -18,6 +18,8 @@
namespace frc971 {
namespace actors {
+namespace chrono = ::std::chrono;
+
DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup* s)
: aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s) {}
@@ -30,8 +32,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(5));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(5));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2015/actors/held_to_lift_actor.cc b/y2015/actors/held_to_lift_actor.cc
index 2336364..ddb2e29 100644
--- a/y2015/actors/held_to_lift_actor.cc
+++ b/y2015/actors/held_to_lift_actor.cc
@@ -17,6 +17,8 @@
constexpr ProfileParams kFastElevatorMove{1.2, 5.0};
} // namespace
+namespace chrono = ::std::chrono;
+
HeldToLiftActor::HeldToLiftActor(HeldToLiftActionQueueGroup *queues)
: FridgeActorBase<HeldToLiftActionQueueGroup>(queues) {}
@@ -90,7 +92,8 @@
MakeLiftAction(params.lift_params);
lift_action->Start();
while (lift_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
diff --git a/y2015/actors/pickup_actor.cc b/y2015/actors/pickup_actor.cc
index 0863ba6..4ae988e 100644
--- a/y2015/actors/pickup_actor.cc
+++ b/y2015/actors/pickup_actor.cc
@@ -92,7 +92,10 @@
::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(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(
+ ::std::chrono::duration_cast<::std::chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) return true;
}
diff --git a/y2015/actors/score_actor.cc b/y2015/actors/score_actor.cc
index c0a398f..50b5f35 100644
--- a/y2015/actors/score_actor.cc
+++ b/y2015/actors/score_actor.cc
@@ -12,6 +12,8 @@
using ::frc971::control_loops::fridge_queue;
+namespace chrono = ::std::chrono;
+
namespace frc971 {
namespace actors {
namespace {
@@ -67,7 +69,9 @@
}
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
@@ -91,7 +95,9 @@
bool started_lowering = false;
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
@@ -129,7 +135,9 @@
}
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
@@ -158,7 +166,9 @@
bool has_lifted = false;
while (true) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(), 2500);
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
+ 2500);
if (ShouldCancel()) {
return true;
}
diff --git a/y2015/actors/stack_and_hold_actor.cc b/y2015/actors/stack_and_hold_actor.cc
index ae1eeda..1e87ff3 100644
--- a/y2015/actors/stack_and_hold_actor.cc
+++ b/y2015/actors/stack_and_hold_actor.cc
@@ -19,6 +19,8 @@
constexpr ProfileParams kFastElevatorMove{1.2, 4.0};
} // namespace
+namespace chrono = ::std::chrono;
+
StackAndHoldActor::StackAndHoldActor(StackAndHoldActionQueueGroup *queues)
: FridgeActorBase<StackAndHoldActionQueueGroup>(queues) {}
@@ -76,7 +78,8 @@
MakeStackAction(stack_params);
stack_action->Start();
while (stack_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
diff --git a/y2015/actors/stack_and_lift_actor.cc b/y2015/actors/stack_and_lift_actor.cc
index 32f7417..bff3c85 100644
--- a/y2015/actors/stack_and_lift_actor.cc
+++ b/y2015/actors/stack_and_lift_actor.cc
@@ -14,6 +14,8 @@
namespace frc971 {
namespace actors {
+namespace chrono = ::std::chrono;
+
StackAndLiftActor::StackAndLiftActor(StackAndLiftActionQueueGroup *queues)
: FridgeActorBase<StackAndLiftActionQueueGroup>(queues) {}
@@ -36,7 +38,8 @@
MakeStackAction(stack_params);
stack_action->Start();
while (stack_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
@@ -75,7 +78,8 @@
::std::unique_ptr<LiftAction> lift_action = MakeLiftAction(lift_params);
lift_action->Start();
while (lift_action->Running()) {
- ::aos::time::PhasedLoopXMS(::aos::controls::kLoopFrequency.ToMSec(),
+ ::aos::time::PhasedLoopXMS(chrono::duration_cast<chrono::milliseconds>(
+ ::aos::controls::kLoopFrequency).count(),
2500);
if (ShouldCancel()) {
diff --git a/y2015/control_loops/claw/claw.cc b/y2015/control_loops/claw/claw.cc
index e6ab9e7..ad8aaee 100644
--- a/y2015/control_loops/claw/claw.cc
+++ b/y2015/control_loops/claw/claw.cc
@@ -13,6 +13,7 @@
namespace control_loops {
using ::aos::time::Time;
+namespace chrono = ::std::chrono;
constexpr double kZeroingVoltage = 4.0;
@@ -169,8 +170,9 @@
SetClawOffset(claw_estimator_.offset());
} else if (!disable) {
claw_goal_velocity = claw_zeroing_velocity();
- claw_goal_ +=
- claw_goal_velocity * ::aos::controls::kLoopFrequency.ToSeconds();
+ claw_goal_ += claw_goal_velocity *
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
// Clear the current profile state if we are zeroing.
diff --git a/y2015/control_loops/fridge/fridge.cc b/y2015/control_loops/fridge/fridge.cc
index 083baf2..9f2241a 100644
--- a/y2015/control_loops/fridge/fridge.cc
+++ b/y2015/control_loops/fridge/fridge.cc
@@ -15,6 +15,8 @@
namespace frc971 {
namespace control_loops {
+namespace chrono = ::std::chrono;
+
namespace {
constexpr double kZeroingVoltage = 4.0;
constexpr double kElevatorZeroingVelocity = 0.10;
@@ -333,7 +335,8 @@
LOG(DEBUG, "Moving elevator to safe height.\n");
if (elevator_goal_ < values.fridge.arm_zeroing_height) {
elevator_goal_ += kElevatorSafeHeightVelocity *
- ::aos::controls::kLoopFrequency.ToSeconds();
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
elevator_goal_velocity_ = kElevatorSafeHeightVelocity;
state_ = ZEROING_ELEVATOR;
} else {
@@ -344,7 +347,8 @@
} else if (!disable) {
elevator_goal_velocity_ = elevator_zeroing_velocity();
elevator_goal_ += elevator_goal_velocity_ *
- ::aos::controls::kLoopFrequency.ToSeconds();
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
// Bypass motion profiles while we are zeroing.
@@ -380,8 +384,9 @@
LOG(DEBUG, "Zeroed the arm!\n");
} else if (!disable) {
arm_goal_velocity_ = arm_zeroing_velocity();
- arm_goal_ +=
- arm_goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ arm_goal_ += arm_goal_velocity_ *
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
// Bypass motion profiles while we are zeroing.
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 5a83626..c48e762 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -19,6 +19,8 @@
namespace frc971 {
namespace actors {
+namespace chrono = ::std::chrono;
+
using ::y2015_bot3::control_loops::drivetrain_queue;
using ::y2015_bot3::control_loops::kDrivetrainTurnWidth;
@@ -35,8 +37,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile profile(chrono::milliseconds(5));
+ ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(5));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
diff --git a/y2015_bot3/control_loops/elevator/elevator.cc b/y2015_bot3/control_loops/elevator/elevator.cc
index 16ba064..d8b1368 100644
--- a/y2015_bot3/control_loops/elevator/elevator.cc
+++ b/y2015_bot3/control_loops/elevator/elevator.cc
@@ -1,6 +1,7 @@
#include "y2015_bot3/control_loops/elevator/elevator.h"
#include <cmath>
+#include <chrono>
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -10,6 +11,8 @@
namespace y2015_bot3 {
namespace control_loops {
+namespace chrono = ::std::chrono;
+
void SimpleCappedStateFeedbackLoop::CapU() {
mutable_U(0, 0) = ::std::min(U(0, 0), max_voltage_);
mutable_U(0, 0) = ::std::max(U(0, 0), -max_voltage_);
@@ -131,8 +134,10 @@
// Move the elevator either up or down based on where the zeroing hall
// effect is located.
- goal_velocity_ = zeroing_velocity_temp;
- goal_ += goal_velocity_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ goal_velocity_ = zeroing_velocity_temp;
+ goal_ += goal_velocity_ *
+ chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count();
}
}
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 629b980..c9ee712 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -1,5 +1,7 @@
#include "y2016/control_loops/shooter/shooter.h"
+#include <chrono>
+
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
@@ -12,6 +14,7 @@
namespace shooter {
using ::aos::time::Time;
+namespace chrono = ::std::chrono;
// TODO(austin): Pseudo current limit?
@@ -57,7 +60,8 @@
// Compute the distance moved over that time period.
status->avg_angular_velocity =
(history_[oldest_history_position] - history_[history_position_]) /
- (::aos::controls::kLoopFrequency.ToSeconds() *
+ (chrono::duration_cast<chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency).count() *
static_cast<double>(kHistoryLength - 1));
status->angular_velocity = loop_->X_hat(1, 0);