Used loop delay variable instead of hardcoded value in autonomous
Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
Change-Id: I4dddfa30c904848dbe9be58c024f59ab44af4c14
Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
diff --git a/aos/actions/actor.h b/aos/actions/actor.h
index c4f2840..33e70f2 100644
--- a/aos/actions/actor.h
+++ b/aos/actions/actor.h
@@ -23,6 +23,10 @@
typedef typename std::remove_reference<decltype(
*static_cast<GoalType *>(nullptr)->params())>::type ParamType;
+ // Commonly used offset for autonomous phased loops
+ static constexpr monotonic_clock::duration kLoopOffset =
+ frc971::controls::kLoopFrequency / 2;
+
ActorBase(::aos::EventLoop *event_loop, const ::std::string &name)
: event_loop_(event_loop),
status_sender_(event_loop->MakeSender<Status>(name)),
@@ -189,7 +193,7 @@
::aos::monotonic_clock::time_point end_time) {
::aos::time::PhasedLoop phased_loop(::frc971::controls::kLoopFrequency,
event_loop_->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ kLoopOffset);
while (!done_condition()) {
if (ShouldCancel() || abort_) {
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 57aa578..eca86b9 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -123,9 +123,9 @@
return;
}
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
// Poll the running bit and see if we should cancel.
phased_loop.SleepUntilNext();
@@ -136,9 +136,9 @@
}
bool BaseAutonomousActor::WaitForDriveDone() {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
@@ -178,9 +178,9 @@
}
bool BaseAutonomousActor::WaitForAboveAngle(double angle) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
return false;
@@ -199,9 +199,9 @@
}
bool BaseAutonomousActor::WaitForBelowAngle(double angle) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
return false;
@@ -220,9 +220,9 @@
}
bool BaseAutonomousActor::WaitForMaxBy(double angle) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
double max_angle = -M_PI;
while (true) {
if (ShouldCancel()) {
@@ -245,9 +245,9 @@
}
bool BaseAutonomousActor::WaitForDriveNear(double distance, double angle) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
constexpr double kPositionTolerance = 0.02;
constexpr double kProfileTolerance = 0.001;
@@ -314,9 +314,9 @@
}
bool BaseAutonomousActor::WaitForDriveProfileNear(double tolerance) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
return false;
@@ -351,9 +351,9 @@
}
bool BaseAutonomousActor::WaitForTurnProfileNear(double tolerance) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
return false;
@@ -427,9 +427,9 @@
bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
double distance) {
::aos::time::PhasedLoop phased_loop(
- ::std::chrono::milliseconds(5),
+ frc971::controls::kLoopFrequency,
base_autonomous_actor_->event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (base_autonomous_actor_->ShouldCancel()) {
return false;
@@ -541,9 +541,9 @@
bool BaseAutonomousActor::SplineHandle::WaitForPlan() {
::aos::time::PhasedLoop phased_loop(
- ::std::chrono::milliseconds(5),
+ frc971::controls::kLoopFrequency,
base_autonomous_actor_->event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (base_autonomous_actor_->ShouldCancel()) {
return false;
@@ -591,9 +591,9 @@
bool BaseAutonomousActor::SplineHandle::WaitForDone() {
::aos::time::PhasedLoop phased_loop(
- ::std::chrono::milliseconds(5),
+ frc971::controls::kLoopFrequency,
base_autonomous_actor_->event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (base_autonomous_actor_->ShouldCancel()) {
return false;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index fef0d13..4b5138e 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -149,7 +149,7 @@
dt_config_.dt);
// TODO(milind): We should be able to get IMU readings at 1 kHz instead of 2.
event_loop_->AddPhasedLoop([this](int) { ReadImu(); },
- std::chrono::milliseconds(5));
+ frc971::controls::kLoopFrequency);
}
void DrivetrainSimulation::Reinitialize() {
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 8aed6b2..4dcfe60 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -102,7 +102,7 @@
Eigen::Matrix<double, 3, 1> measurement;
measurement.setZero();
for (int ii = 0; ii < 200; ++ii) {
- dtukf.Predict(ux * M_PI_2, measurement, std::chrono::milliseconds(5));
+ dtukf.Predict(ux * M_PI_2, measurement, frc971::controls::kLoopFrequency);
}
const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(M_PI_2, ux));
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
@@ -185,7 +185,8 @@
(Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
.norm());
for (int ii = 0; ii < 200; ++ii) {
- dtukf.Predict({0.0, 0.0, 0.0}, measurement, std::chrono::milliseconds(5));
+ dtukf.Predict({0.0, 0.0, 0.0}, measurement,
+ frc971::controls::kLoopFrequency);
}
const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(M_PI_2, ux));
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
@@ -207,7 +208,7 @@
measurement << 0.3, 1.0, 0.0;
for (int ii = 0; ii < 200; ++ii) {
dtukf.Predict({0.0, M_PI_2, 0.0}, measurement,
- std::chrono::milliseconds(5));
+ frc971::controls::kLoopFrequency);
}
const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(M_PI_2, uy));
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 1e-1))
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 23a3d21..0170d7c 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -293,7 +293,7 @@
X_hat_.setZero();
P_ = coefficients().P_steady_state;
UpdateQR(plant, coefficients().Q_continuous, coefficients().R_continuous,
- ::std::chrono::milliseconds(5));
+ frc971::controls::kLoopFrequency);
}
void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index c5b0771..c866457 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -127,7 +127,8 @@
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
Eigen::Matrix<double, 4, 1>::Constant(1),
- Eigen::Matrix<double, 4, 1>::Constant(-1), std::chrono::milliseconds(5));
+ Eigen::Matrix<double, 4, 1>::Constant(-1),
+ frc971::controls::kLoopFrequency);
// Build a plant.
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
@@ -166,7 +167,7 @@
// matches what was computed both in Python and in Matlab.
TEST(StateFeedbackLoopTest, PythonMatch) {
auto test_loop = MakeIntegralShooterLoop();
- test_loop.Update(false, ::std::chrono::milliseconds(5));
+ test_loop.Update(false, frc971::controls::kLoopFrequency);
Eigen::Matrix<double, 3, 3> A_discrete;
A_discrete << 1, 0.00490008, 0.00547272, 0, 0.96029888, 2.17440921, 0, 0, 1;
diff --git a/y2014_bot3/actors/autonomous_actor.cc b/y2014_bot3/actors/autonomous_actor.cc
index 2fc9d8b..8f281d4 100644
--- a/y2014_bot3/actors/autonomous_actor.cc
+++ b/y2014_bot3/actors/autonomous_actor.cc
@@ -48,9 +48,8 @@
AOS_LOG(INFO, "Done %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
- monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ monotonic_now(), ActorBase::kLoopOffset);
while (!ShouldCancel()) {
phased_loop.SleepUntilNext();
}
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index 6401b27..ffba99b 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -204,9 +204,9 @@
AOS_LOG(ERROR, "Sending shooter goal failed.\n");
}
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) return;
@@ -222,9 +222,9 @@
}
void AutonomousActor::WaitForShooterSpeed() {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) return;
@@ -251,9 +251,9 @@
double last_angle = 0.0;
int ready_to_fire = 0;
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
const monotonic_clock::time_point end_time = monotonic_now() + align_duration;
while (end_time > monotonic_now()) {
if (ShouldCancel()) break;
@@ -589,9 +589,9 @@
}
void AutonomousActor::WaitForBallOrDriveDone() {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
return;
@@ -974,9 +974,8 @@
AOS_LOG(INFO, "Done %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
- monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ monotonic_now(), ActorBase::kLoopOffset);
while (!ShouldCancel()) {
phased_loop.SleepUntilNext();
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index cf50331..1440aee 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -34,9 +34,9 @@
const vision_align_action::VisionAlignActionParams * /*params*/) {
const double robot_radius =
control_loops::drivetrain::GetDrivetrainConfig().robot_radius;
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
const int iterations = phased_loop.SleepUntilNext();
if (iterations != 1) {
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index b683f51..19aa329 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -309,9 +309,9 @@
AOS_LOG(INFO, "Done %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (!ShouldCancel()) {
phased_loop.SleepUntilNext();
diff --git a/y2017/actors/autonomous_actor.h b/y2017/actors/autonomous_actor.h
index 5eed336..ef99b72 100644
--- a/y2017/actors/autonomous_actor.h
+++ b/y2017/actors/autonomous_actor.h
@@ -74,9 +74,9 @@
}
void WaitForHoodZeroed() {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) return;
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index cee84d8..ea9daab 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -120,9 +120,9 @@
AOS_LOG(INFO, "Done %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (!ShouldCancel()) {
phased_loop.SleepUntilNext();
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index 67d9c84..3e6be9f 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -123,9 +123,9 @@
bool WaitForArmTrajectoryOrDriveClose(double drive_threshold,
double arm_threshold) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
constexpr double kPositionTolerance = 0.02;
constexpr double kProfileTolerance = 0.001;
@@ -185,9 +185,9 @@
}
bool WaitForArmTrajectoryClose(double threshold) {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
return false;
@@ -207,9 +207,9 @@
}
bool WaitForBoxGrabed() {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
return false;
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index ad5bd5b..b8024ec 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -35,9 +35,9 @@
bool AutonomousActor::WaitForDriveXGreater(double x) {
AOS_LOG(INFO, "Waiting until x > %f\n", x);
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
@@ -54,9 +54,9 @@
bool AutonomousActor::WaitForDriveYCloseToZero(double y) {
AOS_LOG(INFO, "Waiting until |y| < %f\n", y);
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index c0792b8..99b1364 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -169,9 +169,9 @@
}
bool WaitForGamePiece() {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
@@ -192,7 +192,7 @@
return false;
}
// TODO(james): Allow non-multiples of 5.
- ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
+ ::std::this_thread::sleep_for(frc971::controls::kLoopFrequency);
}
return true;
}
@@ -220,9 +220,9 @@
}
bool WaitForSuperstructureDone() {
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- ::std::chrono::milliseconds(5) / 2);
+ ActorBase::kLoopOffset);
while (true) {
if (ShouldCancel()) {
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index eb8ea42..37d1ed7 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -396,7 +396,7 @@
bool AutonomousActor::WaitUntilAbsoluteBallsShot(int absolute_balls) {
::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
event_loop()->monotonic_now(),
- frc971::controls::kLoopFrequency / 2);
+ ActorBase::kLoopOffset);
superstructure_status_fetcher_.Fetch();
CHECK(superstructure_status_fetcher_.get() != nullptr);
int last_balls = superstructure_status_fetcher_->shooter()->balls_shot();
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 18df813..cc04428 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -214,7 +214,7 @@
CHECK_EQ(builder.Send(status_builder.Finish()),
aos::RawSender::Error::kOk);
},
- chrono::milliseconds(5));
+ frc971::controls::kLoopFrequency);
test_event_loop_->OnRun([this]() { SetStartingPosition({3.0, 2.0, 0.0}); });