commit 6f62154083df3d59bc0d1bc1984ba9c4ab36a5a4
author Brian Silverman <brians> Sun Apr 06 16:00:41 2014 -0700
committer Brian Silverman <brians> Sun Apr 06 16:00:41 2014 -0700
tree ef2f5a495429f23c2c56b1ffbe66de791b68ca7e
parent 8d5dda419b9607bedabe5546a63f0703d91af7c8

auto code that waits for the hot goal with 254's DS code

frc971/autonomous/auto.cc

diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 107b380..85bd355 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -7,6 +7,7 @@
 #include "aos/common/util/trapezoid_profile.h"
 #include "aos/common/logging/logging.h"
 #include "aos/common/network/team_number.h"
+#include "aos/common/logging/queue_logging.h"
 
 #include "frc971/autonomous/auto.q.h"
 #include "frc971/constants.h"
@@ -16,6 +17,7 @@
 #include "frc971/actions/action_client.h"
 #include "frc971/actions/shoot_action.h"
 #include "frc971/actions/drivetrain_action.h"
+#include "frc971/queues/hot_goal.q.h"
 
 using ::aos::time::Time;
 
@@ -217,9 +219,20 @@
 }
 
 void HandleAuto() {
+  const ::aos::time::Time start_time = ::aos::time::Time::Now();
+  ::frc971::HotGoal start_counts;
+  hot_goal.FetchLatest();
+  bool hot_goal_wait = true;
+  if (!hot_goal.get()) {
+    LOG(WARNING, "no hot goal message. not waiting\n");
+    hot_goal_wait = false;
+  } else {
+    memcpy(&start_counts, hot_goal.get(), sizeof(start_counts));
+    LOG_STRUCT(INFO, "counts at start", start_counts);
+  }
+
   // The front of the robot is 1.854 meters from the wall
   const double kShootDistance = 3.15;
-  const double kPickupDistance = 0.5;
   LOG(INFO, "Handling auto mode\n");
   ResetDrivetrain();
 
@@ -246,42 +259,23 @@
     WaitUntilDoneOrCanceled(drivetrain_action.get());
     if (ShouldExitAuto()) return;
   }
-  LOG(INFO, "Shooting\n");
+  LOG(INFO, "Shooting (after we have a hot goal)\n");
+
+  time::SleepFor(time::Time::InSeconds(0.1));
+
+  while (hot_goal_wait && (::aos::time::Time::Now() - start_time) <
+                              ::aos::time::Time::InSeconds(7)) {
+    hot_goal.FetchNextBlocking();
+    if ((hot_goal->right_count - start_counts.right_count) > 10) {
+      LOG(INFO, "hot goal time!!!\n");
+      hot_goal_wait = false;
+    }
+  }
+  LOG(INFO, "done waiting for hot goal\n");
 
   // Shoot.
   Shoot();
-  time::SleepFor(time::Time::InSeconds(0.1));
 
-  {
-    if (ShouldExitAuto()) return;
-    // Intake the new ball.
-    LOG(INFO, "Claw ready for intake\n");
-    PositionClawBackIntake();
-    auto drivetrain_action = SetDriveGoal(kShootDistance + kPickupDistance);
-    LOG(INFO, "Waiting until drivetrain is finished\n");
-    WaitUntilDoneOrCanceled(drivetrain_action.get());
-    if (ShouldExitAuto()) return;
-    LOG(INFO, "Wait for the claw.\n");
-    WaitUntilClawDone();
-    if (ShouldExitAuto()) return;
-  }
-
-  // Drive back.
-  {
-    LOG(INFO, "Driving back\n");
-    auto drivetrain_action = SetDriveGoal(-(kShootDistance + kPickupDistance));
-    time::SleepFor(time::Time::InSeconds(0.7));
-    if (ShouldExitAuto()) return;
-    PositionClawForShot();
-    LOG(INFO, "Waiting until drivetrain is finished\n");
-    WaitUntilDoneOrCanceled(drivetrain_action.get());
-    WaitUntilClawDone();
-    if (ShouldExitAuto()) return;
-  }
-
-  LOG(INFO, "Shooting\n");
-  // Shoot
-  Shoot();
   if (ShouldExitAuto()) return;
 
   // Get ready to zero when we come back up.