commit 624088a369e23a735edfd43e634f30e777d4e851
author Austin Schuh <austin.linux@gmail.com> Wed Mar 22 22:36:16 2017 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 25 15:17:09 2017 -0700
tree 216e083a8fcdf013546238aa91be9d634d1376b3
parent 6a42174efc8b1776297ca893115f25bf7ee79b2d

Added 40 kpa auto

Change-Id: I1985625515b02bac1d909900c9593b9c88b3d333

y2017/actors/autonomous_actor.cc

diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 5fbb1e3..701f648 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -25,8 +25,9 @@
       .count();
 }
 
-const ProfileParameters kSlowDrive = {2.0, 2.0};
+const ProfileParameters kSlowDrive = {3.0, 2.0};
 const ProfileParameters kSlowTurn = {3.0, 3.0};
+const ProfileParameters kSmashTurn = {3.0, 5.0};
 
 }  // namespace
 
@@ -39,43 +40,63 @@
     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();
+  Reset();
 
   switch (params.mode) {
     case 0:
-    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;
+    default: {
+      constexpr double kDriveDirection = 1.0;
+      // Test case autonomous mode.
+      // Drives forward 1.0 meters and then turns 180 degrees.
+      set_intake_goal(0.07);
+      SendSuperstructureGoal();
+      StartDrive(-3.7, 0.0, kSlowDrive, kSlowTurn);
+      if (!WaitForDriveNear(2.75, 0.0)) return true;
 
-        this_thread::sleep_for(kDelayTime);
-        if (ShouldCancel()) return true;
+      set_intake_goal(0.23);
+      SendSuperstructureGoal();
+      StartDrive(0.0, -M_PI / 4.0 * kDriveDirection, kSlowDrive, kSlowTurn);
+      if (!WaitForDriveNear(0.05, 0.0)) return true;
 
-        StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
-        if (!WaitForDriveDone()) return true;
+      this_thread::sleep_for(chrono::milliseconds(100));
 
-        this_thread::sleep_for(kDelayTime);
-        if (ShouldCancel()) return true;
+      StartDrive(0.0, (M_PI / 4.0 + 0.20) * kDriveDirection, kSlowDrive,
+                 kSmashTurn);
+      if (!WaitForDriveNear(0.05, 0.2)) return true;
 
-        StartDrive(1.0, 0.0, kSlowDrive, kSlowTurn);
-        if (!WaitForDriveDone()) return true;
+      set_vision_track(true);
+      set_turret_goal(0.0);
+      set_hood_goal(0.63);
+      set_shooter_velocity(371.0);
+      SendSuperstructureGoal();
 
-        this_thread::sleep_for(kDelayTime);
-        if (ShouldCancel()) return true;
+      this_thread::sleep_for(chrono::milliseconds(200));
 
-        StartDrive(0.0, M_PI, kSlowDrive, kSlowTurn);
-        if (!WaitForDriveDone()) return true;
+      StartDrive(0.0, (-0.15) * kDriveDirection, kSlowDrive, kSlowTurn);
+      if (!WaitForDriveNear(0.05, 0.02)) return true;
+      set_indexer_angular_velocity(-1.8 * M_PI);
+      SendSuperstructureGoal();
+      LOG(INFO, "Started shooting at %f\n",
+          DoubleSeconds(monotonic_clock::now() - start_time));
 
-        this_thread::sleep_for(kDelayTime);
-        if (ShouldCancel()) return true;
-      }
+      this_thread::sleep_for(start_time + chrono::seconds(9) -
+                             monotonic_clock::now());
+      StartDrive(0.0, (-0.05) * kDriveDirection, kSlowDrive, kSlowTurn);
+      if (ShouldCancel()) return true;
 
-      break;
+      set_intake_max_velocity(0.05);
+      set_intake_goal(0.08);
+      SendSuperstructureGoal();
 
+      this_thread::sleep_for(start_time + chrono::seconds(15) -
+                             monotonic_clock::now());
+      if (ShouldCancel()) return true;
+
+      set_shooter_velocity(0.0);
+      set_indexer_angular_velocity(0.0);
+      SendSuperstructureGoal();
+
+    } break;
   }
 
   LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));