Convert y2018 to use event loop time
Change-Id: Id695b94dc7877b51ac7898f333fe8152faa7600b
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 24a8880..572a7b0 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -45,6 +45,8 @@
const control_loops::SuperstructureQueue::Position *position,
control_loops::SuperstructureQueue::Output *output,
control_loops::SuperstructureQueue::Status *status) {
+ const monotonic_clock::time_point monotonic_now =
+ event_loop()->monotonic_now();
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
intake_left_.Reset();
@@ -105,6 +107,7 @@
}
}
arm_.Iterate(
+ monotonic_now,
unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
unsafe_goal != nullptr ? unsafe_goal->grab_box : false, open_claw,
unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
@@ -148,7 +151,6 @@
rotation_count_ = 0;
stuck_count_ = 0;
} else {
- monotonic_clock::time_point monotonic_now = monotonic_clock::now();
const bool stuck = position->box_distance < 0.20 &&
filtered_box_velocity_ > -0.05 &&
!position->box_back_beambreak_triggered;
@@ -263,7 +265,6 @@
drivetrain_output_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
- monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
if (status->estopped) {
SendColors(0.5, 0.0, 0.0);
} else if (!vision_status_fetcher_.get() ||