clean up the organization of + split out wpilib_interface
Change-Id: I5278e42d13c4bb46619e52e20a72e8e878863ed0
diff --git a/aos/linux_code/init.cc b/aos/linux_code/init.cc
index d7501af..a4e72db 100644
--- a/aos/linux_code/init.cc
+++ b/aos/linux_code/init.cc
@@ -109,4 +109,12 @@
SetSoftRLimit(RLIMIT_CORE, RLIM_INFINITY, true);
}
+void SetCurrentThreadRealtimePriority(int priority) {
+ struct sched_param param;
+ param.sched_priority = priority;
+ if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
+ PLOG(FATAL, "sched_setscheduler(0, SCHED_FIFO, %d) failed", priority);
+ }
+}
+
} // namespace aos
diff --git a/aos/linux_code/init.h b/aos/linux_code/init.h
index 538684e..ab1aab5 100644
--- a/aos/linux_code/init.h
+++ b/aos/linux_code/init.h
@@ -25,6 +25,9 @@
// behavior without calling Init*.
void WriteCoreDumps();
+// Sets the current thread's realtime priority.
+void SetCurrentThreadRealtimePriority(int priority);
+
} // namespace aos
#endif // AOS_LINUX_CODE_INIT_H_
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 1b3792e..edae002 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -56,28 +56,5 @@
'<(AOS)/common/controls/controls.gyp:output_check',
],
},
- {
- 'target_name': 'wpilib_interface',
- 'type': 'executable',
- 'sources': [
- 'wpilib_interface.cc'
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/build/externals.gyp:WPILib_roboRIO',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/controls/controls.gyp:sensor_generation',
- '<(AOS)/common/util/util.gyp:log_interval',
- '../frc971.gyp:constants',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(AOS)/common/controls/controls.gyp:output_check',
- ],
- },
],
}
diff --git a/frc971/prime/prime.gyp b/frc971/prime/prime.gyp
index 5a9382a..8455ba9 100644
--- a/frc971/prime/prime.gyp
+++ b/frc971/prime/prime.gyp
@@ -64,7 +64,7 @@
'conditions': [
['FULL_COMPILER=="gcc_frc"', {
'dependencies': [
- '../output/output.gyp:wpilib_interface',
+ '../wpilib/wpilib.gyp:wpilib_interface',
],
}, {
'dependencies': [
diff --git a/frc971/wpilib/hall_effect.h b/frc971/wpilib/hall_effect.h
new file mode 100644
index 0000000..8a088c3
--- /dev/null
+++ b/frc971/wpilib/hall_effect.h
@@ -0,0 +1,18 @@
+#ifndef FRC971_WPILIB_HALL_EFFECT_H_
+#define FRC971_WPILIB_HALL_EFFECT_H_
+
+#include "DigitalInput.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class HallEffect : public DigitalInput {
+ public:
+ HallEffect(int index) : DigitalInput(index) {}
+ bool GetHall() { return !Get(); }
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_HALL_EFFECT_H_
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
new file mode 100644
index 0000000..d2a687b
--- /dev/null
+++ b/frc971/wpilib/joystick_sender.cc
@@ -0,0 +1,48 @@
+#include "frc971/wpilib/joystick_sender.h"
+
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/network/team_number.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "DriverStation.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void JoystickSender::operator()() {
+ DriverStation *ds = DriverStation::GetInstance();
+ ::aos::SetCurrentThreadRealtimePriority(29);
+ uint16_t team_id = ::aos::network::GetTeamNumber();
+
+ while (run_) {
+ ds->WaitForData();
+ auto new_state = ::aos::robot_state.MakeMessage();
+
+ new_state->test_mode = ds->IsAutonomous();
+ new_state->fms_attached = ds->IsFMSAttached();
+ new_state->enabled = ds->IsEnabled();
+ new_state->autonomous = ds->IsAutonomous();
+ new_state->team_id = team_id;
+ new_state->fake = false;
+
+ for (int i = 0; i < 4; ++i) {
+ new_state->joysticks[i].buttons = 0;
+ for (int button = 0; button < 16; ++button) {
+ new_state->joysticks[i].buttons |= ds->GetStickButton(i, button + 1)
+ << button;
+ }
+ for (int j = 0; j < 4; ++j) {
+ new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ }
+ }
+ LOG_STRUCT(DEBUG, "robot_state", *new_state);
+
+ if (!new_state.Send()) {
+ LOG(WARNING, "sending robot_state failed\n");
+ }
+ }
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
new file mode 100644
index 0000000..205f9c4
--- /dev/null
+++ b/frc971/wpilib/joystick_sender.h
@@ -0,0 +1,22 @@
+#ifndef FRC971_WPILIB_JOYSTICK_SENDER_H_
+#define FRC971_WPILIB_JOYSTICK_SENDER_H_
+
+#include <atomic>
+
+namespace frc971 {
+namespace wpilib {
+
+class JoystickSender {
+ public:
+ void operator()();
+
+ void Quit() { run_ = false; }
+
+ private:
+ ::std::atomic<bool> run_{true};
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_JOYSTICK_SENDER_H_
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
new file mode 100644
index 0000000..32a213a
--- /dev/null
+++ b/frc971/wpilib/wpilib.gyp
@@ -0,0 +1,57 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'wpilib_interface',
+ 'type': 'executable',
+ 'sources': [
+ 'wpilib_interface.cc'
+ ],
+ 'dependencies': [
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(EXTERNALS):WPILib_roboRIO',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(AOS)/common/controls/controls.gyp:control_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/controls/controls.gyp:sensor_generation',
+ '<(AOS)/common/util/util.gyp:log_interval',
+ '../frc971.gyp:constants',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(AOS)/common/controls/controls.gyp:output_check',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
+ 'hall_effect',
+ 'joystick_sender',
+ ],
+ },
+ {
+ 'target_name': 'joystick_sender',
+ 'type': 'static_library',
+ 'sources': [
+ 'joystick_sender.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/network/network.gyp:team_number',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ ],
+ },
+ {
+ 'target_name': 'hall_effect',
+ 'type': 'static_library',
+ 'sources': [
+ #'hall_effect.h',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ ],
+ },
+ ],
+}
diff --git a/frc971/output/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
similarity index 93%
rename from frc971/output/wpilib_interface.cc
rename to frc971/wpilib/wpilib_interface.cc
index eca629d..1738ea8 100644
--- a/frc971/output/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -7,15 +7,14 @@
#include "aos/prime/output/motor_output.h"
#include "aos/common/controls/output_check.q.h"
+#include "aos/common/messages/robot_state.q.h"
#include "aos/common/controls/sensor_generation.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-#include "aos/common/messages/robot_state.q.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
#include "aos/common/util/phased_loop.h"
#include "aos/common/util/wrapping_counter.h"
-#include "aos/common/network/team_number.h"
#include "aos/linux_code/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -25,8 +24,10 @@
#include "frc971/queues/other_sensors.q.h"
#include "frc971/queues/to_log.q.h"
+#include "frc971/wpilib/hall_effect.h"
+#include "frc971/wpilib/joystick_sender.h"
+
#include "Encoder.h"
-#include "DigitalInput.h"
#include "Talon.h"
#include "DriverStation.h"
#include "AnalogInput.h"
@@ -45,15 +46,7 @@
using ::aos::util::WrappingCounter;
namespace frc971 {
-namespace output {
-
-void SetThreadRealtimePriority(int priority) {
- struct sched_param param;
- param.sched_priority = priority;
- if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
- PLOG(FATAL, "sched_setscheduler failed");
- }
-}
+namespace wpilib {
class priority_mutex {
public:
@@ -64,7 +57,7 @@
priority_mutex() {
pthread_mutexattr_t attr;
#ifdef NDEBUG
-#error "Won't let perror be no-op ed"
+#error "Won't let assert_perror be no-op ed"
#endif
// Turn on priority inheritance.
assert_perror(pthread_mutexattr_init(&attr));
@@ -98,12 +91,7 @@
pthread_mutex_t handle_;
};
-class HallEffect : public DigitalInput {
- public:
- HallEffect(int index) : DigitalInput(index) {}
- bool GetHall() { return !Get(); }
-};
-
+// TODO(brian): Split this out into a separate file once DMA is in.
class EdgeCounter {
public:
EdgeCounter(int priority, Encoder *encoder, HallEffect *input,
@@ -121,7 +109,7 @@
// Updates the any_interrupt_count count when the interrupt comes in without
// the lock.
void operator()() {
- SetThreadRealtimePriority(priority_);
+ ::aos::SetCurrentThreadRealtimePriority(priority_);
input_->RequestInterrupts();
input_->SetUpSourceEdge(true, true);
@@ -217,11 +205,13 @@
::std::unique_ptr<::std::thread> thread_;
};
-// This class will synchronize sampling edges on a bunch of DigitalInputs with
+// This class will synchronize sampling edges on a bunch of HallEffects with
// the periodic poll.
//
// The data is provided to subclasses by calling SaveState when the state is
// consistent and ready.
+//
+// TODO(brian): Split this out into a separate file once DMA is in.
template <int num_sensors>
class PeriodicHallSynchronizer {
public:
@@ -299,7 +289,7 @@
}
void operator()() {
- SetThreadRealtimePriority(priority_);
+ ::aos::SetCurrentThreadRealtimePriority(priority_);
while (run_) {
::aos::time::PhasedLoopXMS(10, 9000);
RunIteration();
@@ -627,7 +617,7 @@
void operator()() {
const int kPriority = 30;
const int kInterruptPriority = 55;
- SetThreadRealtimePriority(kPriority);
+ ::aos::SetCurrentThreadRealtimePriority(kPriority);
::std::array<::std::unique_ptr<HallEffect>, 2> shooter_sensors;
shooter_sensors[0] = ::std::move(shooter_proximal_);
@@ -940,60 +930,17 @@
constexpr ::aos::time::Time MotorWriter::kOldLogInterval;
-class JoystickSender {
- public:
- JoystickSender() : run_(true) {}
-
- void operator()() {
- DriverStation *ds = DriverStation::GetInstance();
- SetThreadRealtimePriority(29);
- uint16_t team_id = ::aos::network::GetTeamNumber();
-
- while (run_) {
- ds->WaitForData();
- auto new_state = ::aos::robot_state.MakeMessage();
-
- new_state->test_mode = ds->IsAutonomous();
- new_state->fms_attached = ds->IsFMSAttached();
- new_state->enabled = ds->IsEnabled();
- new_state->autonomous = ds->IsAutonomous();
- new_state->team_id = team_id;
- new_state->fake = false;
-
- for (int i = 0; i < 4; ++i) {
- new_state->joysticks[i].buttons = 0;
- for (int button = 0; button < 16; ++button) {
- new_state->joysticks[i].buttons |=
- ds->GetStickButton(i, button + 1) << button;
- }
- for (int j = 0; j < 4; ++j) {
- new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
- }
- }
- LOG_STRUCT(DEBUG, "robot_state", *new_state);
-
- if (!new_state.Send()) {
- LOG(WARNING, "sending robot_state failed\n");
- }
- }
- }
-
- void Quit() { run_ = false; }
-
- private:
- ::std::atomic<bool> run_;
-};
-} // namespace output
+} // namespace wpilib
} // namespace frc971
class WPILibRobot : public RobotBase {
public:
virtual void StartCompetition() {
::aos::Init();
- ::frc971::output::MotorWriter writer;
- ::frc971::output::SensorReader reader;
+ ::frc971::wpilib::MotorWriter writer;
+ ::frc971::wpilib::SensorReader reader;
::std::thread reader_thread(::std::ref(reader));
- ::frc971::output::JoystickSender joystick_sender;
+ ::frc971::wpilib::JoystickSender joystick_sender;
::std::thread joystick_thread(::std::ref(joystick_sender));
writer.Run();
LOG(ERROR, "Exiting WPILibRobot\n");