Make a DurationInSeconds function
Also, run clang-format on all the files I changed because I am too lazy
to figure out how to call clang-format on just the lines I changed.
Change-Id: I071c6f81dced533a0a269f25a258348132a7056a
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 6070a71..ee17f49 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -5,26 +5,21 @@
#include <chrono>
#include <cmath>
-#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
+#include "aos/util/phased_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2017/control_loops/drivetrain/drivetrain_base.h"
namespace y2017 {
namespace actors {
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::monotonic_clock;
+using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace {
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
-
const ProfileParameters kGearBallBackDrive = {3.0, 3.5};
const ProfileParameters kGearDrive = {1.5, 2.0};
const ProfileParameters kGearFastDrive = {2.0, 2.5};
@@ -163,11 +158,11 @@
SendSuperstructureGoal();
// Shoot from the peg.
- //set_indexer_angular_velocity(-2.1 * M_PI);
- //SendSuperstructureGoal();
- //this_thread::sleep_for(chrono::milliseconds(1750));
+ // set_indexer_angular_velocity(-2.1 * M_PI);
+ // SendSuperstructureGoal();
+ // this_thread::sleep_for(chrono::milliseconds(1750));
- //this_thread::sleep_for(chrono::milliseconds(500));
+ // this_thread::sleep_for(chrono::milliseconds(500));
this_thread::sleep_for(chrono::milliseconds(750));
set_indexer_angular_velocity(0.0);
set_vision_track(false);
@@ -180,7 +175,7 @@
SendSuperstructureGoal();
LOG(INFO, "Starting drive back %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(-2.75, kDriveDirection * 1.24, kSlowDrive,
kFirstGearStartTurn);
@@ -192,8 +187,7 @@
if (!WaitForDriveNear(0.2, 0.0)) return true;
this_thread::sleep_for(chrono::milliseconds(200));
// Trip the hopper
- StartDrive(0.0, kDriveDirection * 1.10, kSlowDrive,
- kSmashTurn);
+ StartDrive(0.0, kDriveDirection * 1.10, kSlowDrive, kSmashTurn);
if (!WaitForDriveNear(0.2, 0.35)) return true;
set_vision_track(true);
@@ -201,11 +195,10 @@
SendSuperstructureGoal();
if (!WaitForDriveNear(0.2, 0.2)) return true;
- StartDrive(0.0, -kDriveDirection * 0.15, kSlowDrive,
- kSmashTurn);
+ StartDrive(0.0, -kDriveDirection * 0.15, kSlowDrive, kSmashTurn);
LOG(INFO, "Starting second shot %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_indexer_angular_velocity(-2.15 * M_PI);
SendSuperstructureGoal();
if (!WaitForDriveNear(0.2, 0.1)) return true;
@@ -235,7 +228,8 @@
set_intake_goal(0.07);
SendSuperstructureGoal();
- StartDrive(-3.42, kDriveDirection * (M_PI / 10 - 0.057) , kSlowDrive, kFirstTurn);
+ StartDrive(-3.42, kDriveDirection * (M_PI / 10 - 0.057), kSlowDrive,
+ kFirstTurn);
if (!WaitForDriveNear(3.30, 0.0)) return true;
LOG(INFO, "Turn ended: %f left to go\n", DriveDistanceLeft());
// We can go to 2.50 before we hit the previous profile.
@@ -281,7 +275,7 @@
SendSuperstructureGoal();
LOG(INFO, "Started shooting at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
this_thread::sleep_for(start_time + chrono::seconds(9) -
monotonic_clock::now());
@@ -302,7 +296,8 @@
} break;
}
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index 8a3644e..85af86b 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -127,9 +127,7 @@
indexer_dt_velocity_ =
(new_position.indexer.encoder - indexer_last_position_) /
- chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
indexer_last_position_ = new_position.indexer.encoder;
stuck_indexer_detector_->Correct(Y_);
@@ -144,9 +142,7 @@
indexer_average_angular_velocity_ =
(indexer_history_[indexer_oldest_history_position] -
indexer_history_[indexer_history_position_]) /
- (chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count() *
+ (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
static_cast<double>(kHistoryLength - 1));
// Ready if average angular velocity is close to the goal.
@@ -255,9 +251,8 @@
::Eigen::Matrix<double, 2, 1> goal_state =
profile_.Update(unprofiled_goal_(2, 0), unprofiled_goal_(3, 0));
- constexpr double kDt = chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ constexpr double kDt =
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
loop_->mutable_next_R(0, 0) = 0.0;
// TODO(austin): This might not handle saturation right, but I'm not sure I
@@ -297,7 +292,8 @@
bool ColumnProfiledSubsystem::CheckHardLimits() {
// Returns whether hard limits have been exceeded.
- if (turret_position() > range_.upper_hard || turret_position() < range_.lower_hard) {
+ if (turret_position() > range_.upper_hard ||
+ turret_position() < range_.lower_hard) {
LOG(ERROR,
"ColumnProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
turret_position(), range_.lower_hard, range_.upper_hard);
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 73da9b9..4a4dfbd 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -148,9 +148,7 @@
status->voltage_error = X_hat(5, 0);
status->calculated_velocity =
(turret_position() - turret_last_position_) /
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
status->estimator_state = EstimatorState(0);
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index fee9124..5023e6f 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -98,8 +98,7 @@
X_hat_current_ = loop_->X_hat();
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
- dt_velocity_ = dt_position_ /
- chrono::duration_cast<chrono::duration<double>>(dt).count();
+ dt_velocity_ = dt_position_ / ::aos::time::DurationInSeconds(dt);
fixed_dt_velocity_ = dt_position_ / 0.00505;
loop_->Update(disabled, dt);
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index c8f55ff..46a3af6 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -256,9 +256,7 @@
freeze_indexer_ = freeze_indexer;
}
// Triggers the turret to freeze relative to the indexer.
- void set_freeze_turret(bool freeze_turret) {
- freeze_turret_ = freeze_turret;
- }
+ void set_freeze_turret(bool freeze_turret) { freeze_turret_ = freeze_turret; }
// Simulates the superstructure for a single timestep.
void Simulate() {
@@ -511,9 +509,8 @@
RunIteration(enabled);
- const double loop_time =
- chrono::duration_cast<chrono::duration<double>>(
- monotonic_clock::now() - loop_start_time).count();
+ const double loop_time = ::aos::time::DurationInSeconds(
+ monotonic_clock::now() - loop_start_time);
const double intake_acceleration =
(superstructure_plant_.intake_velocity() - begin_intake_velocity) /
loop_time;
@@ -958,7 +955,6 @@
superstructure_plant_.set_hood_voltage_offset(0.0);
RunForTime(chrono::seconds(5), false);
-
EXPECT_EQ(hood::Hood::State::RUNNING, superstructure_.hood().state());
EXPECT_EQ(intake::Intake::State::RUNNING, superstructure_.intake().state());
EXPECT_EQ(column::Column::State::RUNNING, superstructure_.column().state());
@@ -986,7 +982,6 @@
ASSERT_TRUE(goal.Send());
}
-
RunForTime(chrono::seconds(5));
VerifyNearGoal();
EXPECT_TRUE(superstructure_queue_.status->shooter.ready);
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.cc b/y2017/control_loops/superstructure/vision_time_adjuster.cc
index 57f3228..1e9c94f 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.cc
@@ -59,12 +59,8 @@
monotonic_clock::time_point after_time, double after_data,
monotonic_clock::time_point current_time) {
const double age_ratio =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- current_time - before_time)
- .count() /
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(after_time -
- before_time)
- .count();
+ ::aos::time::DurationInSeconds(current_time - before_time) /
+ ::aos::time::DurationInSeconds(after_time - before_time);
return before_data * (1 - age_ratio) + after_data * age_ratio;
}
@@ -142,8 +138,7 @@
if (column_angle_is_valid && drivetrain_angle_is_valid) {
LOG(INFO, "Accepting Vision angle of %f, age %f\n",
most_recent_vision_angle_,
- chrono::duration_cast<chrono::duration<double>>(
- monotonic_now - last_target_time).count());
+ ::aos::time::DurationInSeconds(monotonic_now - last_target_time));
most_recent_vision_reading_ = vision_status->angle;
most_recent_vision_angle_ =
vision_status->angle + column_angle + drivetrain_angle;