Add a test autonomous as mode 0
This is just a sanity check to verify that the infrastructure is
actually working. I broke the drivebase out of the 2016 code so
hopefully this will just work.
What I added attempts to move the robot forward by 1 meter and then
rotate 180 degrees.
Change-Id: I04c061008c5e9827c603aee78b65e1a5d631f61e
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 52f3df2..9dd93d6 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -19,10 +19,15 @@
namespace this_thread = ::std::this_thread;
namespace {
+
double DoubleSeconds(monotonic_clock::duration duration) {
return ::std::chrono::duration_cast<::std::chrono::duration<double>>(duration)
.count();
}
+
+const ProfileParameters kSlowDrive = {0.8, 2.5};
+const ProfileParameters kSlowTurn = {0.8, 3.0};
+
} // namespace
AutonomousActor::AutonomousActor(
@@ -37,6 +42,12 @@
switch (params.mode) {
case 0:
+ // Test case autonomous mode.
+ // Drives forward 1.0 meters and then turns 180 degrees.
+ StartDrive(1.1, 0.0, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveNear(1.0, 0.0)) return true;
+ StartDrive(0.0, M_PI / 2, kSlowDrive, kSlowTurn);
+ if (!WaitForDriveDone()) return true;
break;
default: