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/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_;