Convert y2018 to use event loop time
Change-Id: Id695b94dc7877b51ac7898f333fe8152faa7600b
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index f3f58f8..d3cb262 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -44,7 +44,8 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
-void Arm::Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
+void Arm::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
@@ -261,7 +262,7 @@
if (claw_beambreak_triggered) {
grab_state_ = GrabState::CLAW_CLOSE;
// Snap time for the delay here.
- claw_close_start_time_ = ::aos::monotonic_clock::now();
+ claw_close_start_time_ = monotonic_now;
} else {
grab_state_ = GrabState::SHORT_BOX;
}
@@ -275,14 +276,14 @@
if (claw_beambreak_triggered) {
grab_state_ = GrabState::CLAW_CLOSE;
// Snap time for the delay here.
- claw_close_start_time_ = ::aos::monotonic_clock::now();
+ claw_close_start_time_ = monotonic_now;
} else {
grab_state_ = GrabState::WAIT_FOR_BOX;
}
}
break;
case GrabState::CLAW_CLOSE:
- if (::aos::monotonic_clock::now() >
+ if (monotonic_now >
claw_close_start_time_ + ::std::chrono::milliseconds(300)) {
grab_state_ = GrabState::OPEN_INTAKE;
}
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 0a4666a..8cd6b39 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -34,13 +34,13 @@
static constexpr double kPathlessVMax() { return 5.0; }
static constexpr double kGotoPathVMax() { return 6.0; }
- void Iterate(const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
+ void Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
bool close_claw, const control_loops::ArmPosition *position,
const bool claw_beambreak_triggered,
const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box,
- bool suicide, bool trajectory_override,
- double *proximal_output,
+ const bool intake_clear_of_box, bool suicide,
+ bool trajectory_override, double *proximal_output,
double *distal_output, bool *release_arm_brake,
bool *claw_closed, control_loops::ArmStatus *status);
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() ||