commit 5090c4348c80d30da198840410a1f186a26d005c
author Brian Silverman <brians> Sat Jan 02 14:44:26 2016 -0800
committer Brian Silverman <brians> Sat Jan 02 14:51:01 2016 -0800
tree 5ec970d79274bcc4a84918323949832696d29402
parent de625e7418d0a8245b7f1b172bff115d951b57f5

Clean up the realtime priorities of all the robots etc

Change-Id: Ie19ea31ec7b19d9660230477e5f77de9edc5eb59

y2015/wpilib/wpilib_interface.cc

diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index 9a6097e..e70c92f 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -242,9 +242,17 @@
     wrist_encoder_.Start();
     dma_synchronizer_->Start();
 
-    ::aos::SetCurrentThreadRealtimePriority(kPriority);
+    ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+                                        ::aos::time::Time::InMS(4));
+
+    ::aos::SetCurrentThreadRealtimePriority(40);
     while (run_) {
-      ::aos::time::PhasedLoopXMS(5, 4000);
+      {
+        const int iterations = phased_loop.SleepUntilNext();
+        if (iterations != 1) {
+          LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+        }
+      }
       RunIteration();
     }
 
@@ -304,9 +312,6 @@
   void Quit() { run_ = false; }
 
  private:
-  static const int kPriority = 30;
-  static const int kInterruptPriority = 55;
-
   int32_t my_pid_;
   DriverStation *ds_;
   ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
@@ -357,7 +362,7 @@
   DMAEncoderAndPotentiometer arm_left_encoder_, arm_right_encoder_,
       elevator_left_encoder_, elevator_right_encoder_;
 
-  InterruptEncoderAndPotentiometer wrist_encoder_{kInterruptPriority};
+  InterruptEncoderAndPotentiometer wrist_encoder_{55};
 
   ::std::unique_ptr<Encoder> left_encoder_;
   ::std::unique_ptr<Encoder> right_encoder_;
@@ -413,10 +418,18 @@
 
   void operator()() {
     ::aos::SetCurrentThreadName("Solenoids");
-    ::aos::SetCurrentThreadRealtimePriority(30);
+    ::aos::SetCurrentThreadRealtimePriority(27);
+
+    ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(20),
+                                        ::aos::time::Time::InMS(1));
 
     while (run_) {
-      ::aos::time::PhasedLoopXMS(20, 1000);
+      {
+        const int iterations = phased_loop.SleepUntilNext();
+        if (iterations != 1) {
+          LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
+        }
+      }
 
       {
         fridge_.FetchLatest();
@@ -727,7 +740,14 @@
     ::std::thread solenoid_thread(::std::ref(solenoid_writer));
 
     // Wait forever. Not much else to do...
-    PCHECK(select(0, nullptr, nullptr, nullptr, nullptr));
+    while (true) {
+      const int r = select(0, nullptr, nullptr, nullptr, nullptr);
+      if (r != 0) {
+        PLOG(WARNING, "infinite select failed");
+      } else {
+        PLOG(WARNING, "infinite select succeeded??\n");
+      }
+    }
 
     LOG(ERROR, "Exiting WPILibRobot\n");