diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index 190634ce99..88329ca3de 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -13,6 +13,10 @@ #include #include #include +#include +#include +#include +#include #include "AnalogInput.h" #include "MotorSafetyHelper.h" @@ -28,6 +32,42 @@ struct MatchInfoData { int replayNumber = 0; DriverStation::MatchType matchType = DriverStation::MatchType::kNone; }; + +class MatchDataSender { + public: + std::shared_ptr table; + nt::NetworkTableEntry typeMetadata; + nt::NetworkTableEntry gameSpecificMessage; + nt::NetworkTableEntry eventName; + nt::NetworkTableEntry matchNumber; + nt::NetworkTableEntry replayNumber; + nt::NetworkTableEntry matchType; + nt::NetworkTableEntry alliance; + nt::NetworkTableEntry station; + nt::NetworkTableEntry controlWord; + + MatchDataSender() { + table = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo"); + typeMetadata = table->GetEntry(".type"); + typeMetadata.ForceSetString("FMSInfo"); + gameSpecificMessage = table->GetEntry("GameSpecificMessage"); + gameSpecificMessage.ForceSetString(""); + eventName = table->GetEntry("EventName"); + eventName.ForceSetString(""); + matchNumber = table->GetEntry("MatchNumber"); + matchNumber.ForceSetDouble(0); + replayNumber = table->GetEntry("ReplayNumber"); + replayNumber.ForceSetDouble(0); + matchType = table->GetEntry("MatchType"); + matchType.ForceSetDouble(0); + alliance = table->GetEntry("IsRedAlliance"); + alliance.ForceSetBoolean(true); + station = table->GetEntry("StationNumber"); + station.ForceSetDouble(1); + controlWord = table->GetEntry("FMSControlData"); + controlWord.ForceSetDouble(0); + } +}; } // namespace frc using namespace frc; @@ -640,6 +680,65 @@ double DriverStation::GetBatteryVoltage() const { return voltage; } +void DriverStation::SendMatchData() { + int32_t status = 0; + HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status); + bool isRedAlliance = false; + int stationNumber = 1; + switch (alliance) { + case HAL_AllianceStationID::HAL_AllianceStationID_kBlue1: + isRedAlliance = false; + stationNumber = 1; + break; + case HAL_AllianceStationID::HAL_AllianceStationID_kBlue2: + isRedAlliance = false; + stationNumber = 2; + break; + case HAL_AllianceStationID::HAL_AllianceStationID_kBlue3: + isRedAlliance = false; + stationNumber = 3; + break; + case HAL_AllianceStationID::HAL_AllianceStationID_kRed1: + isRedAlliance = true; + stationNumber = 1; + break; + case HAL_AllianceStationID::HAL_AllianceStationID_kRed2: + isRedAlliance = true; + stationNumber = 2; + break; + default: + isRedAlliance = true; + stationNumber = 3; + break; + } + + MatchInfoData tmpDataStore; + { + std::lock_guard lock(m_cacheDataMutex); + tmpDataStore = *m_matchInfo; + } + + m_matchDataSender->alliance.SetBoolean(isRedAlliance); + m_matchDataSender->station.SetDouble(stationNumber); + m_matchDataSender->eventName.SetString(tmpDataStore.eventName); + m_matchDataSender->gameSpecificMessage.SetString( + tmpDataStore.gameSpecificMessage); + m_matchDataSender->matchNumber.SetDouble(tmpDataStore.matchNumber); + m_matchDataSender->replayNumber.SetDouble(tmpDataStore.replayNumber); + m_matchDataSender->matchType.SetDouble( + static_cast(tmpDataStore.matchType)); + + HAL_ControlWord ctlWord; + { + // Valid, as in other places we guarentee ctlWord >= int32 + std::lock_guard lock(m_controlWordMutex); + ctlWord = m_controlWordCache; + } + int32_t wordInt = 0; + std::memcpy(&wordInt, &ctlWord, sizeof(wordInt)); + m_matchDataSender->controlWord.SetDouble(wordInt); +} + /** * Copy data from the DS task for the user. * @@ -670,30 +769,37 @@ void DriverStation::GetData() { // Force a control word update, to make sure the data is the newest. HAL_ControlWord controlWord; UpdateControlWord(true, controlWord); - // Obtain a write lock on the data, swap the cached data into the - // main data arrays - std::lock_guard lock(m_cacheDataMutex); - for (int32_t i = 0; i < kJoystickPorts; i++) { - // If buttons weren't pressed and are now, set flags in m_buttonsPressed - m_joystickButtonsPressed[i] |= - ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons; + { + // Obtain a write lock on the data, swap the cached data into the + // main data arrays + std::lock_guard lock(m_cacheDataMutex); - // If buttons were pressed and aren't now, set flags in m_buttonsReleased - m_joystickButtonsReleased[i] |= - m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons; + for (int32_t i = 0; i < kJoystickPorts; i++) { + // If buttons weren't pressed and are now, set flags in m_buttonsPressed + m_joystickButtonsPressed[i] |= + ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons; + + // If buttons were pressed and aren't now, set flags in m_buttonsReleased + m_joystickButtonsReleased[i] |= + m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons; + } + + m_joystickAxes.swap(m_joystickAxesCache); + m_joystickPOVs.swap(m_joystickPOVsCache); + m_joystickButtons.swap(m_joystickButtonsCache); + m_joystickDescriptor.swap(m_joystickDescriptorCache); + m_matchInfo.swap(m_matchInfoCache); } - m_joystickAxes.swap(m_joystickAxesCache); - m_joystickPOVs.swap(m_joystickPOVsCache); - m_joystickButtons.swap(m_joystickButtonsCache); - m_joystickDescriptor.swap(m_joystickDescriptorCache); - m_matchInfo.swap(m_matchInfoCache); + { + std::lock_guard waitLock(m_waitForDataMutex); + // Nofify all threads + m_waitForDataCounter++; + m_waitForDataCond.notify_all(); + } - std::lock_guard waitLock(m_waitForDataMutex); - // Nofify all threads - m_waitForDataCounter++; - m_waitForDataCond.notify_all(); + SendMatchData(); } /** @@ -717,6 +823,8 @@ DriverStation::DriverStation() { std::make_unique(kJoystickPorts); m_matchInfoCache = std::make_unique(); + m_matchDataSender = std::make_unique(); + // All joysticks should default to having zero axes, povs and buttons, so // uninitialized memory doesn't get sent to speed controllers. for (unsigned int i = 0; i < kJoystickPorts; i++) { diff --git a/wpilibc/src/main/native/include/DriverStation.h b/wpilibc/src/main/native/include/DriverStation.h index 345bb7fc4e..9f9b1af82b 100644 --- a/wpilibc/src/main/native/include/DriverStation.h +++ b/wpilibc/src/main/native/include/DriverStation.h @@ -25,6 +25,7 @@ namespace frc { struct MatchInfoData; +class MatchDataSender; /** * Provide access to the network communication data to / from the Driver @@ -133,6 +134,7 @@ class DriverStation : public ErrorBase, public RobotStateInterface { void ReportJoystickUnpluggedWarning(const llvm::Twine& message); void Run(); void UpdateControlWord(bool force, HAL_ControlWord& controlWord) const; + void SendMatchData(); // Joystick User Data std::unique_ptr m_joystickAxes; @@ -148,6 +150,8 @@ class DriverStation : public ErrorBase, public RobotStateInterface { std::unique_ptr m_joystickDescriptorCache; std::unique_ptr m_matchInfoCache; + std::unique_ptr m_matchDataSender; + // Joystick button rising/falling edge flags std::array m_joystickButtonsPressed; std::array m_joystickButtonsReleased; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index f4981079d9..f6c847d077 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -13,6 +13,9 @@ import java.util.concurrent.locks.Condition; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.hal.AllianceStationID; import edu.wpi.first.wpilibj.hal.ControlWord; import edu.wpi.first.wpilibj.hal.HAL; @@ -77,6 +80,51 @@ public class DriverStation implements RobotState.Interface { } } /* DriverStationTask */ + private static class MatchDataSender { + @SuppressWarnings("MemberName") + NetworkTable table; + @SuppressWarnings("MemberName") + NetworkTableEntry typeMetadata; + @SuppressWarnings("MemberName") + NetworkTableEntry gameSpecificMessage; + @SuppressWarnings("MemberName") + NetworkTableEntry eventName; + @SuppressWarnings("MemberName") + NetworkTableEntry matchNumber; + @SuppressWarnings("MemberName") + NetworkTableEntry replayNumber; + @SuppressWarnings("MemberName") + NetworkTableEntry matchType; + @SuppressWarnings("MemberName") + NetworkTableEntry alliance; + @SuppressWarnings("MemberName") + NetworkTableEntry station; + @SuppressWarnings("MemberName") + NetworkTableEntry controlWord; + + MatchDataSender() { + table = NetworkTableInstance.getDefault().getTable("FMSInfo"); + typeMetadata = table.getEntry(".type"); + typeMetadata.forceSetString("FMSInfo"); + gameSpecificMessage = table.getEntry("GameSpecificMessage"); + gameSpecificMessage.forceSetString(""); + eventName = table.getEntry("EventName"); + eventName.forceSetString(""); + matchNumber = table.getEntry("MatchNumber"); + matchNumber.forceSetDouble(0); + replayNumber = table.getEntry("ReplayNumber"); + replayNumber.forceSetDouble(0); + matchType = table.getEntry("MatchType"); + matchType.forceSetDouble(0); + alliance = table.getEntry("IsRedAlliance"); + alliance.forceSetBoolean(true); + station = table.getEntry("StationNumber"); + station.forceSetDouble(1); + controlWord = table.getEntry("FMSControlData"); + controlWord.forceSetDouble(0); + } + } + private static DriverStation instance = new DriverStation(); // Joystick User Data @@ -98,6 +146,8 @@ public class DriverStation implements RobotState.Interface { // preallocated byte buffer for button count private ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1); + private MatchDataSender m_matchDataSender; + // Internal Driver Station thread private Thread m_thread; private volatile boolean m_threadKeepAlive = true; @@ -157,6 +207,8 @@ public class DriverStation implements RobotState.Interface { m_controlWordCache = new ControlWord(); m_lastControlWordUpdate = 0; + m_matchDataSender = new MatchDataSender(); + m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation"); m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2); @@ -908,6 +960,61 @@ public class DriverStation implements RobotState.Interface { m_userInTest = entering; } + private void sendMatchData() { + AllianceStationID alliance = HAL.getAllianceStation(); + boolean isRedAlliance = false; + int stationNumber = 1; + switch (alliance) { + case Blue1: + isRedAlliance = false; + stationNumber = 1; + break; + case Blue2: + isRedAlliance = false; + stationNumber = 2; + break; + case Blue3: + isRedAlliance = false; + stationNumber = 3; + break; + case Red1: + isRedAlliance = true; + stationNumber = 1; + break; + case Red2: + isRedAlliance = true; + stationNumber = 2; + break; + default: + isRedAlliance = true; + stationNumber = 3; + break; + } + + + String eventName; + String gameSpecificMessage; + int matchNumber; + int replayNumber; + int matchType; + synchronized (m_cacheDataMutex) { + eventName = m_matchInfo.eventName; + gameSpecificMessage = m_matchInfo.gameSpecificMessage; + matchNumber = m_matchInfo.matchNumber; + replayNumber = m_matchInfo.replayNumber; + matchType = m_matchInfo.matchType; + } + + m_matchDataSender.alliance.setBoolean(isRedAlliance); + m_matchDataSender.station.setDouble(stationNumber); + m_matchDataSender.eventName.setString(eventName); + m_matchDataSender.gameSpecificMessage.setString(gameSpecificMessage); + m_matchDataSender.matchNumber.setDouble(matchNumber); + m_matchDataSender.replayNumber.setDouble(replayNumber); + m_matchDataSender.matchType.setDouble(matchType); + m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord()); + } + /** * Copy data from the DS task for the user. If no new data exists, it will just be returned, * otherwise the data will be copied from the DS polling loop. @@ -962,6 +1069,8 @@ public class DriverStation implements RobotState.Interface { m_waitForDataCount++; m_waitForDataCond.signalAll(); m_waitForDataMutex.unlock(); + + sendMatchData(); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/HAL.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/HAL.java index 66b447e7d8..2516a40989 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/HAL.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/HAL.java @@ -52,7 +52,7 @@ public class HAL extends JNIWrapper { */ public static native int report(int resource, int instanceNumber, int context, String feature); - private static native int nativeGetControlWord(); + public static native int nativeGetControlWord(); @SuppressWarnings("JavadocMethod") public static void getControlWord(ControlWord controlWord) {