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/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index ed91158..4b0fce1 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/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 "y2018/control_loops/drivetrain/drivetrain_base.h"
@@ -15,18 +15,13 @@
namespace actors {
using ::frc971::ProfileParameters;
-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 kFinalSwitchDrive = {0.5, 1.5};
const ProfileParameters kDrive = {5.0, 2.5};
const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
@@ -109,7 +104,8 @@
}
*/
- 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);
@@ -138,7 +134,8 @@
StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance)) return true;
- StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive, kSweepingTurn);
+ StartDrive(0.0, turn_scalar * (-M_PI / 2.0), kFarSwitchTurnDrive,
+ kSweepingTurn);
if (!WaitForTurnProfileDone()) return true;
// Now, close so let's move the arm up.
@@ -165,7 +162,7 @@
if (!WaitForArmTrajectoryClose(0.001)) return true;
LOG(INFO, "Arm close at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(1000));
@@ -236,7 +233,8 @@
return true;
StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
- if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance)) return true;
+ if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance))
+ return true;
StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
set_arm_goal_position(arm::BackHighBoxIndex());
SendSuperstructureGoal();
@@ -251,7 +249,7 @@
if (!WaitForDriveProfileNear(kFullDriveLength - (kSecondTurnDistance)))
return true;
LOG(INFO, "Final turn at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(0.0, M_PI / 2.0, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
if (!WaitForDriveProfileNear(0.15)) return true;
@@ -259,13 +257,13 @@
StartDrive(0.0, 0.3, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
LOG(INFO, "Dropping at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_open_claw(true);
SendSuperstructureGoal();
::std::this_thread::sleep_for(chrono::milliseconds(1000));
LOG(INFO, "Backing up at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(1.5, -0.55, kFarScaleFinalTurnDrive, kFarScaleSweepingTurn);
@@ -307,7 +305,7 @@
::std::this_thread::sleep_for(chrono::milliseconds(500));
LOG(INFO, "Dropping second box at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_open_claw(true);
SendSuperstructureGoal();
@@ -334,12 +332,13 @@
return true;
StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
- if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance)) return true;
+ if (!WaitForDriveProfileNear(kFullDriveLength - kFirstTurnDistance))
+ return true;
StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
set_arm_goal_position(arm::UpIndex());
SendSuperstructureGoal();
LOG(INFO, "Lifting arm at %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForTurnProfileDone()) return true;
StartDrive(0.0, 0.0, kDrive, kTurn);
@@ -372,7 +371,7 @@
if (!WaitForDriveNear(0.2, 0.2)) return true;
set_max_drivetrain_voltage(6.0);
LOG(INFO, "Lowered drivetrain voltage %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(300));
set_open_claw(true);
@@ -424,7 +423,7 @@
SendSuperstructureGoal();
set_intake_angle(-0.60);
LOG(INFO, "Dropped first box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(700));
@@ -432,7 +431,7 @@
SendSuperstructureGoal();
LOG(INFO, "Starting second box drive %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
constexpr double kSecondBoxSwerveAngle = 0.35;
constexpr double kSecondBoxDrive = 1.38;
StartDrive(kSecondBoxDrive, 0.0, kDrive, kFastTurn);
@@ -459,7 +458,7 @@
SendSuperstructureGoal();
LOG(INFO, "Grabbing second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(200));
StartDrive(-0.04, 0.0, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
@@ -467,17 +466,17 @@
set_max_drivetrain_voltage(12.0);
LOG(INFO, "Got second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(500));
set_grab_box(false);
- //set_arm_goal_position(arm::UpIndex());
+ // set_arm_goal_position(arm::UpIndex());
set_arm_goal_position(arm::BackHighBoxIndex());
set_roller_voltage(0.0);
set_disable_box_correct(false);
SendSuperstructureGoal();
LOG(INFO, "Driving to place second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(-kSecondBoxDrive + 0.16, kSecondBoxSwerveAngle, kDrive, kFastTurn);
if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
@@ -487,19 +486,19 @@
kFastTurn);
LOG(INFO, "Starting throw %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
if (!WaitForArmTrajectoryClose(0.25)) return true;
SendSuperstructureGoal();
// Throw the box.
- //if (!WaitForArmTrajectoryClose(0.20)) return true;
+ // if (!WaitForArmTrajectoryClose(0.20)) return true;
if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
set_open_claw(true);
set_intake_angle(-M_PI / 4.0);
LOG(INFO, "Releasing second box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
SendSuperstructureGoal();
::std::this_thread::sleep_for(chrono::milliseconds(700));
@@ -507,7 +506,7 @@
SendSuperstructureGoal();
LOG(INFO, "Driving to third box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
StartDrive(kThirdCubeDrive, kSecondBoxEndExtraAngle, kThirdBoxDrive,
kFastTurn);
if (!WaitForDriveNear(kThirdCubeDrive - 0.1, M_PI / 4.0)) return true;
@@ -528,15 +527,15 @@
if (!WaitForDriveProfileDone()) return true;
LOG(INFO, "Waiting for third box %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
if (!WaitForBoxGrabed()) return true;
LOG(INFO, "Third box grabbed %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
const bool too_late =
monotonic_clock::now() > start_time + chrono::milliseconds(12500);
if (too_late) {
LOG(INFO, "Third box too long, going up. %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
set_grab_box(false);
set_arm_goal_position(arm::UpIndex());
set_roller_voltage(0.0);
@@ -566,7 +565,7 @@
SendSuperstructureGoal();
LOG(INFO, "Final open %f\n",
- DoubleSeconds(monotonic_clock::now() - start_time));
+ ::aos::time::DurationInSeconds(monotonic_clock::now() - start_time));
}
if (!WaitForDriveProfileDone()) return true;
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index 552086a..c3d9772 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -47,9 +47,7 @@
loop_;
constexpr static double kDt =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- ::aos::controls::kLoopFrequency)
- .count();
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
// Sets the offset of the controller to be the zeroing estimator offset when
// possible otherwise zero.
@@ -93,9 +91,7 @@
State state() const { return state_; }
- bool clear_of_box() const {
- return controller_.output_position() < -0.1;
- }
+ bool clear_of_box() const { return controller_.output_position() < -0.1; }
double output_position() const { return controller_.output_position(); }