Convert y2017 to use event loop time
Change-Id: I101d113542b81fc7034640693435157ed6f637a0
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index b4762a7..2a3cde7 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -384,7 +384,8 @@
freeze_ = false;
}
-void Column::Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
+void Column::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const control_loops::IndexerGoal *unsafe_indexer_goal,
const control_loops::TurretGoal *unsafe_turret_goal,
const ColumnPosition *position,
const vision::VisionStatus *vision_status,
@@ -395,8 +396,8 @@
bool disable = turret_output == nullptr || indexer_output == nullptr;
profiled_subsystem_.Correct(*position);
- vision_time_adjuster_.Tick(::aos::monotonic_clock::now(),
- profiled_subsystem_.X_hat(2, 0), vision_status);
+ vision_time_adjuster_.Tick(monotonic_now, profiled_subsystem_.X_hat(2, 0),
+ vision_status);
switch (state_) {
case State::UNINITIALIZED:
@@ -560,8 +561,6 @@
// If a "stuck" event is detected, reverse. Stay reversed until either
// unstuck, or 0.5 seconds have elapsed.
// Then, start going forwards. Don't detect stuck for 0.5 seconds.
-
- monotonic_clock::time_point monotonic_now = monotonic_clock::now();
switch (indexer_state_) {
case IndexerState::RUNNING:
// The velocity goal is already set above in this case, so leave it
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 0caa66e..0382059 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -179,7 +179,8 @@
static constexpr double kTurretMin = -0.1;
static constexpr double kTurretMax = M_PI / 2.0 + 0.1;
- void Iterate(const control_loops::IndexerGoal *unsafe_indexer_goal,
+ void Iterate(::aos::monotonic_clock::time_point monotonic_now,
+ const control_loops::IndexerGoal *unsafe_indexer_goal,
const control_loops::TurretGoal *unsafe_turret_goal,
const ColumnPosition *position,
const vision::VisionStatus *vision_status,
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
index f19bbf7..862a32c 100644
--- a/y2017/control_loops/superstructure/hood/hood.cc
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -61,7 +61,8 @@
last_position_ = 0;
}
-void Hood::Iterate(const control_loops::HoodGoal *unsafe_goal,
+void Hood::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const control_loops::HoodGoal *unsafe_goal,
const ::frc971::IndexPosition *position, double *output,
::frc971::control_loops::IndexProfiledJointStatus *status) {
bool disable = output == nullptr;
@@ -177,11 +178,11 @@
if (::std::abs(profiled_subsystem_.position() - last_position_) >
kErrorOnPositionTillNotMoving) {
// Currently moving. Update time of last move.
- last_move_time_ = ::aos::monotonic_clock::now();
+ last_move_time_ = monotonic_now;
// Save last position.
last_position_ = profiled_subsystem_.position();
}
- if (::aos::monotonic_clock::now() > kTimeTillNotMoving + last_move_time_) {
+ if (monotonic_now > kTimeTillNotMoving + last_move_time_) {
error_voltage = kNotMovingVoltage;
}
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index cf45850..55d5344 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -45,7 +45,8 @@
::std::chrono::milliseconds(15);
static constexpr double kNotMovingVoltage = 2.0;
- void Iterate(const control_loops::HoodGoal *unsafe_goal,
+ void Iterate(::aos::monotonic_clock::time_point monotonic_now,
+ const control_loops::HoodGoal *unsafe_goal,
const ::frc971::IndexPosition *position, double *output,
::frc971::control_loops::IndexProfiledJointStatus *status);
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index c76dd19..e7f7ca8 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -55,6 +55,8 @@
const control_loops::SuperstructureQueue::Position *position,
control_loops::SuperstructureQueue::Output *output,
control_loops::SuperstructureQueue::Status *status) {
+ const ::aos::monotonic_clock::time_point monotonic_now =
+ event_loop()->monotonic_now();
if (WasReset()) {
LOG(ERROR, "WPILib reset, restarting\n");
hood_.Reset();
@@ -82,7 +84,7 @@
distance_average_.Reset();
}
- distance_average_.Tick(::aos::monotonic_clock::now(), vision_status);
+ distance_average_.Tick(monotonic_now, vision_status);
status->vision_distance = distance_average_.Get();
// If we are moving too fast, disable shooting and clear the accumulator.
@@ -124,9 +126,10 @@
}
}
- hood_.Iterate(
- unsafe_goal != nullptr ? &hood_goal : nullptr, &(position->hood),
- output != nullptr ? &(output->voltage_hood) : nullptr, &(status->hood));
+ hood_.Iterate(monotonic_now, unsafe_goal != nullptr ? &hood_goal : nullptr,
+ &(position->hood),
+ output != nullptr ? &(output->voltage_hood) : nullptr,
+ &(status->hood));
shooter_.Iterate(unsafe_goal != nullptr ? &shooter_goal : nullptr,
&(position->theta_shooter), position->sent_time,
output != nullptr ? &(output->voltage_shooter) : nullptr,
@@ -179,7 +182,8 @@
output != nullptr ? &(output->voltage_intake) : nullptr,
&(status->intake));
- column_.Iterate(unsafe_goal != nullptr ? &indexer_goal : nullptr,
+ column_.Iterate(monotonic_now,
+ unsafe_goal != nullptr ? &indexer_goal : nullptr,
unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
&(position->column), vision_status,
output != nullptr ? &(output->voltage_indexer) : nullptr,