Clean up the realtime priorities of all the robots etc
Change-Id: Ie19ea31ec7b19d9660230477e5f77de9edc5eb59
diff --git a/y2014_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
index 814bfe2..dc3999f 100644
--- a/y2014_bot3/wpilib/wpilib_interface.cc
+++ b/y2014_bot3/wpilib/wpilib_interface.cc
@@ -98,9 +98,17 @@
&DriverStation::GetInstance();
#endif
- ::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();
}
}
@@ -133,9 +141,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_;
@@ -180,10 +185,19 @@
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);
+ }
+ }
+
// Drivetrain
{
drivetrain_.FetchLatest();
@@ -390,7 +404,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");