Added ahal
This is a formatted copy of WPILib's default user-visible C++ API, with
a bit of completely unnecessary functionality stripped out. Most of the
stripping so far is only related to weird threading decisions.
Change-Id: Icbfd949b48cd115561862cb909bcc572aba0e753
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 3984d70..e005475 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -5,63 +5,54 @@
#include "aos/network/team_number.h"
#include "aos/logging/queue_logging.h"
-#include "DriverStation.h"
-#if defined(WPILIB2017) || defined(WPILIB2018)
#include "HAL/HAL.h"
-#else
-#include "HAL/HAL.hpp"
-#endif
+#include "frc971/wpilib/ahal/DriverStation.h"
namespace frc971 {
namespace wpilib {
void JoystickSender::operator()() {
- DriverStation *ds =
-#ifdef WPILIB2015
- DriverStation::GetInstance();
-#else
- &DriverStation::GetInstance();
-#endif
+ frc::DriverStation *const ds = &frc::DriverStation::GetInstance();
::aos::SetCurrentThreadName("DSReader");
uint16_t team_id = ::aos::network::GetTeamNumber();
::aos::SetCurrentThreadRealtimePriority(29);
+ // TODO(Brian): Fix the potential deadlock when stopping here (condition
+ // variable / mutex needs to get exposed all the way out or something).
while (run_) {
- ds->WaitForData();
- auto new_state = ::aos::joystick_state.MakeMessage();
+ ds->RunIteration([&]() {
+ auto new_state = ::aos::joystick_state.MakeMessage();
- HAL_ControlWord control_word;
- HAL_GetControlWord(&control_word);
- HAL_MatchInfo match_info;
- auto status = HAL_GetMatchInfo(&match_info);
- if (status == 0) {
- new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
- match_info.gameSpecificMessage[0] == 'l';
- new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
- match_info.gameSpecificMessage[1] == 'l';
- }
- HAL_FreeMatchInfo(&match_info);
-
- new_state->test_mode = control_word.test;
- new_state->fms_attached = control_word.fmsAttached;
- new_state->enabled = control_word.enabled;
- new_state->autonomous = control_word.autonomous;
- new_state->team_id = team_id;
- new_state->fake = false;
-
- for (int i = 0; i < 4; ++i) {
- new_state->joysticks[i].buttons = ds->GetStickButtons(i);
- for (int j = 0; j < 6; ++j) {
- new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ HAL_MatchInfo match_info;
+ auto status = HAL_GetMatchInfo(&match_info);
+ if (status == 0) {
+ new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
+ match_info.gameSpecificMessage[0] == 'l';
+ new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
+ match_info.gameSpecificMessage[1] == 'l';
}
- new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
- }
- LOG_STRUCT(DEBUG, "joystick_state", *new_state);
+ HAL_FreeMatchInfo(&match_info);
- if (!new_state.Send()) {
- LOG(WARNING, "sending joystick_state failed\n");
- }
+ new_state->test_mode = ds->IsTestMode();
+ 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 = ds->GetStickButtons(i);
+ for (int j = 0; j < 6; ++j) {
+ new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ }
+ LOG_STRUCT(DEBUG, "joystick_state", *new_state);
+
+ }
+ if (!new_state.Send()) {
+ LOG(WARNING, "sending joystick_state failed\n");
+ }
+ });
}
}