Add 3 autonomous modes.
Change-Id: Ied6692009ebd5373a422881180107c1b2d2b7116
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index b8d58e2..cf5766c 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -77,6 +77,25 @@
LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
+
+ bool WaitForArmTrajectoryClose(double threshold) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ superstructure_queue.status.FetchLatest();
+ if (superstructure_queue.status.get()) {
+ if (superstructure_queue.status->arm.current_node == arm_goal_position_ &&
+ superstructure_queue.status->arm.path_distance_to_go < threshold) {
+ return true;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
};
} // namespace actors