commit 366f7eda404add2d80cb2d1316812d05a444ba9a
author Austin Schuh <austin.linux@gmail.com> Sat Mar 11 21:57:14 2017 -0800
committer Austin Schuh <austin.linux@gmail.com> Sun Mar 12 00:29:23 2017 -0800
tree 0f380a14a00a1752985d4456a707d8009cb1ed24
parent 35c75e895960280796233b4dcb5418f4db0b29ab

Added a simple working driving auto.

The auto drives, turns around, drives, and turns back.

Change-Id: I0dd5dc9f64c9808b9a50a69a1d4fd4cbddb0ee31

y2017/actors/autonomous_actor.cc

diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 9dd93d6..5fbb1e3 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -25,8 +25,8 @@
       .count();
 }
 
-const ProfileParameters kSlowDrive = {0.8, 2.5};
-const ProfileParameters kSlowTurn = {0.8, 3.0};
+const ProfileParameters kSlowDrive = {2.0, 2.0};
+const ProfileParameters kSlowTurn = {3.0, 3.0};
 
 }  // namespace
 
@@ -39,20 +39,43 @@
     const ::frc971::autonomous::AutonomousActionParams &params) {
   monotonic_clock::time_point start_time = monotonic_clock::now();
   LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
+  InitializeEncoders();
+  ResetDrivetrain();
 
   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;
+    default:
+      while (true) {
+        constexpr auto kDelayTime = chrono::milliseconds(1);
+        // Test case autonomous mode.
+        // Drives forward 1.0 meters and then turns 180 degrees.
+        StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
+        if (!WaitForDriveDone()) return true;
+
+        this_thread::sleep_for(kDelayTime);
+        if (ShouldCancel()) return true;
+
+        StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
+        if (!WaitForDriveDone()) return true;
+
+        this_thread::sleep_for(kDelayTime);
+        if (ShouldCancel()) return true;
+
+        StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
+        if (!WaitForDriveDone()) return true;
+
+        this_thread::sleep_for(kDelayTime);
+        if (ShouldCancel()) return true;
+
+        StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
+        if (!WaitForDriveDone()) return true;
+
+        this_thread::sleep_for(kDelayTime);
+        if (ShouldCancel()) return true;
+      }
+
       break;
 
-    default:
-      LOG(ERROR, "Invalid auto mode %d\n", params.mode);
-      return true;
   }
 
   LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));