Clean up the realtime priorities of all the robots etc
Change-Id: Ie19ea31ec7b19d9660230477e5f77de9edc5eb59
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index 0046069..b1af0bb 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -143,7 +143,7 @@
} // namespace y2012
int main() {
- ::aos::Init();
+ ::aos::Init(-1);
::y2012::input::joysticks::Reader reader;
reader.Run();
::aos::Cleanup();
diff --git a/y2012/wpilib/BUILD b/y2012/wpilib/BUILD
index f42e228..5c0c900 100644
--- a/y2012/wpilib/BUILD
+++ b/y2012/wpilib/BUILD
@@ -25,6 +25,7 @@
'//frc971/wpilib:gyro_sender',
'//frc971/wpilib:dma_edge_counting',
'//frc971/wpilib:interrupt_edge_counting',
+ '//frc971/wpilib:wpilib_robot_base',
'//frc971/wpilib:encoder_and_potentiometer',
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
diff --git a/y2012/wpilib/wpilib_interface.cc b/y2012/wpilib/wpilib_interface.cc
index 337f5d2..6ed3bb3 100644
--- a/y2012/wpilib/wpilib_interface.cc
+++ b/y2012/wpilib/wpilib_interface.cc
@@ -13,7 +13,7 @@
#include "AnalogInput.h"
#include "Compressor.h"
#include "Relay.h"
-#include "SampleRobot.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
#include "dma.h"
#ifndef WPILIB2015
#include "DigitalGlitchFilter.h"
@@ -103,10 +103,11 @@
#else
&DriverStation::GetInstance();
#endif
+
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
::aos::time::Time::InMS(4));
- ::aos::SetCurrentThreadRealtimePriority(kPriority);
+ ::aos::SetCurrentThreadRealtimePriority(40);
while (run_) {
{
const int iterations = phased_loop.SleepUntilNext();
@@ -141,9 +142,6 @@
void Quit() { run_ = false; }
private:
- static const int kPriority = 30;
- static const int kInterruptPriority = 55;
-
int32_t my_pid_;
DriverStation *ds_;
@@ -183,10 +181,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);
+ }
+ }
{
accessories_.FetchLatest();
@@ -297,14 +303,14 @@
::std::unique_ptr<Talon> talon1_, talon2_;
};
-class WPILibRobot : public SampleRobot {
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<Encoder> make_encoder(int index) {
return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
Encoder::k4X);
}
- virtual void RobotMain() {
+ void Run() override {
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
@@ -342,7 +348,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");
@@ -364,4 +377,4 @@
} // namespace y2012
-START_ROBOT_CLASS(::y2012::wpilib::WPILibRobot);
+AOS_ROBOT_CLASS(::y2012::wpilib::WPILibRobot);
diff --git a/y2014/BUILD b/y2014/BUILD
index 5a5011e..6561047 100644
--- a/y2014/BUILD
+++ b/y2014/BUILD
@@ -51,9 +51,9 @@
'//y2014/control_loops/claw:claw',
'//y2014/control_loops/shooter:shooter',
'//y2014/autonomous:auto',
- '//y2014/wpilib:wpilib_interface',
'//y2014/actors:binaries',
'//aos:prime_start_binaries',
+ '//y2014/wpilib:wpilib_interface',
],
srcs = [
'//aos:prime_binaries',
diff --git a/y2014/actors/drivetrain_actor_main.cc b/y2014/actors/drivetrain_actor_main.cc
index ef65d5b..4cc0070 100644
--- a/y2014/actors/drivetrain_actor_main.cc
+++ b/y2014/actors/drivetrain_actor_main.cc
@@ -7,7 +7,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
::y2014::actors::DrivetrainActor drivetrain(
&::y2014::actors::drivetrain_action);
diff --git a/y2014/actors/shoot_actor_main.cc b/y2014/actors/shoot_actor_main.cc
index fb27f93..1d33404 100644
--- a/y2014/actors/shoot_actor_main.cc
+++ b/y2014/actors/shoot_actor_main.cc
@@ -7,7 +7,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
::y2014::actors::ShootActor shoot(&::y2014::actors::shoot_action);
shoot.Run();
diff --git a/y2014/autonomous/auto_main.cc b/y2014/autonomous/auto_main.cc
index 253b237..bf3acf8 100644
--- a/y2014/autonomous/auto_main.cc
+++ b/y2014/autonomous/auto_main.cc
@@ -9,7 +9,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
LOG(INFO, "Auto main started\n");
::frc971::autonomous::autonomous.FetchLatest();
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index a0f2320..87dc3f1 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -522,7 +522,7 @@
} // namespace y2014
int main() {
- ::aos::Init();
+ ::aos::Init(-1);
::y2014::input::joysticks::Reader reader;
reader.Run();
::aos::Cleanup();
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index a399601..cc634d9 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -264,7 +264,7 @@
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
::aos::time::Time::InMS(4));
- ::aos::SetCurrentThreadRealtimePriority(kPriority);
+ ::aos::SetCurrentThreadRealtimePriority(40);
while (run_) {
{
const int iterations = phased_loop.SleepUntilNext();
@@ -410,7 +410,7 @@
multiplier * claw_translate(counter->last_negative_encoder_value());
}
- ::frc971::wpilib::InterruptSynchronizer synchronizer_{kInterruptPriority};
+ ::frc971::wpilib::InterruptSynchronizer synchronizer_{55};
::std::unique_ptr<::frc971::wpilib::EdgeCounter> front_counter_;
::std::unique_ptr<::frc971::wpilib::EdgeCounter> calibration_counter_;
@@ -426,9 +426,6 @@
const bool reversed_;
};
- static const int kPriority = 30;
- static const int kInterruptPriority = 55;
-
void CopyShooterPosedgeCounts(
const ::frc971::wpilib::DMAEdgeCounter *counter,
::frc971::PosedgeOnlyCountedHallEffectStruct *output) {
@@ -507,10 +504,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);
+ }
+ }
{
shooter_.FetchLatest();
@@ -771,7 +776,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");
diff --git a/y2014_bot3/autonomous/auto_main.cc b/y2014_bot3/autonomous/auto_main.cc
index 7966d97..d0b1952 100644
--- a/y2014_bot3/autonomous/auto_main.cc
+++ b/y2014_bot3/autonomous/auto_main.cc
@@ -9,7 +9,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
LOG(INFO, "Auto main started.\n");
::y2014_bot3::autonomous::autonomous.FetchLatest();
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index f1ee7a2..3e6e3e2 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -129,7 +129,7 @@
} // namespace y2014_bot3
int main() {
- ::aos::Init();
+ ::aos::Init(-1);
::y2014_bot3::input::joysticks::Reader reader;
reader.Run();
::aos::Cleanup();
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");
diff --git a/y2015/actors/can_pickup_actor_main.cc b/y2015/actors/can_pickup_actor_main.cc
index bed9412..1afb500 100644
--- a/y2015/actors/can_pickup_actor_main.cc
+++ b/y2015/actors/can_pickup_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/can_pickup_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::actors::CanPickupActor can_pickup(&::frc971::actors::can_pickup_action);
can_pickup.Run();
diff --git a/y2015/actors/drivetrain_actor_main.cc b/y2015/actors/drivetrain_actor_main.cc
index 7e461ff..6f567d7 100644
--- a/y2015/actors/drivetrain_actor_main.cc
+++ b/y2015/actors/drivetrain_actor_main.cc
@@ -7,7 +7,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
frc971::actors::DrivetrainActor drivetrain(
&::frc971::actors::drivetrain_action);
diff --git a/y2015/actors/held_to_lift_actor_main.cc b/y2015/actors/held_to_lift_actor_main.cc
index a5d78e1..705a073 100644
--- a/y2015/actors/held_to_lift_actor_main.cc
+++ b/y2015/actors/held_to_lift_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/held_to_lift_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::actors::HeldToLiftActor lift(
&::frc971::actors::held_to_lift_action);
diff --git a/y2015/actors/horizontal_can_pickup_actor_main.cc b/y2015/actors/horizontal_can_pickup_actor_main.cc
index 67f986c..fbe8c0d 100644
--- a/y2015/actors/horizontal_can_pickup_actor_main.cc
+++ b/y2015/actors/horizontal_can_pickup_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/horizontal_can_pickup_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::actors::HorizontalCanPickupActor horizontal_can_pickup(
&::frc971::actors::horizontal_can_pickup_action);
diff --git a/y2015/actors/lift_actor_main.cc b/y2015/actors/lift_actor_main.cc
index ed809ef..68389ca 100644
--- a/y2015/actors/lift_actor_main.cc
+++ b/y2015/actors/lift_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/lift_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::actors::LiftActor lift(&::frc971::actors::lift_action);
lift.Run();
diff --git a/y2015/actors/pickup_actor_main.cc b/y2015/actors/pickup_actor_main.cc
index 9c2c488..0ddda42 100644
--- a/y2015/actors/pickup_actor_main.cc
+++ b/y2015/actors/pickup_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/pickup_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
frc971::actors::PickupActor pickup(&::frc971::actors::pickup_action);
pickup.Run();
diff --git a/y2015/actors/score_actor_main.cc b/y2015/actors/score_actor_main.cc
index cc149e6..754d3bd 100644
--- a/y2015/actors/score_actor_main.cc
+++ b/y2015/actors/score_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/score_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
frc971::actors::ScoreActor score(&::frc971::actors::score_action);
score.Run();
diff --git a/y2015/actors/stack_actor_main.cc b/y2015/actors/stack_actor_main.cc
index f65733c..03a3055 100644
--- a/y2015/actors/stack_actor_main.cc
+++ b/y2015/actors/stack_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/stack_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::actors::StackActor stack(&::frc971::actors::stack_action);
stack.Run();
diff --git a/y2015/actors/stack_and_hold_actor_main.cc b/y2015/actors/stack_and_hold_actor_main.cc
index 3d5d1c1..b193dba 100644
--- a/y2015/actors/stack_and_hold_actor_main.cc
+++ b/y2015/actors/stack_and_hold_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/stack_and_hold_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::actors::StackAndHoldActor stack_and_hold(
&::frc971::actors::stack_and_hold_action);
diff --git a/y2015/actors/stack_and_lift_actor_main.cc b/y2015/actors/stack_and_lift_actor_main.cc
index fe9f4b5..2118a32 100644
--- a/y2015/actors/stack_and_lift_actor_main.cc
+++ b/y2015/actors/stack_and_lift_actor_main.cc
@@ -5,7 +5,7 @@
#include "y2015/actors/stack_and_lift_actor.h"
int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::actors::StackAndLiftActor stack_and_lift(
&::frc971::actors::stack_and_lift_action);
diff --git a/y2015/autonomous/auto_main.cc b/y2015/autonomous/auto_main.cc
index 4a24e6c..776067a 100644
--- a/y2015/autonomous/auto_main.cc
+++ b/y2015/autonomous/auto_main.cc
@@ -9,7 +9,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
LOG(INFO, "Auto main started\n");
::frc971::autonomous::autonomous.FetchLatest();
diff --git a/y2015/joystick_reader.cc b/y2015/joystick_reader.cc
index 3e43e35..f949c1a 100644
--- a/y2015/joystick_reader.cc
+++ b/y2015/joystick_reader.cc
@@ -550,7 +550,7 @@
} // namespace frc971
int main() {
- ::aos::Init();
+ ::aos::Init(-1);
::frc971::input::joysticks::Reader reader;
reader.Run();
::aos::Cleanup();
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");
diff --git a/y2015_bot3/actors/drivetrain_actor_main.cc b/y2015_bot3/actors/drivetrain_actor_main.cc
index 68b860b..a31b233 100644
--- a/y2015_bot3/actors/drivetrain_actor_main.cc
+++ b/y2015_bot3/actors/drivetrain_actor_main.cc
@@ -7,7 +7,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
frc971::actors::DrivetrainActor drivetrain(
&::frc971::actors::drivetrain_action);
diff --git a/y2015_bot3/autonomous/auto_main.cc b/y2015_bot3/autonomous/auto_main.cc
index 1556561..2c006e0 100644
--- a/y2015_bot3/autonomous/auto_main.cc
+++ b/y2015_bot3/autonomous/auto_main.cc
@@ -9,7 +9,7 @@
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
+ ::aos::Init(-1);
LOG(INFO, "Auto main started\n");
::y2015_bot3::autonomous::autonomous.FetchLatest();
diff --git a/y2015_bot3/joystick_reader.cc b/y2015_bot3/joystick_reader.cc
index 72082b1..be9bd7f 100644
--- a/y2015_bot3/joystick_reader.cc
+++ b/y2015_bot3/joystick_reader.cc
@@ -379,7 +379,7 @@
} // namespace y2015_bot3
int main() {
- ::aos::Init();
+ ::aos::Init(-1);
::y2015_bot3::input::joysticks::Reader reader;
reader.Run();
::aos::Cleanup();
diff --git a/y2015_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index 02230f0..6eee1ef 100644
--- a/y2015_bot3/wpilib/wpilib_interface.cc
+++ b/y2015_bot3/wpilib/wpilib_interface.cc
@@ -138,9 +138,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();
}
}
@@ -185,9 +193,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_;
@@ -237,10 +242,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);
+ }
+ }
// Can Grabber
{
@@ -514,7 +527,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");