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/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 5f95072..4f68f20 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -5,8 +5,8 @@
#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 "y2016/control_loops/drivetrain/drivetrain_base.h"
@@ -17,8 +17,8 @@
namespace y2016 {
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;
@@ -43,10 +43,6 @@
const ProfileParameters kTwoBallBallPickup = {2.0, 1.75};
const ProfileParameters kTwoBallBallPickupAccel = {2.0, 2.5};
-double DoubleSeconds(monotonic_clock::duration duration) {
- return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
- .count();
-}
} // namespace
AutonomousActor::AutonomousActor(
@@ -594,7 +590,7 @@
WaitForIntake();
LOG(INFO, "Intake done at %f seconds, starting to drive\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (ShouldCancel()) return;
const double kDriveDistance = 5.05;
StartDrive(-kDriveDistance, 0.0, kTwoBallLowDrive, kSlowTurn);
@@ -609,12 +605,12 @@
const bool ball_detected = ::y2016::sensors::ball_detector->voltage > 2.5;
first_ball_there = ball_detected;
LOG(INFO, "Saw the ball: %d at %f\n", first_ball_there,
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
}
MoveSuperstructure(0.10, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
false, 0.0);
LOG(INFO, "Shutting off rollers at %f seconds, starting to straighten out\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(0.0, -0.4, kTwoBallLowDrive, kSwerveTurn);
MoveSuperstructure(-0.05, -0.010, 0.0, {8.0, 40.0}, {4.0, 10.0}, {10.0, 25.0},
false, 0.0);
@@ -634,7 +630,7 @@
if (!WaitForDriveDone()) return;
LOG(INFO, "First shot done driving at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForSuperstructureProfile();
@@ -657,7 +653,7 @@
}
LOG(INFO, "First shot at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (ShouldCancel()) return;
SetShooterSpeed(0.0);
@@ -677,7 +673,7 @@
if (!WaitForDriveNear(0.06, kDoNotTurnCare)) return;
LOG(INFO, "At Low Bar %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
OpenShooter();
constexpr double kSecondBallAfterBarDrive = 2.10;
@@ -695,7 +691,7 @@
false, 12.0);
LOG(INFO, "Done backing up %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
constexpr double kDriveBackDistance = 5.15 - 0.4;
StartDrive(-kDriveBackDistance, 0.0, kTwoBallLowDrive, kFinishTurn);
@@ -704,7 +700,7 @@
StartDrive(0.0, -kBallSmallWallTurn, kTwoBallLowDrive, kFinishTurn);
LOG(INFO, "Straightening up at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
CloseIfBall();
if (!WaitForDriveNear(kDriveBackDistance - 2.3, kDoNotTurnCare)) return;
@@ -715,7 +711,7 @@
if (!ball_detected) {
if (!WaitForDriveDone()) return;
LOG(INFO, "Aborting, no ball %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
return;
}
}
@@ -732,7 +728,7 @@
if (!WaitForDriveDone()) return;
LOG(INFO, "Second shot done driving at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForSuperstructure();
AlignWithVisionGoal();
if (ShouldCancel()) return;
@@ -743,7 +739,7 @@
// 2.2 with 0.4 of vision.
// 1.8 without any vision.
LOG(INFO, "Going to vision align at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForAlignedWithVision(
(start_time + chrono::milliseconds(13500) + kVisionExtra * 2) -
monotonic_clock::now());
@@ -751,21 +747,23 @@
WaitForSuperstructureProfile();
if (ShouldCancel()) return;
LOG(INFO, "Shoot at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
Shoot();
LOG(INFO, "Second shot at %f seconds\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (ShouldCancel()) return;
SetShooterSpeed(0.0);
LOG(INFO, "Folding superstructure back down\n");
TuckArm(true, false);
- LOG(INFO, "Shot %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Shot %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
WaitForSuperstructureLow();
- LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
+ LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
}
void AutonomousActor::StealAndMoveOverBy(double distance) {
@@ -932,7 +930,8 @@
StartDrive(0.5, 0.0, kMoatDrive, kFastTurn);
if (!WaitForDriveDone()) return true;
- 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/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 6ae7f0e..08ab1be 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -8,7 +8,6 @@
#include "y2016/control_loops/shooter/shooter_plant.h"
-
namespace y2016 {
namespace control_loops {
namespace shooter {
@@ -37,9 +36,7 @@
history_position_ = (history_position_ + 1) % kHistoryLength;
}
-double ShooterSide::voltage() const {
- return loop_->U(0, 0);
-}
+double ShooterSide::voltage() const { return loop_->U(0, 0); }
void ShooterSide::Update(bool disabled) {
loop_->mutable_R() = loop_->next_R();
@@ -60,8 +57,7 @@
// Compute the distance moved over that time period.
status->avg_angular_velocity =
(history_[oldest_history_position] - history_[history_position_]) /
- (chrono::duration_cast<chrono::duration<double>>(
- ::aos::controls::kLoopFrequency).count() *
+ (::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency) *
static_cast<double>(kHistoryLength - 1));
status->angular_velocity = loop_->X_hat(1, 0);
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index d0c4362..e7983f3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -88,11 +88,12 @@
pot_encoder_shoulder_(
constants::Values::kShoulderEncoderIndexDifference),
pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
- superstructure_queue_(".y2016.control_loops.superstructure_queue",
- ".y2016.control_loops.superstructure_queue.goal",
- ".y2016.control_loops.superstructure_queue.position",
- ".y2016.control_loops.superstructure_queue.output",
- ".y2016.control_loops.superstructure_queue.status") {
+ superstructure_queue_(
+ ".y2016.control_loops.superstructure_queue",
+ ".y2016.control_loops.superstructure_queue.goal",
+ ".y2016.control_loops.superstructure_queue.position",
+ ".y2016.control_loops.superstructure_queue.output",
+ ".y2016.control_loops.superstructure_queue.status") {
InitializeIntakePosition(0.0);
InitializeShoulderPosition(0.0);
InitializeRelativeWristPosition(0.0);
@@ -246,11 +247,12 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : superstructure_queue_(".y2016.control_loops.superstructure_queue",
- ".y2016.control_loops.superstructure_queue.goal",
- ".y2016.control_loops.superstructure_queue.position",
- ".y2016.control_loops.superstructure_queue.output",
- ".y2016.control_loops.superstructure_queue.status"),
+ : superstructure_queue_(
+ ".y2016.control_loops.superstructure_queue",
+ ".y2016.control_loops.superstructure_queue.goal",
+ ".y2016.control_loops.superstructure_queue.position",
+ ".y2016.control_loops.superstructure_queue.output",
+ ".y2016.control_loops.superstructure_queue.status"),
superstructure_(&event_loop_),
superstructure_plant_() {}
@@ -300,9 +302,8 @@
double begin_wrist_velocity =
superstructure_plant_.wrist_angular_velocity();
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 shoulder_acceleration =
(superstructure_plant_.shoulder_angular_velocity() -
begin_shoulder_velocity) /
@@ -921,8 +922,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
@@ -941,8 +942,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(0.5)
.max_angular_velocity_intake(1)
.max_angular_acceleration_intake(1)
@@ -967,7 +968,7 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
.angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
@@ -986,7 +987,7 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.5)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference)
.angle_wrist(0.0)
.max_angular_velocity_intake(4.5)
.max_angular_acceleration_intake(800)
@@ -1051,8 +1052,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(0.0)
.max_angular_velocity_intake(20)
.max_angular_acceleration_intake(20)
@@ -1071,8 +1072,8 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(0.0)
.angle_shoulder(
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
- 0.1)
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference +
+ 0.1)
.angle_wrist(1.3)
.max_angular_velocity_intake(1.0)
.max_angular_acceleration_intake(1.0)
@@ -1139,7 +1140,7 @@
superstructure_queue_.goal.MakeWithBuilder()
.angle_intake(constants::Values::kIntakeRange.upper) // stowed
.angle_shoulder(M_PI / 2.0) // in the collision area
- .angle_wrist(M_PI) // forward
+ .angle_wrist(M_PI) // forward
.Send());
RunForTime(chrono::seconds(5));
@@ -1341,4 +1342,4 @@
} // namespace testing
} // namespace superstructure
} // namespace control_loops
-} // namespace frc971
+} // namespace y2016
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 04e91f6..84af420 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -20,9 +20,9 @@
#include "frc971/autonomous/auto.q.h"
-#include "y2016/vision/vision.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
+#include "y2016/vision/vision.q.h"
namespace chrono = ::std::chrono;
@@ -128,7 +128,7 @@
AddPoint("big indicator", big_indicator);
AddPoint("superstructure state indicator", superstructure_state_indicator);
- if(auto_mode_indicator != 15) {
+ if (auto_mode_indicator != 15) {
AddPoint("auto mode indicator", auto_mode_indicator);
}
#endif
@@ -198,11 +198,10 @@
if (static_cast<size_t>(adjusted_index) <
sample_items_.at(0).datapoints.size()) {
message << cur_sample << "%"
- << chrono::duration_cast<chrono::duration<double>>(
+ << ::aos::time::DurationInSeconds(
sample_items_.at(0)
.datapoints.at(adjusted_index)
.time.time_since_epoch())
- .count()
<< "%"; // Send time.
// Add comma-separated list of data points.
for (size_t cur_measure = 0; cur_measure < sample_items_.size();
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 3409671..4b95034 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -10,11 +10,11 @@
#include <thread>
#include <vector>
+#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
-#include "aos/init.h"
#include "aos/vision/events/udp.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -179,12 +179,11 @@
double angle, double last_angle,
::aos::vision::Vector<2> *interpolated_result,
double *interpolated_angle) {
- const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
- older.capture_time() - newer.last_capture_time())
- .count() /
- chrono::duration_cast<chrono::duration<double>>(
- newer.capture_time() - newer.last_capture_time())
- .count();
+ const double age_ratio =
+ ::aos::time::DurationInSeconds(older.capture_time() -
+ newer.last_capture_time()) /
+ ::aos::time::DurationInSeconds(newer.capture_time() -
+ newer.last_capture_time());
interpolated_result->Set(
newer_center.x() * age_ratio + (1 - age_ratio) * last_newer_center.x(),
newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
@@ -226,12 +225,9 @@
status->drivetrain_left_position = before.left;
status->drivetrain_right_position = before.right;
} else {
- const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
- capture_time - before.time)
- .count() /
- chrono::duration_cast<chrono::duration<double>>(
- after.time - before.time)
- .count();
+ const double age_ratio =
+ ::aos::time::DurationInSeconds(capture_time - before.time) /
+ ::aos::time::DurationInSeconds(after.time - before.time);
status->drivetrain_left_position =
before.left * (1 - age_ratio) + after.left * age_ratio;
status->drivetrain_right_position =