commit 333e588f88a45d2789a2146e51af46a048f6c08e
author Austin Schuh <austin@peloton-tech.com> Sun Apr 13 10:28:32 2014 -0700
committer Austin Schuh <austin@peloton-tech.com> Sun Apr 13 10:28:32 2014 -0700
tree dda3e0ce142880bd6e8c941005da7fecebf86a36
parent 1b6022f1b888ee5c67727752740d88a50028bb78

Actions don't start if there is a pending action on startup.

frc971/actions/action.h

diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index 9a4ddac..821ccaa 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -25,7 +25,25 @@
   void Run() {
     LOG(DEBUG, "Waiting for input to start\n");
     action_q_->goal.FetchLatest();
-    while (!action_q_->goal.get()) {
+    uint32_t initial_running_id = 0;
+    bool has_initial_running_id = false;
+    if (action_q_->goal.get()) {
+      has_initial_running_id = true;
+      initial_running_id = action_q_->goal->run;
+    }
+    while (true) {
+      if (action_q_->goal.get()) {
+        LOG(INFO, "Run is %d\n", action_q_->goal->run);
+        if (!action_q_->goal->run) {
+          break;
+        }
+        LOG(INFO, "Got a run of %d\n", action_q_->goal->run);
+        if (!has_initial_running_id) {
+          break;
+        } else if (initial_running_id != action_q_->goal->run) {
+          break;
+        }
+      }
       action_q_->goal.FetchNextBlocking();
     }
 
@@ -53,12 +71,12 @@
 
       // If we have a new one to run, we shouldn't say we're stopped in between.
       if (action_q_->goal->run != 0 && action_q_->goal->run != running_id) {
+        LOG(INFO, "skipping sending stopped status for %" PRIx32 "\n",
+            running_id);
+      } else {
         if (!action_q_->status.MakeWithBuilder().running(0).Send()) {
           LOG(ERROR, "Failed to send the status.\n");
         }
-      } else {
-        LOG(INFO, "skipping sending stopped status for %" PRIx32 "\n",
-            running_id);
       }
 
       while (action_q_->goal->run == running_id) {