diff --git a/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp b/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp index ee8c2b1b24..458046c87e 100644 --- a/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp +++ b/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp @@ -49,6 +49,7 @@ class TestEnvironment : public testing::Environment { HAL_GetControlWord(&controlWord); return controlWord.enabled && controlWord.dsAttached; }; + HAL_RefreshDSData(); while (!checkEnabled()) { if (enableCounter > 50) { // Robot did not enable properly after 5 seconds. @@ -60,6 +61,7 @@ class TestEnvironment : public testing::Environment { std::this_thread::sleep_for(100ms); fmt::print("Waiting for enable: {}\n", enableCounter++); + HAL_RefreshDSData(); } std::this_thread::sleep_for(500ms); } diff --git a/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java b/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java new file mode 100644 index 0000000000..f40a38cb72 --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java @@ -0,0 +1,137 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.hal; + +import java.nio.ByteBuffer; + +public class DriverStationJNI extends JNIWrapper { + public static native void observeUserProgramStarting(); + + public static native void observeUserProgramDisabled(); + + public static native void observeUserProgramAutonomous(); + + public static native void observeUserProgramTeleop(); + + public static native void observeUserProgramTest(); + + public static void report(int resource, int instanceNumber) { + report(resource, instanceNumber, 0, ""); + } + + public static void report(int resource, int instanceNumber, int context) { + report(resource, instanceNumber, context, ""); + } + + /** + * Report the usage of a resource of interest. + * + *

Original signature: uint32_t report(tResourceType, uint8_t, uint8_t, const + * char*) + * + * @param resource one of the values in the tResourceType above (max value 51). + * @param instanceNumber an index that identifies the resource instance. + * @param context an optional additional context number for some cases (such as module number). + * Set to 0 to omit. + * @param feature a string to be included describing features in use on a specific resource. + * Setting the same resource more than once allows you to change the feature string. + * @return TODO + */ + public static native int report(int resource, int instanceNumber, int context, String feature); + + public static native int nativeGetControlWord(); + + @SuppressWarnings("MissingJavadocMethod") + public static void getControlWord(ControlWord controlWord) { + int word = nativeGetControlWord(); + controlWord.update( + (word & 1) != 0, + ((word >> 1) & 1) != 0, + ((word >> 2) & 1) != 0, + ((word >> 3) & 1) != 0, + ((word >> 4) & 1) != 0, + ((word >> 5) & 1) != 0); + } + + private static native int nativeGetAllianceStation(); + + public static final int kRed1AllianceStation = 0; + public static final int kRed2AllianceStation = 1; + public static final int kRed3AllianceStation = 2; + public static final int kBlue1AllianceStation = 3; + public static final int kBlue2AllianceStation = 4; + public static final int kBlue3AllianceStation = 5; + + @SuppressWarnings("MissingJavadocMethod") + public static AllianceStationID getAllianceStation() { + switch (nativeGetAllianceStation()) { + case kRed1AllianceStation: + return AllianceStationID.Red1; + case kRed2AllianceStation: + return AllianceStationID.Red2; + case kRed3AllianceStation: + return AllianceStationID.Red3; + case kBlue1AllianceStation: + return AllianceStationID.Blue1; + case kBlue2AllianceStation: + return AllianceStationID.Blue2; + case kBlue3AllianceStation: + return AllianceStationID.Blue3; + default: + return null; + } + } + + public static final int kMaxJoystickAxes = 12; + public static final int kMaxJoystickPOVs = 12; + public static final int kMaxJoysticks = 6; + + public static native int getJoystickAxes(byte joystickNum, float[] axesArray); + + public static native int getJoystickAxesRaw(byte joystickNum, int[] rawAxesArray); + + public static native int getJoystickPOVs(byte joystickNum, short[] povsArray); + + public static native int getJoystickButtons(byte joystickNum, ByteBuffer count); + + public static native void getAllJoystickData( + float[] axesArray, byte[] rawAxesArray, short[] povsArray, long[] buttonsAndMetadata); + + public static native int setJoystickOutputs( + byte joystickNum, int outputs, short leftRumble, short rightRumble); + + public static native int getJoystickIsXbox(byte joystickNum); + + public static native int getJoystickType(byte joystickNum); + + public static native String getJoystickName(byte joystickNum); + + public static native int getJoystickAxisType(byte joystickNum, byte axis); + + public static native double getMatchTime(); + + public static native int getMatchInfo(MatchInfoData info); + + public static native int sendError( + boolean isError, + int errorCode, + boolean isLVCode, + String details, + String location, + String callStack, + boolean printMsg); + + public static native int sendConsoleLine(String line); + + public static native void refreshDSData(); + + public static native void provideNewDataEventHandle(int handle); + + public static native void removeNewDataEventHandle(int handle); + + public static native boolean getOutputsActive(); + + private DriverStationJNI() {} +} diff --git a/hal/src/main/java/edu/wpi/first/hal/HAL.java b/hal/src/main/java/edu/wpi/first/hal/HAL.java index 5a63d50abd..68e6ec8493 100644 --- a/hal/src/main/java/edu/wpi/first/hal/HAL.java +++ b/hal/src/main/java/edu/wpi/first/hal/HAL.java @@ -4,7 +4,6 @@ package edu.wpi.first.hal; -import java.nio.ByteBuffer; import java.util.ArrayList; import java.util.List; @@ -13,8 +12,6 @@ import java.util.List; * . */ public final class HAL extends JNIWrapper { - public static native void waitForDSData(); - public static native boolean initialize(int timeout, int mode); public static native void shutdown(); @@ -116,15 +113,13 @@ public final class HAL extends JNIWrapper { } } - public static native void observeUserProgramStarting(); + public static native boolean getBrownedOut(); - public static native void observeUserProgramDisabled(); + public static native boolean getSystemActive(); - public static native void observeUserProgramAutonomous(); + public static native int getPortWithModule(byte module, byte channel); - public static native void observeUserProgramTeleop(); - - public static native void observeUserProgramTest(); + public static native int getPort(byte channel); public static void report(int resource, int instanceNumber) { report(resource, instanceNumber, 0, ""); @@ -134,114 +129,9 @@ public final class HAL extends JNIWrapper { report(resource, instanceNumber, context, ""); } - /** - * Report the usage of a resource of interest. - * - *

Original signature: uint32_t report(tResourceType, uint8_t, uint8_t, const - * char*) - * - * @param resource one of the values in the tResourceType above (max value 51). - * @param instanceNumber an index that identifies the resource instance. - * @param context an optional additional context number for some cases (such as module number). - * Set to 0 to omit. - * @param feature a string to be included describing features in use on a specific resource. - * Setting the same resource more than once allows you to change the feature string. - * @return TODO - */ - public static native int report(int resource, int instanceNumber, int context, String feature); - - public static native int nativeGetControlWord(); - - /** - * Get the current DriverStation control word. - * - * @param controlWord Storage for control word. - */ - public static void getControlWord(ControlWord controlWord) { - int word = nativeGetControlWord(); - controlWord.update( - (word & 1) != 0, - ((word >> 1) & 1) != 0, - ((word >> 2) & 1) != 0, - ((word >> 3) & 1) != 0, - ((word >> 4) & 1) != 0, - ((word >> 5) & 1) != 0); + public static int report(int resource, int instanceNumber, int context, String feature) { + return DriverStationJNI.report(resource, instanceNumber, context, feature); } - private static native int nativeGetAllianceStation(); - - /** - * Get the alliance station. - * - * @return The alliance station. - */ - public static AllianceStationID getAllianceStation() { - switch (nativeGetAllianceStation()) { - case 0: - return AllianceStationID.Red1; - case 1: - return AllianceStationID.Red2; - case 2: - return AllianceStationID.Red3; - case 3: - return AllianceStationID.Blue1; - case 4: - return AllianceStationID.Blue2; - case 5: - return AllianceStationID.Blue3; - default: - return null; - } - } - - public static native boolean isNewControlData(); - - public static native void releaseDSMutex(); - - public static native boolean waitForDSDataTimeout(double timeout); - - public static final int kMaxJoystickAxes = 12; - public static final int kMaxJoystickPOVs = 12; - - public static native short getJoystickAxes(byte joystickNum, float[] axesArray); - - public static native short getJoystickPOVs(byte joystickNum, short[] povsArray); - - public static native int getJoystickButtons(byte joystickNum, ByteBuffer count); - - public static native int setJoystickOutputs( - byte joystickNum, int outputs, short leftRumble, short rightRumble); - - public static native int getJoystickIsXbox(byte joystickNum); - - public static native int getJoystickType(byte joystickNum); - - public static native String getJoystickName(byte joystickNum); - - public static native int getJoystickAxisType(byte joystickNum, byte axis); - - public static native double getMatchTime(); - - public static native boolean getSystemActive(); - - public static native boolean getBrownedOut(); - - public static native int getMatchInfo(MatchInfoData info); - - public static native int sendError( - boolean isError, - int errorCode, - boolean isLVCode, - String details, - String location, - String callStack, - boolean printMsg); - - public static native int sendConsoleLine(String line); - - public static native int getPortWithModule(byte module, byte channel); - - public static native int getPort(byte channel); - private HAL() {} } diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index e7ef1948f9..b9c6c73521 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/hal/src/main/native/athena/FRCDriverStation.cpp @@ -13,11 +13,14 @@ #include #include #include +#include #include +#include #include #include #include "hal/DriverStation.h" +#include "hal/Errors.h" static_assert(sizeof(int32_t) >= sizeof(int), "FRC_NetworkComm status variable is larger than 32 bits"); @@ -27,25 +30,45 @@ struct HAL_JoystickAxesInt { int16_t axes[HAL_kMaxJoystickAxes]; }; -static constexpr int kJoystickPorts = 6; +namespace { +struct JoystickDataCache { + JoystickDataCache() { std::memset(this, 0, sizeof(*this)); } + void Update(); + + HAL_JoystickAxes axes[HAL_kMaxJoysticks]; + HAL_JoystickPOVs povs[HAL_kMaxJoysticks]; + HAL_JoystickButtons buttons[HAL_kMaxJoysticks]; + HAL_AllianceStationID allianceStation; + float matchTime; + bool updated; +}; +static_assert(std::is_standard_layout_v); +// static_assert(std::is_trivial_v); + +struct FRCDriverStation { + wpi::EventVector newDataEvents; +}; +} // namespace + +static ::FRCDriverStation* driverStation; // Message and Data variables static wpi::mutex msgMutex; static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum, HAL_JoystickAxes* axes) { - HAL_JoystickAxesInt axesInt; + JoystickAxes_t netcommAxes; int retVal = FRC_NetworkCommunication_getJoystickAxes( - joystickNum, reinterpret_cast(&axesInt), - HAL_kMaxJoystickAxes); + joystickNum, &netcommAxes, HAL_kMaxJoystickAxes); // copy integer values to double values - axes->count = axesInt.count; + axes->count = netcommAxes.count; // current scaling is -128 to 127, can easily be patched in the future by // changing this function. - for (int32_t i = 0; i < axesInt.count; i++) { - int8_t value = axesInt.axes[i]; + for (int32_t i = 0; i < netcommAxes.count; i++) { + int8_t value = netcommAxes.axes[i]; + axes->raw[i] = value; if (value < 0) { axes->axes[i] = value / 128.0; } else { @@ -68,6 +91,30 @@ static int32_t HAL_GetJoystickButtonsInternal(int32_t joystickNum, return FRC_NetworkCommunication_getJoystickButtons( joystickNum, &buttons->buttons, &buttons->count); } + +void JoystickDataCache::Update() { + for (int i = 0; i < HAL_kMaxJoysticks; i++) { + HAL_GetJoystickAxesInternal(i, &axes[i]); + HAL_GetJoystickPOVsInternal(i, &povs[i]); + HAL_GetJoystickButtonsInternal(i, &buttons[i]); + } + FRC_NetworkCommunication_getAllianceStation( + reinterpret_cast(&allianceStation)); + FRC_NetworkCommunication_getMatchTime(&matchTime); +} + +#define CHECK_JOYSTICK_NUMBER(stickNum) \ + if ((stickNum) < 0 || (stickNum) >= HAL_kMaxJoysticks) \ + return PARAMETER_OUT_OF_RANGE + +static HAL_ControlWord newestControlWord; +static JoystickDataCache caches[3]; +static JoystickDataCache* currentRead = &caches[0]; +static JoystickDataCache* currentCache = &caches[1]; +static JoystickDataCache* cacheToUpdate = &caches[2]; + +static wpi::mutex cacheMutex; + /** * Retrieve the Joystick Descriptor for particular slot. * @@ -102,12 +149,6 @@ static int32_t HAL_GetJoystickDescriptorInternal(int32_t joystickNum, return retval; } -static int32_t HAL_GetControlWordInternal(HAL_ControlWord* controlWord) { - std::memset(controlWord, 0, sizeof(HAL_ControlWord)); - return FRC_NetworkCommunication_getControlWord( - reinterpret_cast(controlWord)); -} - static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) { MatchType_t matchType = MatchType_t::kMatchType_none; info->gameSpecificMessageSize = sizeof(info->gameSpecificMessage); @@ -126,16 +167,11 @@ static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) { return status; } -static wpi::mutex* newDSDataAvailableMutex; -static wpi::condition_variable* newDSDataAvailableCond; -static int newDSDataAvailableCounter{0}; - namespace hal::init { void InitializeFRCDriverStation() { - static wpi::mutex newMutex; - newDSDataAvailableMutex = &newMutex; - static wpi::condition_variable newCond; - newDSDataAvailableCond = &newCond; + std::memset(&newestControlWord, 0, sizeof(newestControlWord)); + static FRCDriverStation ds; + driverStation = &ds; } } // namespace hal::init @@ -248,20 +284,39 @@ int32_t HAL_SendConsoleLine(const char* line) { } int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { - return HAL_GetControlWordInternal(controlWord); + std::scoped_lock lock{cacheMutex}; + *controlWord = newestControlWord; + return 0; } int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) { - return HAL_GetJoystickAxesInternal(joystickNum, axes); + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{cacheMutex}; + *axes = currentRead->axes[joystickNum]; + return 0; } int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) { - return HAL_GetJoystickPOVsInternal(joystickNum, povs); + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{cacheMutex}; + *povs = currentRead->povs[joystickNum]; + return 0; } int32_t HAL_GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons) { - return HAL_GetJoystickButtonsInternal(joystickNum, buttons); + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{cacheMutex}; + *buttons = currentRead->buttons[joystickNum]; + return 0; +} + +void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs, + HAL_JoystickButtons* buttons) { + std::scoped_lock lock{cacheMutex}; + std::memcpy(axes, currentRead->axes, sizeof(currentRead->axes)); + std::memcpy(povs, currentRead->povs, sizeof(currentRead->povs)); + std::memcpy(buttons, currentRead->buttons, sizeof(currentRead->buttons)); } int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, @@ -274,10 +329,8 @@ int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) { } HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) { - HAL_AllianceStationID allianceStation; - *status = FRC_NetworkCommunication_getAllianceStation( - reinterpret_cast(&allianceStation)); - return allianceStation; + std::scoped_lock lock{cacheMutex}; + return currentRead->allianceStation; } HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) { @@ -317,6 +370,7 @@ void HAL_FreeJoystickName(char* name) { } int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) { + CHECK_JOYSTICK_NUMBER(joystickNum); HAL_JoystickDescriptor joystickDesc; if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { return -1; @@ -327,14 +381,14 @@ int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) { int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, int32_t leftRumble, int32_t rightRumble) { + CHECK_JOYSTICK_NUMBER(joystickNum); return FRC_NetworkCommunication_setJoystickOutputs(joystickNum, outputs, leftRumble, rightRumble); } double HAL_GetMatchTime(int32_t* status) { - float matchTime; - *status = FRC_NetworkCommunication_getMatchTime(&matchTime); - return matchTime; + std::scoped_lock lock{cacheMutex}; + return currentRead->matchTime; } void HAL_ObserveUserProgramStarting(void) { @@ -357,55 +411,6 @@ void HAL_ObserveUserProgramTest(void) { FRC_NetworkCommunication_observeUserProgramTest(); } -static int& GetThreadLocalLastCount() { - // There is a rollover error condition here. At Packet# = n * (uintmax), this - // will return false when instead it should return true. However, this at a - // 20ms rate occurs once every 2.7 years of DS connected runtime, so not - // worth the cycles to check. - thread_local int lastCount{0}; - return lastCount; -} - -HAL_Bool HAL_IsNewControlData(void) { - std::scoped_lock lock{*newDSDataAvailableMutex}; - int& lastCount = GetThreadLocalLastCount(); - int currentCount = newDSDataAvailableCounter; - if (lastCount == currentCount) { - return false; - } - lastCount = currentCount; - return true; -} - -void HAL_WaitForDSData(void) { - HAL_WaitForDSDataTimeout(0); -} - -HAL_Bool HAL_WaitForDSDataTimeout(double timeout) { - std::unique_lock lock{*newDSDataAvailableMutex}; - int& lastCount = GetThreadLocalLastCount(); - int currentCount = newDSDataAvailableCounter; - if (lastCount != currentCount) { - lastCount = currentCount; - return true; - } - auto timeoutTime = - std::chrono::steady_clock::now() + std::chrono::duration(timeout); - - while (newDSDataAvailableCounter == currentCount) { - if (timeout > 0) { - auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime); - if (timedOut == std::cv_status::timeout) { - return false; - } - } else { - newDSDataAvailableCond->wait(lock); - } - } - lastCount = newDSDataAvailableCounter; - return true; -} - // Constant number to be used for our occur handle constexpr int32_t refNumber = 42; @@ -415,45 +420,47 @@ static void newDataOccur(uint32_t refNum) { if (refNum != refNumber) { return; } - std::scoped_lock lock{*newDSDataAvailableMutex}; - // Notify all threads - ++newDSDataAvailableCounter; - newDSDataAvailableCond->notify_all(); + cacheToUpdate->Update(); + { + std::scoped_lock lock{cacheMutex}; + std::swap(currentCache, cacheToUpdate); + currentCache->updated = true; + } + driverStation->newDataEvents.Wakeup(); } -/* - * Call this to initialize the driver station communication. This will properly - * handle multiple calls. However note that this CANNOT be called from a library - * that interfaces with LabVIEW. - */ -void HAL_InitializeDriverStation(void) { - static std::atomic_bool initialized{false}; - static wpi::mutex initializeMutex; - // Initial check, as if it's true initialization has finished - if (initialized) { - return; +void HAL_RefreshDSData(void) { + HAL_ControlWord controlWord; + std::memset(&controlWord, 0, sizeof(controlWord)); + FRC_NetworkCommunication_getControlWord( + reinterpret_cast(&controlWord)); + std::scoped_lock lock{cacheMutex}; + if (currentCache->updated) { + std::swap(currentCache, currentRead); + currentCache->updated = false; } + newestControlWord = controlWord; +} - std::scoped_lock lock(initializeMutex); - // Second check in case another thread was waiting - if (initialized) { - return; - } +void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) { + driverStation->newDataEvents.Add(handle); +} +void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) { + driverStation->newDataEvents.Remove(handle); +} + +HAL_Bool HAL_GetOutputsEnabled(void) { + return FRC_NetworkCommunication_getWatchdogActive(); +} + +} // extern "C" + +namespace hal { +void InitializeDriverStation() { // Set up the occur function internally with NetComm NetCommRPCProxy_SetOccurFuncPointer(newDataOccur); // Set up our occur reference number setNewDataOccurRef(refNumber); - - initialized = true; } - -/* - * Releases the DS Mutex to allow proper shutdown of any threads that are - * waiting on it. - */ -void HAL_ReleaseDSMutex(void) { - newDataOccur(refNumber); -} - -} // extern "C" +} // namespace hal diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 5eba6e4b4c..08abf14bb6 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -40,6 +40,7 @@ static uint64_t dsStartTime; using namespace hal; namespace hal { +void InitializeDriverStation(); namespace init { void InitializeHAL() { InitializeCTREPCM(); @@ -431,7 +432,7 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { return false; } - HAL_InitializeDriverStation(); + hal::InitializeDriverStation(); dsStartTime = HAL_GetFPGATime(&status); if (status != 0) { diff --git a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp new file mode 100644 index 0000000000..f050f02129 --- /dev/null +++ b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp @@ -0,0 +1,450 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include +#include + +#include "HALUtil.h" +#include "edu_wpi_first_hal_DriverStationJNI.h" +#include "hal/DriverStation.h" +#include "hal/FRCUsageReporting.h" +#include "hal/HALBase.h" + +// TODO Static asserts + +using namespace hal; +using namespace wpi::java; + +extern "C" { + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: observeUserProgramStarting + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramStarting + (JNIEnv*, jclass) +{ + HAL_ObserveUserProgramStarting(); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: observeUserProgramDisabled + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramDisabled + (JNIEnv*, jclass) +{ + HAL_ObserveUserProgramDisabled(); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: observeUserProgramAutonomous + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramAutonomous + (JNIEnv*, jclass) +{ + HAL_ObserveUserProgramAutonomous(); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: observeUserProgramTeleop + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramTeleop + (JNIEnv*, jclass) +{ + HAL_ObserveUserProgramTeleop(); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: observeUserProgramTest + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramTest + (JNIEnv*, jclass) +{ + HAL_ObserveUserProgramTest(); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: report + * Signature: (IIILjava/lang/String;)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_report + (JNIEnv* paramEnv, jclass, jint paramResource, jint paramInstanceNumber, + jint paramContext, jstring paramFeature) +{ + JStringRef featureStr{paramEnv, paramFeature}; + jint returnValue = HAL_Report(paramResource, paramInstanceNumber, + paramContext, featureStr.c_str()); + return returnValue; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: nativeGetControlWord + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_nativeGetControlWord + (JNIEnv*, jclass) +{ + static_assert(sizeof(HAL_ControlWord) == sizeof(jint), + "Java int must match the size of control word"); + HAL_ControlWord controlWord; + HAL_GetControlWord(&controlWord); + jint retVal = 0; + std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord)); + return retVal; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: nativeGetAllianceStation + * Signature: ()I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_nativeGetAllianceStation + (JNIEnv*, jclass) +{ + int32_t status = 0; + auto allianceStation = HAL_GetAllianceStation(&status); + return static_cast(allianceStation); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickAxesRaw + * Signature: (B[I)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickAxesRaw + (JNIEnv* env, jclass, jbyte joystickNum, jintArray axesRawArray) +{ + HAL_JoystickAxes axes; + HAL_GetJoystickAxes(joystickNum, &axes); + + jsize javaSize = env->GetArrayLength(axesRawArray); + if (axes.count > javaSize) { + ThrowIllegalArgumentException( + env, + fmt::format("Native array size larger then passed in java array " + "size\nNative Size: {} Java Size: {}", + static_cast(axes.count), static_cast(javaSize))); + return 0; + } + + jint raw[HAL_kMaxJoystickAxes]; + for (int16_t i = 0; i < axes.count; i++) { + raw[i] = axes.raw[i]; + } + env->SetIntArrayRegion(axesRawArray, 0, axes.count, raw); + + return axes.count; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickAxes + * Signature: (B[F)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickAxes + (JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray) +{ + HAL_JoystickAxes axes; + HAL_GetJoystickAxes(joystickNum, &axes); + + jsize javaSize = env->GetArrayLength(axesArray); + if (axes.count > javaSize) { + ThrowIllegalArgumentException( + env, + fmt::format("Native array size larger then passed in java array " + "size\nNative Size: {} Java Size: {}", + static_cast(axes.count), static_cast(javaSize))); + return 0; + } + + env->SetFloatArrayRegion(axesArray, 0, axes.count, axes.axes); + + return axes.count; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickPOVs + * Signature: (B[S)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickPOVs + (JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray) +{ + HAL_JoystickPOVs povs; + HAL_GetJoystickPOVs(joystickNum, &povs); + + jsize javaSize = env->GetArrayLength(povsArray); + if (povs.count > javaSize) { + ThrowIllegalArgumentException( + env, + fmt::format("Native array size larger then passed in java array " + "size\nNative Size: {} Java Size: {}", + static_cast(povs.count), static_cast(javaSize))); + return 0; + } + + env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs); + + return povs.count; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getAllJoystickData + * Signature: ([F[B[S[J)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getAllJoystickData + (JNIEnv* env, jclass cls, jfloatArray axesArray, jbyteArray rawAxesArray, + jshortArray povsArray, jlongArray buttonsAndMetadataArray) +{ + HAL_JoystickAxes axes[HAL_kMaxJoysticks]; + HAL_JoystickPOVs povs[HAL_kMaxJoysticks]; + HAL_JoystickButtons buttons[HAL_kMaxJoysticks]; + + HAL_GetAllJoystickData(axes, povs, buttons); + + CriticalJFloatArrayRef jAxes(env, axesArray); + CriticalJByteArrayRef jRawAxes(env, rawAxesArray); + CriticalJShortArrayRef jPovs(env, povsArray); + CriticalJLongArrayRef jButtons(env, buttonsAndMetadataArray); + + static_assert(sizeof(jAxes[0]) == sizeof(axes[0].axes[0])); + static_assert(sizeof(jRawAxes[0]) == sizeof(axes[0].raw[0])); + static_assert(sizeof(jPovs[0]) == sizeof(povs[0].povs[0])); + + for (size_t i = 0; i < HAL_kMaxJoysticks; i++) { + std::memcpy(&jAxes[i * HAL_kMaxJoystickAxes], axes[i].axes, + sizeof(axes[i].axes)); + std::memcpy(&jRawAxes[i * HAL_kMaxJoystickAxes], axes[i].raw, + sizeof(axes[i].raw)); + std::memcpy(&jPovs[i * HAL_kMaxJoystickPOVs], povs[i].povs, + sizeof(povs[i].povs)); + jButtons[i * 4] = axes[i].count; + jButtons[(i * 4) + 1] = povs[i].count; + jButtons[(i * 4) + 2] = buttons[i].count; + jButtons[(i * 4) + 3] = buttons[i].buttons; + } +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickButtons + * Signature: (BLjava/lang/Object;)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickButtons + (JNIEnv* env, jclass, jbyte joystickNum, jobject count) +{ + HAL_JoystickButtons joystickButtons; + HAL_GetJoystickButtons(joystickNum, &joystickButtons); + jbyte* countPtr = + reinterpret_cast(env->GetDirectBufferAddress(count)); + *countPtr = joystickButtons.count; + return joystickButtons.buttons; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: setJoystickOutputs + * Signature: (BISS)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_setJoystickOutputs + (JNIEnv*, jclass, jbyte port, jint outputs, jshort leftRumble, + jshort rightRumble) +{ + return HAL_SetJoystickOutputs(port, outputs, leftRumble, rightRumble); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickIsXbox + * Signature: (B)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickIsXbox + (JNIEnv*, jclass, jbyte port) +{ + return HAL_GetJoystickIsXbox(port); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickType + * Signature: (B)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickType + (JNIEnv*, jclass, jbyte port) +{ + return HAL_GetJoystickType(port); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickName + * Signature: (B)Ljava/lang/String; + */ +JNIEXPORT jstring JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickName + (JNIEnv* env, jclass, jbyte port) +{ + char* joystickName = HAL_GetJoystickName(port); + jstring str = MakeJString(env, joystickName); + HAL_FreeJoystickName(joystickName); + return str; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getJoystickAxisType + * Signature: (BB)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getJoystickAxisType + (JNIEnv*, jclass, jbyte joystickNum, jbyte axis) +{ + return HAL_GetJoystickAxisType(joystickNum, axis); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getMatchTime + * Signature: ()D + */ +JNIEXPORT jdouble JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getMatchTime + (JNIEnv* env, jclass) +{ + int32_t status = 0; + return HAL_GetMatchTime(&status); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getMatchInfo + * Signature: (Ljava/lang/Object;)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getMatchInfo + (JNIEnv* env, jclass, jobject info) +{ + HAL_MatchInfo matchInfo; + auto status = HAL_GetMatchInfo(&matchInfo); + if (status == 0) { + SetMatchInfoObject(env, info, matchInfo); + } + return status; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: sendError + * Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_sendError + (JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode, + jstring details, jstring location, jstring callStack, jboolean printMsg) +{ + JStringRef detailsStr{env, details}; + JStringRef locationStr{env, location}; + JStringRef callStackStr{env, callStack}; + + jint returnValue = + HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(), + locationStr.c_str(), callStackStr.c_str(), printMsg); + return returnValue; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: sendConsoleLine + * Signature: (Ljava/lang/String;)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_sendConsoleLine + (JNIEnv* env, jclass, jstring line) +{ + JStringRef lineStr{env, line}; + + jint returnValue = HAL_SendConsoleLine(lineStr.c_str()); + return returnValue; +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: refreshDSData + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_refreshDSData + (JNIEnv*, jclass) +{ + HAL_RefreshDSData(); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: provideNewDataEventHandle + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_provideNewDataEventHandle + (JNIEnv*, jclass, jint handle) +{ + HAL_ProvideNewDataEventHandle(handle); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: removeNewDataEventHandle + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_removeNewDataEventHandle + (JNIEnv*, jclass, jint handle) +{ + HAL_RemoveNewDataEventHandle(handle); +} + +/* + * Class: edu_wpi_first_hal_DriverStationJNI + * Method: getOutputsActive + * Signature: ()Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_DriverStationJNI_getOutputsActive + (JNIEnv*, jclass) +{ + return HAL_GetOutputsEnabled(); +} +} // extern "C" diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp index 4e260324c9..b603a76cb3 100644 --- a/hal/src/main/native/cpp/jni/HAL.cpp +++ b/hal/src/main/native/cpp/jni/HAL.cpp @@ -106,310 +106,6 @@ Java_edu_wpi_first_hal_HAL_simPeriodicAfterNative HAL_SimPeriodicAfter(); } -/* - * Class: edu_wpi_first_hal_HAL - * Method: observeUserProgramStarting - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_HAL_observeUserProgramStarting - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramStarting(); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: observeUserProgramDisabled - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_HAL_observeUserProgramDisabled - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramDisabled(); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: observeUserProgramAutonomous - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_HAL_observeUserProgramAutonomous - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramAutonomous(); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: observeUserProgramTeleop - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_HAL_observeUserProgramTeleop - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramTeleop(); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: observeUserProgramTest - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_HAL_observeUserProgramTest - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramTest(); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: report - * Signature: (IIILjava/lang/String;)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_report - (JNIEnv* paramEnv, jclass, jint paramResource, jint paramInstanceNumber, - jint paramContext, jstring paramFeature) -{ - JStringRef featureStr{paramEnv, paramFeature}; - jint returnValue = HAL_Report(paramResource, paramInstanceNumber, - paramContext, featureStr.c_str()); - return returnValue; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: nativeGetControlWord - * Signature: ()I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_nativeGetControlWord - (JNIEnv*, jclass) -{ - static_assert(sizeof(HAL_ControlWord) == sizeof(jint), - "Java int must match the size of control word"); - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - jint retVal = 0; - std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord)); - return retVal; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: nativeGetAllianceStation - * Signature: ()I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_nativeGetAllianceStation - (JNIEnv*, jclass) -{ - int32_t status = 0; - auto allianceStation = HAL_GetAllianceStation(&status); - return static_cast(allianceStation); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getJoystickAxes - * Signature: (B[F)S - */ -JNIEXPORT jshort JNICALL -Java_edu_wpi_first_hal_HAL_getJoystickAxes - (JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray) -{ - HAL_JoystickAxes axes; - HAL_GetJoystickAxes(joystickNum, &axes); - - jsize javaSize = env->GetArrayLength(axesArray); - if (axes.count > javaSize) { - ThrowIllegalArgumentException( - env, - fmt::format("Native array size larger then passed in java array " - "size\nNative Size: {} Java Size: {}", - static_cast(axes.count), static_cast(javaSize))); - return 0; - } - - env->SetFloatArrayRegion(axesArray, 0, axes.count, axes.axes); - - return axes.count; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getJoystickPOVs - * Signature: (B[S)S - */ -JNIEXPORT jshort JNICALL -Java_edu_wpi_first_hal_HAL_getJoystickPOVs - (JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray) -{ - HAL_JoystickPOVs povs; - HAL_GetJoystickPOVs(joystickNum, &povs); - - jsize javaSize = env->GetArrayLength(povsArray); - if (povs.count > javaSize) { - ThrowIllegalArgumentException( - env, - fmt::format("Native array size larger then passed in java array " - "size\nNative Size: {} Java Size: {}", - static_cast(povs.count), static_cast(javaSize))); - return 0; - } - - env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs); - - return povs.count; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getJoystickButtons - * Signature: (BLjava/lang/Object;)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_getJoystickButtons - (JNIEnv* env, jclass, jbyte joystickNum, jobject count) -{ - HAL_JoystickButtons joystickButtons; - HAL_GetJoystickButtons(joystickNum, &joystickButtons); - jbyte* countPtr = - reinterpret_cast(env->GetDirectBufferAddress(count)); - *countPtr = joystickButtons.count; - return joystickButtons.buttons; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: setJoystickOutputs - * Signature: (BISS)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_setJoystickOutputs - (JNIEnv*, jclass, jbyte port, jint outputs, jshort leftRumble, - jshort rightRumble) -{ - return HAL_SetJoystickOutputs(port, outputs, leftRumble, rightRumble); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getJoystickIsXbox - * Signature: (B)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_getJoystickIsXbox - (JNIEnv*, jclass, jbyte port) -{ - return HAL_GetJoystickIsXbox(port); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getJoystickType - * Signature: (B)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_getJoystickType - (JNIEnv*, jclass, jbyte port) -{ - return HAL_GetJoystickType(port); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getJoystickName - * Signature: (B)Ljava/lang/String; - */ -JNIEXPORT jstring JNICALL -Java_edu_wpi_first_hal_HAL_getJoystickName - (JNIEnv* env, jclass, jbyte port) -{ - char* joystickName = HAL_GetJoystickName(port); - jstring str = MakeJString(env, joystickName); - HAL_FreeJoystickName(joystickName); - return str; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getJoystickAxisType - * Signature: (BB)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_getJoystickAxisType - (JNIEnv*, jclass, jbyte joystickNum, jbyte axis) -{ - return HAL_GetJoystickAxisType(joystickNum, axis); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: isNewControlData - * Signature: ()Z - */ -JNIEXPORT jboolean JNICALL -Java_edu_wpi_first_hal_HAL_isNewControlData - (JNIEnv*, jclass) -{ - return static_cast(HAL_IsNewControlData()); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: waitForDSData - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_HAL_waitForDSData - (JNIEnv* env, jclass) -{ - HAL_WaitForDSData(); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: releaseDSMutex - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_edu_wpi_first_hal_HAL_releaseDSMutex - (JNIEnv* env, jclass) -{ - HAL_ReleaseDSMutex(); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: waitForDSDataTimeout - * Signature: (D)Z - */ -JNIEXPORT jboolean JNICALL -Java_edu_wpi_first_hal_HAL_waitForDSDataTimeout - (JNIEnv*, jclass, jdouble timeout) -{ - return static_cast(HAL_WaitForDSDataTimeout(timeout)); -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: getMatchTime - * Signature: ()D - */ -JNIEXPORT jdouble JNICALL -Java_edu_wpi_first_hal_HAL_getMatchTime - (JNIEnv* env, jclass) -{ - int32_t status = 0; - return HAL_GetMatchTime(&status); -} - /* * Class: edu_wpi_first_hal_HAL * Method: getSystemActive @@ -440,58 +136,6 @@ Java_edu_wpi_first_hal_HAL_getBrownedOut return val; } -/* - * Class: edu_wpi_first_hal_HAL - * Method: getMatchInfo - * Signature: (Ljava/lang/Object;)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_getMatchInfo - (JNIEnv* env, jclass, jobject info) -{ - HAL_MatchInfo matchInfo; - auto status = HAL_GetMatchInfo(&matchInfo); - if (status == 0) { - SetMatchInfoObject(env, info, matchInfo); - } - return status; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: sendError - * Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_sendError - (JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode, - jstring details, jstring location, jstring callStack, jboolean printMsg) -{ - JStringRef detailsStr{env, details}; - JStringRef locationStr{env, location}; - JStringRef callStackStr{env, callStack}; - - jint returnValue = - HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(), - locationStr.c_str(), callStackStr.c_str(), printMsg); - return returnValue; -} - -/* - * Class: edu_wpi_first_hal_HAL - * Method: sendConsoleLine - * Signature: (Ljava/lang/String;)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_hal_HAL_sendConsoleLine - (JNIEnv* env, jclass, jstring line) -{ - JStringRef lineStr{env, line}; - - jint returnValue = HAL_SendConsoleLine(lineStr.c_str()); - return returnValue; -} - /* * Class: edu_wpi_first_hal_HAL * Method: getPortWithModule diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h index 1839cfc99c..fabc45f905 100644 --- a/hal/src/main/native/include/hal/DriverStation.h +++ b/hal/src/main/native/include/hal/DriverStation.h @@ -6,6 +6,8 @@ #include +#include + #include "hal/DriverStationTypes.h" #include "hal/Types.h" @@ -87,6 +89,9 @@ int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs); int32_t HAL_GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons); +void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs, + HAL_JoystickButtons* buttons); + /** * Retrieves the Joystick Descriptor for particular slot. * @@ -183,6 +188,11 @@ int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, */ double HAL_GetMatchTime(int32_t* status); +/** + * Gets if outputs are enabled by the control system. + */ +HAL_Bool HAL_GetOutputsEnabled(void); + /** * Gets info about a specific match. * @@ -191,44 +201,10 @@ double HAL_GetMatchTime(int32_t* status); */ int32_t HAL_GetMatchInfo(HAL_MatchInfo* info); -/** - * Releases the DS Mutex to allow proper shutdown of any threads that are - * waiting on it. - */ -void HAL_ReleaseDSMutex(void); +void HAL_RefreshDSData(void); -/** - * Has a new control packet from the driver station arrived since the last - * time this function was called? - * - * @return true if the control data has been updated since the last call - */ -HAL_Bool HAL_IsNewControlData(void); - -/** - * Waits for the newest DS packet to arrive. Note that this is a blocking call. - * Checks if new control data has arrived since the last HAL_WaitForDSData or - * HAL_IsNewControlData call. If new data has not arrived, waits for new data - * to arrive. Otherwise, returns immediately. - */ -void HAL_WaitForDSData(void); - -/** - * Waits for the newest DS packet to arrive. If timeout is <= 0, this will wait - * forever. Otherwise, it will wait until either a new packet, or the timeout - * time has passed. - * - * @param[in] timeout timeout in seconds - * @return true for new data, false for timeout - */ -HAL_Bool HAL_WaitForDSDataTimeout(double timeout); - -/** - * Initializes the driver station communication. This will properly - * handle multiple calls. However note that this CANNOT be called from a library - * that interfaces with LabVIEW. - */ -void HAL_InitializeDriverStation(void); +void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle); +void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle); /** * Sets the program starting flag in the DS. diff --git a/hal/src/main/native/include/hal/DriverStationTypes.h b/hal/src/main/native/include/hal/DriverStationTypes.h index 21f9088bd8..277c2860f2 100644 --- a/hal/src/main/native/include/hal/DriverStationTypes.h +++ b/hal/src/main/native/include/hal/DriverStationTypes.h @@ -69,6 +69,7 @@ HAL_ENUM(HAL_MatchType) { struct HAL_JoystickAxes { int16_t count; float axes[HAL_kMaxJoystickAxes]; + uint8_t raw[HAL_kMaxJoystickAxes]; }; typedef struct HAL_JoystickAxes HAL_JoystickAxes; diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 723cdacfa8..399080d5a5 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -14,32 +14,78 @@ #include #include +#include #include #include #include "HALInitializer.h" +#include "hal/Errors.h" #include "hal/cpp/fpga_clock.h" #include "hal/simulation/MockHooks.h" #include "mockdata/DriverStationDataInternal.h" static wpi::mutex msgMutex; -static wpi::condition_variable* newDSDataAvailableCond; -static wpi::mutex newDSDataAvailableMutex; -static int newDSDataAvailableCounter{0}; -static std::atomic_bool isFinalized{false}; static std::atomic sendErrorHandler{nullptr}; static std::atomic sendConsoleLineHandler{ nullptr}; +using namespace hal; + +static constexpr int kJoystickPorts = 6; + +namespace { +struct JoystickDataCache { + JoystickDataCache() { std::memset(this, 0, sizeof(*this)); } + void Update(); + + HAL_JoystickAxes axes[kJoystickPorts]; + HAL_JoystickPOVs povs[kJoystickPorts]; + HAL_JoystickButtons buttons[kJoystickPorts]; + HAL_AllianceStationID allianceStation; + double matchTime; + bool updated; +}; +static_assert(std::is_standard_layout_v); +// static_assert(std::is_trivial_v); + +static std::atomic_bool gShutdown{false}; + +struct FRCDriverStation { + ~FRCDriverStation() { gShutdown = true; } + wpi::EventVector newDataEvents; + wpi::mutex cacheMutex; +}; +} // namespace + +void JoystickDataCache::Update() { + for (int i = 0; i < kJoystickPorts; i++) { + SimDriverStationData->GetJoystickAxes(i, &axes[i]); + SimDriverStationData->GetJoystickPOVs(i, &povs[i]); + SimDriverStationData->GetJoystickButtons(i, &buttons[i]); + } + allianceStation = SimDriverStationData->allianceStationId; + matchTime = SimDriverStationData->matchTime; +} + +#define CHECK_JOYSTICK_NUMBER(stickNum) \ + if ((stickNum) < 0 || (stickNum) >= HAL_kMaxJoysticks) \ + return PARAMETER_OUT_OF_RANGE + +static HAL_ControlWord newestControlWord; +static JoystickDataCache caches[3]; +static JoystickDataCache* currentRead = &caches[0]; +static JoystickDataCache* currentCache = &caches[1]; +static JoystickDataCache* cacheToUpdate = &caches[2]; + +static ::FRCDriverStation* driverStation; + namespace hal::init { void InitializeDriverStation() { - static wpi::condition_variable nddaC; - newDSDataAvailableCond = &nddaC; + static FRCDriverStation ds; + driverStation = &ds; } } // namespace hal::init -using namespace hal; - extern "C" { void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) { @@ -122,39 +168,49 @@ int32_t HAL_SendConsoleLine(const char* line) { } int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { - std::memset(controlWord, 0, sizeof(HAL_ControlWord)); - controlWord->enabled = SimDriverStationData->enabled; - controlWord->autonomous = SimDriverStationData->autonomous; - controlWord->test = SimDriverStationData->test; - controlWord->eStop = SimDriverStationData->eStop; - controlWord->fmsAttached = SimDriverStationData->fmsAttached; - controlWord->dsAttached = SimDriverStationData->dsAttached; + std::scoped_lock lock{driverStation->cacheMutex}; + *controlWord = newestControlWord; return 0; } HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) { - *status = 0; - return SimDriverStationData->allianceStationId; + std::scoped_lock lock{driverStation->cacheMutex}; + return currentRead->allianceStation; } int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) { - SimDriverStationData->GetJoystickAxes(joystickNum, axes); + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{driverStation->cacheMutex}; + *axes = currentRead->axes[joystickNum]; return 0; } int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) { - SimDriverStationData->GetJoystickPOVs(joystickNum, povs); + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{driverStation->cacheMutex}; + *povs = currentRead->povs[joystickNum]; return 0; } int32_t HAL_GetJoystickButtons(int32_t joystickNum, HAL_JoystickButtons* buttons) { - SimDriverStationData->GetJoystickButtons(joystickNum, buttons); + CHECK_JOYSTICK_NUMBER(joystickNum); + std::scoped_lock lock{driverStation->cacheMutex}; + *buttons = currentRead->buttons[joystickNum]; return 0; } +void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs, + HAL_JoystickButtons* buttons) { + std::scoped_lock lock{driverStation->cacheMutex}; + std::memcpy(axes, currentRead->axes, sizeof(currentRead->axes)); + std::memcpy(povs, currentRead->povs, sizeof(currentRead->povs)); + std::memcpy(buttons, currentRead->buttons, sizeof(currentRead->buttons)); +} + int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, HAL_JoystickDescriptor* desc) { + CHECK_JOYSTICK_NUMBER(joystickNum); SimDriverStationData->GetJoystickDescriptor(joystickNum, desc); return 0; } @@ -196,7 +252,8 @@ int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, } double HAL_GetMatchTime(int32_t* status) { - return SimDriverStationData->matchTime; + std::scoped_lock lock{driverStation->cacheMutex}; + return currentRead->matchTime; } int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) { @@ -224,103 +281,57 @@ void HAL_ObserveUserProgramTest(void) { // TODO } -static int& GetThreadLocalLastCount() { - // There is a rollover error condition here. At Packet# = n * (uintmax), this - // will return false when instead it should return true. However, this at a - // 20ms rate occurs once every 2.7 years of DS connected runtime, so not - // worth the cycles to check. - thread_local int lastCount{0}; - return lastCount; +void HAL_RefreshDSData(void) { + HAL_ControlWord controlWord; + std::memset(&controlWord, 0, sizeof(controlWord)); + controlWord.enabled = SimDriverStationData->enabled; + controlWord.autonomous = SimDriverStationData->autonomous; + controlWord.test = SimDriverStationData->test; + controlWord.eStop = SimDriverStationData->eStop; + controlWord.fmsAttached = SimDriverStationData->fmsAttached; + controlWord.dsAttached = SimDriverStationData->dsAttached; + std::scoped_lock lock{driverStation->cacheMutex}; + if (currentCache->updated) { + std::swap(currentCache, currentRead); + currentCache->updated = false; + } + newestControlWord = controlWord; } -HAL_Bool HAL_IsNewControlData(void) { - std::scoped_lock lock(newDSDataAvailableMutex); - int& lastCount = GetThreadLocalLastCount(); - int currentCount = newDSDataAvailableCounter; - if (lastCount == currentCount) { - return false; - } - lastCount = currentCount; - return true; -} - -void HAL_WaitForDSData(void) { - HAL_WaitForDSDataTimeout(0); -} - -HAL_Bool HAL_WaitForDSDataTimeout(double timeout) { - std::unique_lock lock(newDSDataAvailableMutex); - int& lastCount = GetThreadLocalLastCount(); - int currentCount = newDSDataAvailableCounter; - if (lastCount != currentCount) { - lastCount = currentCount; - return true; - } - - if (isFinalized.load()) { - return false; - } - auto timeoutTime = - std::chrono::steady_clock::now() + std::chrono::duration(timeout); - - while (newDSDataAvailableCounter == currentCount) { - if (timeout > 0) { - auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime); - if (timedOut == std::cv_status::timeout) { - return false; - } - } else { - newDSDataAvailableCond->wait(lock); - } - } - lastCount = newDSDataAvailableCounter; - return true; -} - -// Constant number to be used for our occur handle -constexpr int32_t refNumber = 42; - -static int32_t newDataOccur(uint32_t refNum) { - // Since we could get other values, require our specific handle - // to signal our threads - if (refNum != refNumber) { - return 0; - } - SimDriverStationData->CallNewDataCallbacks(); - std::scoped_lock lock(newDSDataAvailableMutex); - // Nofify all threads - newDSDataAvailableCounter++; - newDSDataAvailableCond->notify_all(); - return 0; -} - -void HAL_InitializeDriverStation(void) { - hal::init::CheckInit(); - static std::atomic_bool initialized{false}; - static wpi::mutex initializeMutex; - // Initial check, as if it's true initialization has finished - if (initialized) { +void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) { + if (gShutdown) { return; } - - std::scoped_lock lock(initializeMutex); - // Second check in case another thread was waiting - if (initialized) { - return; - } - - SimDriverStationData->ResetData(); - - std::atexit([]() { - isFinalized.store(true); - HAL_ReleaseDSMutex(); - }); - - initialized = true; + driverStation->newDataEvents.Add(handle); } -void HAL_ReleaseDSMutex(void) { - newDataOccur(refNumber); +void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) { + if (gShutdown) { + return; + } + driverStation->newDataEvents.Remove(handle); +} + +HAL_Bool HAL_GetOutputsEnabled(void) { + std::scoped_lock lock{driverStation->cacheMutex}; + return newestControlWord.enabled && newestControlWord.dsAttached; } } // extern "C" + +namespace hal { +void NewDriverStationData() { + cacheToUpdate->Update(); + { + std::scoped_lock lock{driverStation->cacheMutex}; + std::swap(currentCache, cacheToUpdate); + currentCache->updated = true; + } + driverStation->newDataEvents.Wakeup(); + SimDriverStationData->CallNewDataCallbacks(); +} + +void InitializeDriverStation() { + SimDriverStationData->ResetData(); +} +} // namespace hal diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 9856f0110d..eca7c2578b 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -57,6 +57,10 @@ static std::vector> gOnShutdown; static SimPeriodicCallbackRegistry gSimPeriodicBefore; static SimPeriodicCallbackRegistry gSimPeriodicAfter; +namespace hal { +void InitializeDriverStation(); +} // namespace hal + namespace hal::init { void InitializeHAL() { InitializeAccelerometerData(); @@ -335,7 +339,7 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { hal::init::HAL_IsInitialized.store(true); hal::RestartTiming(); - HAL_InitializeDriverStation(); + hal::InitializeDriverStation(); initialized = true; diff --git a/hal/src/main/native/sim/mockdata/DriverStationData.cpp b/hal/src/main/native/sim/mockdata/DriverStationData.cpp index 1c76a7a7ca..3756d6e492 100644 --- a/hal/src/main/native/sim/mockdata/DriverStationData.cpp +++ b/hal/src/main/native/sim/mockdata/DriverStationData.cpp @@ -216,8 +216,12 @@ void DriverStationData::CallNewDataCallbacks() { m_newDataCallbacks(&empty); } +namespace hal { +void NewDriverStationData(); +} // namespace hal + void DriverStationData::NotifyNewData() { - HAL_ReleaseDSMutex(); + hal::NewDriverStationData(); } void DriverStationData::SetJoystickButton(int32_t stick, int32_t button, diff --git a/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp b/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp index 4925e162b7..59658cbaa1 100644 --- a/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp +++ b/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp @@ -61,6 +61,9 @@ TEST(DriverStationTest, Joystick) { HALSIM_SetJoystickPOVs(joystickUnderTest, &set_povs); HALSIM_SetJoystickButtons(joystickUnderTest, &set_buttons); + HALSIM_NotifyDriverStationNewData(); + HAL_RefreshDSData(); + // Check the set values HAL_GetJoystickAxes(joystickUnderTest, &axes); HAL_GetJoystickPOVs(joystickUnderTest, &povs); @@ -89,6 +92,9 @@ TEST(DriverStationTest, Joystick) { // Reset HALSIM_ResetDriverStationData(); + HALSIM_NotifyDriverStationNewData(); + HAL_RefreshDSData(); + for (int joystickNum = 0; joystickNum < 6; ++joystickNum) { HAL_GetJoystickAxes(joystickNum, &axes); HAL_GetJoystickPOVs(joystickNum, &povs); diff --git a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp index 382656fd8a..a9f739a5a8 100644 --- a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp +++ b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp @@ -157,6 +157,8 @@ TEST_F(WebServerIntegrationTest, DriverStation) { using namespace std::chrono_literals; std::this_thread::sleep_for(1s); + HAL_RefreshDSData(); + // Compare results HAL_ControlWord cw; HAL_GetControlWord(&cw); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java index b0ffc88651..2de1a07583 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java @@ -34,5 +34,6 @@ public final class MockHardwareExtension implements BeforeAllCallback { DriverStationSim.setAutonomous(false); DriverStationSim.setEnabled(true); DriverStationSim.setTest(true); + DriverStationSim.notifyNewData(); } } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java index 1f308c2ed1..0876de1504 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java @@ -31,7 +31,6 @@ public class CommandTestBase { DriverStationSim.setEnabled(enabled); DriverStationSim.notifyNewData(); - DriverStation.isNewControlData(); while (DriverStation.isEnabled() != enabled) { try { Thread.sleep(1); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp index 4dccc0ca6d..6e27d248c0 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp @@ -19,6 +19,7 @@ CommandScheduler CommandTestBase::GetScheduler() { void CommandTestBase::SetUp() { frc::sim::DriverStationSim::SetEnabled(true); + frc::sim::DriverStationSim::NotifyNewData(); } void CommandTestBase::TearDown() { @@ -27,4 +28,5 @@ void CommandTestBase::TearDown() { void CommandTestBase::SetDSEnabled(bool enabled) { frc::sim::DriverStationSim::SetEnabled(enabled); + frc::sim::DriverStationSim::NotifyNewData(); } diff --git a/wpilibc/src/main/native/cpp/DataLogManager.cpp b/wpilibc/src/main/native/cpp/DataLogManager.cpp index c5546c42ba..b92faa387b 100644 --- a/wpilibc/src/main/native/cpp/DataLogManager.cpp +++ b/wpilibc/src/main/native/cpp/DataLogManager.cpp @@ -147,8 +147,13 @@ void Thread::Main() { m_log, "systemTime", "{\"source\":\"DataLogManager\",\"format\":\"time_t_us\"}"}; + wpi::Event newDataEvent; + DriverStation::ProvideRefreshedDataEventHandle(newDataEvent.GetHandle()); + for (;;) { - bool newData = DriverStation::WaitForData(0.25_s); + bool timedOut = false; + bool newData = + wpi::WaitForObject(newDataEvent.GetHandle(), 0.25, &timedOut); if (!m_active) { break; } @@ -236,6 +241,7 @@ void Thread::Main() { sysTimeEntry.Append(wpi::GetSystemTime(), wpi::Now()); } } + DriverStation::RemoveRefreshedDataEventHandle(newDataEvent.GetHandle()); } void Thread::StartNTLog() { diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index b351b79b0e..aead3439af 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -117,6 +118,7 @@ struct Instance { Instance(); ~Instance(); + wpi::EventVector refreshEvents; MatchDataSender matchDataSender; std::atomic dataLogSender{nullptr}; @@ -127,14 +129,6 @@ struct Instance { std::array joystickButtonsPressed; std::array joystickButtonsReleased; - // Internal Driver Station thread - std::thread dsThread; - std::atomic isRunning{false}; - - mutable wpi::mutex waitForDataMutex; - wpi::condition_variable waitForDataCond; - int waitForDataCounter = 0; - bool silenceJoystickWarning = false; // Robot state status variables @@ -154,7 +148,6 @@ static Instance& GetInstance() { return instance; } -static void Run(); static void SendMatchData(); /** @@ -185,15 +178,6 @@ static inline void ReportJoystickUnpluggedWarning(const S& format, ReportJoystickUnpluggedWarningV(format, fmt::make_format_args(args...)); } -static int& GetDSLastCount() { - // There is a rollover error condition here. At Packet# = n * (uintmax), this - // will return false when instead it should return true. However, this at a - // 20ms rate occurs once every 2.7 years of DS connected runtime, so not - // worth the cycles to check. - thread_local int lastCount{0}; - return lastCount; -} - Instance::Instance() { HAL_Initialize(500, 0); @@ -205,16 +189,9 @@ Instance::Instance() { previousButtonStates[i].count = 0; previousButtonStates[i].buttons = 0; } - - dsThread = std::thread(&Run); } Instance::~Instance() { - isRunning = false; - // Trigger a DS mutex release in case there is no driver station running. - HAL_ReleaseDSMutex(); - dsThread.join(); - if (dataLogSender) { delete dataLogSender.load(); } @@ -513,18 +490,6 @@ bool DriverStation::IsDSAttached() { return controlWord.dsAttached; } -bool DriverStation::IsNewControlData() { - auto& inst = ::GetInstance(); - std::unique_lock lock(inst.waitForDataMutex); - int& lastCount = GetDSLastCount(); - int currentCount = inst.waitForDataCounter; - if (lastCount == currentCount) { - return false; - } - lastCount = currentCount; - return true; -} - bool DriverStation::IsFMSAttached() { HAL_ControlWord controlWord; HAL_GetControlWord(&controlWord); @@ -597,36 +562,6 @@ int DriverStation::GetLocation() { } } -void DriverStation::WaitForData() { - WaitForData(0_s); -} - -bool DriverStation::WaitForData(units::second_t timeout) { - auto& inst = ::GetInstance(); - auto timeoutTime = std::chrono::steady_clock::now() + - std::chrono::steady_clock::duration{timeout}; - - std::unique_lock lock(inst.waitForDataMutex); - int& lastCount = GetDSLastCount(); - int currentCount = inst.waitForDataCounter; - if (lastCount != currentCount) { - lastCount = currentCount; - return true; - } - while (inst.waitForDataCounter == currentCount) { - if (timeout > 0_s) { - auto timedOut = inst.waitForDataCond.wait_until(lock, timeoutTime); - if (timedOut == std::cv_status::timeout) { - return false; - } - } else { - inst.waitForDataCond.wait(lock); - } - } - lastCount = inst.waitForDataCounter; - return true; -} - double DriverStation::GetMatchTime() { int32_t status = 0; return HAL_GetMatchTime(&status); @@ -640,37 +575,14 @@ double DriverStation::GetBatteryVoltage() { return voltage; } -void DriverStation::InDisabled(bool entering) { - ::GetInstance().userInDisabled = entering; -} - -void DriverStation::InAutonomous(bool entering) { - ::GetInstance().userInAutonomous = entering; -} - -void DriverStation::InTeleop(bool entering) { - ::GetInstance().userInTeleop = entering; -} - -void DriverStation::InTest(bool entering) { - ::GetInstance().userInTest = entering; -} - -void DriverStation::WakeupWaitForData() { - auto& inst = ::GetInstance(); - std::scoped_lock waitLock(inst.waitForDataMutex); - // Nofify all threads - inst.waitForDataCounter++; - inst.waitForDataCond.notify_all(); -} - /** * 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. */ -void GetData() { +void DriverStation::RefreshData() { + HAL_RefreshDSData(); auto& inst = ::GetInstance(); { // Compute the pressed and released buttons @@ -692,13 +604,24 @@ void GetData() { } } - DriverStation::WakeupWaitForData(); + inst.refreshEvents.Wakeup(); + SendMatchData(); if (auto sender = inst.dataLogSender.load()) { sender->Send(wpi::Now()); } } +void DriverStation::ProvideRefreshedDataEventHandle(WPI_EventHandle handle) { + auto& inst = ::GetInstance(); + inst.refreshEvents.Add(handle); +} + +void DriverStation::RemoveRefreshedDataEventHandle(WPI_EventHandle handle) { + auto& inst = ::GetInstance(); + inst.refreshEvents.Remove(handle); +} + void DriverStation::SilenceJoystickConnectionWarning(bool silence) { ::GetInstance().silenceJoystickWarning = silence; } @@ -747,37 +670,6 @@ void ReportJoystickUnpluggedWarningV(fmt::string_view format, } } -void Run() { - auto& inst = GetInstance(); - inst.isRunning = true; - int safetyCounter = 0; - while (inst.isRunning) { - HAL_WaitForDSData(); - GetData(); - - if (DriverStation::IsDisabled()) { - safetyCounter = 0; - } - - if (++safetyCounter >= 4) { - MotorSafety::CheckMotors(); - safetyCounter = 0; - } - if (inst.userInDisabled) { - HAL_ObserveUserProgramDisabled(); - } - if (inst.userInAutonomous) { - HAL_ObserveUserProgramAutonomous(); - } - if (inst.userInTeleop) { - HAL_ObserveUserProgramTeleop(); - } - if (inst.userInTest) { - HAL_ObserveUserProgramTest(); - } - } -} - void SendMatchData() { int32_t status = 0; HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status); diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index 3732dd0f67..d783139aa1 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -4,6 +4,8 @@ #include "frc/IterativeRobotBase.h" +#include + #include #include #include @@ -97,6 +99,7 @@ units::second_t IterativeRobotBase::GetPeriod() const { } void IterativeRobotBase::LoopFunc() { + DriverStation::RefreshData(); m_watchdog.Reset(); // Get current mode diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp index 388a887a97..0101e066f1 100644 --- a/wpilibc/src/main/native/cpp/MotorSafety.cpp +++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp @@ -7,6 +7,8 @@ #include #include +#include +#include #include #include "frc/DriverStation.h" @@ -14,26 +16,84 @@ using namespace frc; -static wpi::SmallPtrSet instanceList; -static wpi::mutex listMutex; +namespace { +class Thread : public wpi::SafeThread { + public: + Thread() {} + void Main() override; +}; + +void Thread::Main() { + wpi::Event event{true, false}; + HAL_ProvideNewDataEventHandle(event.GetHandle()); + + int safetyCounter = 0; + while (m_active) { + bool timedOut = false; + bool signaled = wpi::WaitForObject(event.GetHandle(), 0.1, &timedOut); + if (signaled) { + HAL_ControlWord controlWord; + std::memset(&controlWord, 0, sizeof(controlWord)); + HAL_GetControlWord(&controlWord); + if (!(controlWord.enabled && controlWord.dsAttached)) { + safetyCounter = 0; + } + if (++safetyCounter >= 4) { + MotorSafety::CheckMotors(); + safetyCounter = 0; + } + } else { + safetyCounter = 0; + } + } + + HAL_RemoveNewDataEventHandle(event.GetHandle()); +} + +static wpi::SafeThreadOwner m_owner; +} // namespace + +static std::atomic_bool gShutdown{false}; + +namespace { +struct MotorSafetyManager { + ~MotorSafetyManager() { gShutdown = true; } + + wpi::SmallPtrSet instanceList; + wpi::mutex listMutex; + bool threadStarted = false; +}; +} // namespace + +static MotorSafetyManager& GetManager() { + static MotorSafetyManager manager; + return manager; +} #ifndef __FRC_ROBORIO__ namespace frc::impl { void ResetMotorSafety() { - std::scoped_lock lock(listMutex); - instanceList.clear(); + auto& manager = GetManager(); + std::scoped_lock lock(manager.listMutex); + manager.instanceList.clear(); } } // namespace frc::impl #endif MotorSafety::MotorSafety() { - std::scoped_lock lock(listMutex); - instanceList.insert(this); + auto& manager = GetManager(); + std::scoped_lock lock(manager.listMutex); + manager.instanceList.insert(this); + if (!manager.threadStarted) { + manager.threadStarted = true; + m_owner.Start(); + } } MotorSafety::~MotorSafety() { - std::scoped_lock lock(listMutex); - instanceList.erase(this); + auto& manager = GetManager(); + std::scoped_lock lock(manager.listMutex); + manager.instanceList.erase(this); } MotorSafety::MotorSafety(MotorSafety&& rhs) @@ -111,8 +171,9 @@ void MotorSafety::Check() { } void MotorSafety::CheckMotors() { - std::scoped_lock lock(listMutex); - for (auto elem : instanceList) { + auto& manager = GetManager(); + std::scoped_lock lock(manager.listMutex); + for (auto elem : manager.instanceList) { elem->Check(); } } diff --git a/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp new file mode 100644 index 0000000000..7c28bad08a --- /dev/null +++ b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp @@ -0,0 +1,64 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/internal/DriverStationModeThread.h" + +#include +#include + +#include "frc/DriverStation.h" + +using namespace frc::internal; + +DriverStationModeThread::DriverStationModeThread() { + m_keepAlive = true; + m_thread = std::thread{[&] { Run(); }}; +} + +DriverStationModeThread::~DriverStationModeThread() { + m_keepAlive = false; + if (m_thread.joinable()) { + m_thread.join(); + } +} + +void DriverStationModeThread::InAutonomous(bool entering) { + m_userInAutonomous = entering; +} +void DriverStationModeThread::InDisabled(bool entering) { + m_userInDisabled = entering; +} + +void DriverStationModeThread::InTeleop(bool entering) { + m_userInTeleop = entering; +} + +void DriverStationModeThread::InTest(bool entering) { + m_userInTest = entering; +} + +void DriverStationModeThread::Run() { + wpi::Event event{true, false}; + HAL_ProvideNewDataEventHandle(event.GetHandle()); + + while (m_keepAlive.load()) { + bool timedOut = false; + wpi::WaitForObject(event.GetHandle(), 0.1, &timedOut); + frc::DriverStation::RefreshData(); + if (m_userInDisabled) { + HAL_ObserveUserProgramDisabled(); + } + if (m_userInAutonomous) { + HAL_ObserveUserProgramAutonomous(); + } + if (m_userInTeleop) { + HAL_ObserveUserProgramTeleop(); + } + if (m_userInTest) { + HAL_ObserveUserProgramTest(); + } + } + + HAL_RemoveNewDataEventHandle(event.GetHandle()); +} diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp index 5f2c6458d4..61c29492ae 100644 --- a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -154,8 +155,12 @@ void DriverStationSim::SetMatchTime(double matchTime) { } void DriverStationSim::NotifyNewData() { + wpi::Event waitEvent{true}; + HAL_ProvideNewDataEventHandle(waitEvent.GetHandle()); HALSIM_NotifyDriverStationNewData(); - DriverStation::WaitForData(); + wpi::WaitForObject(waitEvent.GetHandle()); + HAL_RemoveNewDataEventHandle(waitEvent.GetHandle()); + frc::DriverStation::RefreshData(); } void DriverStationSim::SetSendError(bool shouldSend) { diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp index 2206bff8ed..edf6291364 100644 --- a/wpilibc/src/main/native/cppcs/RobotBase.cpp +++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp @@ -204,10 +204,6 @@ bool RobotBase::IsTest() const { return DriverStation::IsTest(); } -bool RobotBase::IsNewDataAvailable() const { - return DriverStation::IsNewControlData(); -} - std::thread::id RobotBase::GetThreadId() { return m_threadId; } @@ -256,8 +252,8 @@ RobotBase::RobotBase() { } } - // Call DriverStation::InDisabled() to kick off DS thread - DriverStation::InDisabled(true); + // Call DriverStation::RefreshData() to kick things off + DriverStation::RefreshData(); // First and one-time initialization LiveWindow::SetEnabled(false); diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h index 4d54579132..4cbf6daabb 100644 --- a/wpilibc/src/main/native/include/frc/DriverStation.h +++ b/wpilibc/src/main/native/include/frc/DriverStation.h @@ -7,6 +7,7 @@ #include #include +#include namespace wpi::log { class DataLog; @@ -215,17 +216,6 @@ class DriverStation final { */ static bool IsDSAttached(); - /** - * Has a new control packet from the driver station arrived since the last - * time this function was called? - * - * Warning: If you call this function from more than one place at the same - * time, you will not get the intended behavior. - * - * @return True if the control data has been updated since the last call. - */ - static bool IsNewControlData(); - /** * Is the driver station attached to a Field Management System? * @@ -289,41 +279,6 @@ class DriverStation final { */ static int GetLocation(); - /** - * Wait until a new packet comes from the driver station. - * - * This blocks on a semaphore, so the waiting is efficient. - * - * This is a good way to delay processing until there is new driver station - * data to act on. - * - * Checks if new control data has arrived since the last waitForData call - * on the current thread. If new data has not arrived, returns immediately. - */ - static void WaitForData(); - - /** - * Wait until a new packet comes from the driver station, or wait for a - * timeout. - * - * Checks if new control data has arrived since the last waitForData call - * on the current thread. If new data has not arrived, returns immediately. - * - * If the timeout is less then or equal to 0, wait indefinitely. - * - * Timeout is in milliseconds - * - * This blocks on a semaphore, so the waiting is efficient. - * - * This is a good way to delay processing until there is new driver station - * data to act on. - * - * @param timeout Timeout - * - * @return true if new data, otherwise false - */ - static bool WaitForData(units::second_t timeout); - /** * Return the approximate match time. * @@ -348,45 +303,10 @@ class DriverStation final { */ static double GetBatteryVoltage(); - /** - * Only to be used to tell the Driver Station what code you claim to be - * executing for diagnostic purposes only. - * - * @param entering If true, starting disabled code; if false, leaving disabled - * code. - */ - static void InDisabled(bool entering); + static void RefreshData(); - /** - * Only to be used to tell the Driver Station what code you claim to be - * executing for diagnostic purposes only. - * - * @param entering If true, starting autonomous code; if false, leaving - * autonomous code. - */ - static void InAutonomous(bool entering); - - /** - * Only to be used to tell the Driver Station what code you claim to be - * executing for diagnostic purposes only. - * - * @param entering If true, starting teleop code; if false, leaving teleop - * code. - */ - static void InTeleop(bool entering); - - /** - * Only to be used to tell the Driver Station what code you claim to be - * executing for diagnostic purposes only. - * - * @param entering If true, starting test code; if false, leaving test code. - */ - static void InTest(bool entering); - - /** - * Forces WaitForData() to return immediately. - */ - static void WakeupWaitForData(); + static void ProvideRefreshedDataEventHandle(WPI_EventHandle handle); + static void RemoveRefreshedDataEventHandle(WPI_EventHandle handle); /** * Allows the user to specify whether they want joystick connection warnings diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h index a536cda2c7..5070525cb0 100644 --- a/wpilibc/src/main/native/include/frc/RobotBase.h +++ b/wpilibc/src/main/native/include/frc/RobotBase.h @@ -174,14 +174,6 @@ class RobotBase { */ bool IsTest() const; - /** - * Indicates if new data is available from the driver station. - * - * @return Has new data arrived over the network since the last time this - * function was called? - */ - bool IsNewDataAvailable() const; - /** * Gets the ID of the main robot thread. */ diff --git a/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h b/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h new file mode 100644 index 0000000000..f4fd11ca82 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +namespace frc::internal { +class DriverStationModeThread { + public: + DriverStationModeThread(); + ~DriverStationModeThread(); + + DriverStationModeThread(const DriverStationModeThread& other) = delete; + DriverStationModeThread(DriverStationModeThread&& other) = delete; + DriverStationModeThread& operator=(const DriverStationModeThread& other) = + delete; + DriverStationModeThread& operator=(DriverStationModeThread&& other) = delete; + + void InAutonomous(bool entering); + void InDisabled(bool entering); + void InTeleop(bool entering); + void InTest(bool entering); + + private: + std::atomic_bool m_keepAlive{false}; + std::thread m_thread; + void Run(); + bool m_userInDisabled{false}; + bool m_userInAutonomous{false}; + bool m_userInTeleop{false}; + bool m_userInTest{false}; +}; +} // namespace frc::internal diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp index ea9656bb01..4ccce1b92d 100644 --- a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp +++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp @@ -418,6 +418,7 @@ TEST_F(TimedRobotTest, ModeChange) { frc::sim::DriverStationSim::SetEnabled(true); frc::sim::DriverStationSim::SetAutonomous(true); frc::sim::DriverStationSim::SetTest(false); + frc::sim::DriverStationSim::NotifyNewData(); frc::sim::StepTiming(20_ms); @@ -435,6 +436,7 @@ TEST_F(TimedRobotTest, ModeChange) { frc::sim::DriverStationSim::SetEnabled(true); frc::sim::DriverStationSim::SetAutonomous(false); frc::sim::DriverStationSim::SetTest(false); + frc::sim::DriverStationSim::NotifyNewData(); frc::sim::StepTiming(20_ms); @@ -452,6 +454,7 @@ TEST_F(TimedRobotTest, ModeChange) { frc::sim::DriverStationSim::SetEnabled(true); frc::sim::DriverStationSim::SetAutonomous(false); frc::sim::DriverStationSim::SetTest(true); + frc::sim::DriverStationSim::NotifyNewData(); frc::sim::StepTiming(20_ms); @@ -469,6 +472,7 @@ TEST_F(TimedRobotTest, ModeChange) { frc::sim::DriverStationSim::SetEnabled(false); frc::sim::DriverStationSim::SetAutonomous(false); frc::sim::DriverStationSim::SetTest(false); + frc::sim::DriverStationSim::NotifyNewData(); frc::sim::StepTiming(20_ms); diff --git a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp index d01ca27937..6aa31c6805 100644 --- a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp @@ -132,6 +132,7 @@ TEST(DriverStationTest, AllianceStationId) { // B1 allianceStation = HAL_AllianceStationID_kBlue1; DriverStationSim::SetAllianceStationId(allianceStation); + frc::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId()); EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance()); EXPECT_EQ(1, DriverStation::GetLocation()); @@ -141,6 +142,7 @@ TEST(DriverStationTest, AllianceStationId) { // B2 allianceStation = HAL_AllianceStationID_kBlue2; DriverStationSim::SetAllianceStationId(allianceStation); + frc::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId()); EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance()); EXPECT_EQ(2, DriverStation::GetLocation()); @@ -150,6 +152,7 @@ TEST(DriverStationTest, AllianceStationId) { // B3 allianceStation = HAL_AllianceStationID_kBlue3; DriverStationSim::SetAllianceStationId(allianceStation); + frc::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId()); EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance()); EXPECT_EQ(3, DriverStation::GetLocation()); @@ -159,6 +162,7 @@ TEST(DriverStationTest, AllianceStationId) { // R1 allianceStation = HAL_AllianceStationID_kRed1; DriverStationSim::SetAllianceStationId(allianceStation); + frc::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId()); EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance()); EXPECT_EQ(1, DriverStation::GetLocation()); @@ -168,6 +172,7 @@ TEST(DriverStationTest, AllianceStationId) { // R2 allianceStation = HAL_AllianceStationID_kRed2; DriverStationSim::SetAllianceStationId(allianceStation); + frc::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId()); EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance()); EXPECT_EQ(2, DriverStation::GetLocation()); @@ -177,6 +182,7 @@ TEST(DriverStationTest, AllianceStationId) { // R3 allianceStation = HAL_AllianceStationID_kRed3; DriverStationSim::SetAllianceStationId(allianceStation); + frc::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId()); EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance()); EXPECT_EQ(3, DriverStation::GetLocation()); @@ -211,6 +217,7 @@ TEST(DriverStationTest, MatchTime) { false); constexpr double kTestTime = 19.174; DriverStationSim::SetMatchTime(kTestTime); + frc::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(kTestTime, DriverStationSim::GetMatchTime()); EXPECT_EQ(kTestTime, DriverStation::GetMatchTime()); EXPECT_TRUE(callback.WasTriggered()); diff --git a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c index 0db402bf76..b0905f9d6e 100644 --- a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c +++ b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c @@ -87,13 +87,18 @@ int main(void) { return 1; } + WPI_EventHandle eventHandle = WPI_CreateEvent(1, 0); + HAL_ProvideNewDataEventHandle(eventHandle); + while (1) { // Wait for DS data, with a timeout - HAL_Bool validData = HAL_WaitForDSDataTimeout(1.0); - if (!validData) { + int timed_out = 0; + int signaled = WPI_WaitForObjectTimeout(eventHandle, 1.0, &timed_out); + if (!signaled) { // Do something here on no packet continue; } + enum DriverStationMode dsMode = getDSMode(); switch (dsMode) { case DisabledMode: @@ -116,6 +121,9 @@ int main(void) { } // Clean up resources + HAL_RemoveNewDataEventHandle(eventHandle); + WPI_DestroyEvent(eventHandle); + status = 0; HAL_FreeDIOPort(dio); HAL_FreePWMPort(pwmPort, &status); diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp index 29bae0bf1d..a521b47257 100644 --- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp @@ -5,6 +5,7 @@ #include "Robot.h" #include +#include #include #include #include @@ -23,41 +24,46 @@ void Robot::Test() {} void Robot::StartCompetition() { RobotInit(); + frc::internal::DriverStationModeThread modeThread; + + wpi::Event event{true, false}; + frc::DriverStation::ProvideRefreshedDataEventHandle(event.GetHandle()); + // Tell the DS that the robot is ready to be enabled HAL_ObserveUserProgramStarting(); while (!m_exit) { if (IsDisabled()) { - frc::DriverStation::InDisabled(true); + modeThread.InDisabled(true); Disabled(); - frc::DriverStation::InDisabled(false); + modeThread.InDisabled(false); while (IsDisabled()) { - frc::DriverStation::WaitForData(); + wpi::WaitForObject(event.GetHandle()); } } else if (IsAutonomous()) { - frc::DriverStation::InAutonomous(true); + modeThread.InAutonomous(true); Autonomous(); - frc::DriverStation::InAutonomous(false); + modeThread.InAutonomous(false); while (IsAutonomousEnabled()) { - frc::DriverStation::WaitForData(); + wpi::WaitForObject(event.GetHandle()); } } else if (IsTest()) { frc::LiveWindow::SetEnabled(true); frc::Shuffleboard::EnableActuatorWidgets(); - frc::DriverStation::InTest(true); + modeThread.InTest(true); Test(); - frc::DriverStation::InTest(false); + modeThread.InTest(false); while (IsTest() && IsEnabled()) { - frc::DriverStation::WaitForData(); + wpi::WaitForObject(event.GetHandle()); } frc::LiveWindow::SetEnabled(false); frc::Shuffleboard::DisableActuatorWidgets(); } else { - frc::DriverStation::InTeleop(true); + modeThread.InTeleop(true); Teleop(); - frc::DriverStation::InTeleop(false); + modeThread.InTeleop(false); while (IsTeleopEnabled()) { - frc::DriverStation::WaitForData(); + wpi::WaitForObject(event.GetHandle()); } } } diff --git a/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp index e4a2721a48..0369717533 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp @@ -4,6 +4,7 @@ #include "frc/DriverStation.h" // NOLINT(build/include_order) +#include #include #include @@ -20,11 +21,16 @@ TEST(DriverStationTest, WaitForData) { units::microsecond_t initialTime(frc::RobotController::GetFPGATime()); + wpi::Event waitEvent{true}; + HAL_ProvideNewDataEventHandle(waitEvent.GetHandle()); + // 20ms waiting intervals * 50 = 1s for (int i = 0; i < 50; i++) { - frc::DriverStation::WaitForData(); + wpi::WaitForObject(waitEvent.GetHandle()); } + HAL_RemoveNewDataEventHandle(waitEvent.GetHandle()); + units::microsecond_t finalTime(frc::RobotController::GetFPGATime()); EXPECT_NEAR_UNITS(1_s, finalTime - initialTime, 200_ms); diff --git a/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp b/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp index 1f5e3602fc..59a3a58443 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp @@ -45,6 +45,7 @@ class TestEnvironment : public testing::Environment { fmt::print("Started coms\n"); int enableCounter = 0; + frc::DriverStation::RefreshData(); while (!frc::DriverStation::IsEnabled()) { if (enableCounter > 50) { // Robot did not enable properly after 5 seconds. @@ -56,6 +57,7 @@ class TestEnvironment : public testing::Environment { std::this_thread::sleep_for(100ms); fmt::print("Waiting for enable: {}\n", enableCounter++); + frc::DriverStation::RefreshData(); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java index 37e046df0f..8837d4167a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java @@ -16,12 +16,12 @@ public class DSControlWord { *

Upon construction, the current Driver Station control word is read and stored internally. */ public DSControlWord() { - update(); + refresh(); } /** Update internal Driver Station control word. */ - public void update() { - DriverStation.updateControlWordFromCache(m_controlWord); + public void refresh() { + DriverStation.refreshControlWordFromCache(m_controlWord); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java index 42d6be0d41..927c14d16a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java @@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.util.concurrent.Event; import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.IntegerLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; @@ -279,12 +280,19 @@ public final class DataLogManager { new IntegerLogEntry( m_log, "systemTime", "{\"source\":\"DataLogManager\",\"format\":\"time_t_us\"}"); + Event newDataEvent = new Event(); + DriverStation.provideRefreshedDataEventHandle(newDataEvent.getHandle()); while (!Thread.interrupted()) { - boolean newData = DriverStation.waitForData(0.25); + boolean timedOut; + try { + timedOut = WPIUtilJNI.waitForObjectTimeout(newDataEvent.getHandle(), 0.25); + } catch (InterruptedException e) { + break; + } if (Thread.interrupted()) { break; } - if (!newData) { + if (timedOut) { timeoutCount++; // pause logging after being disconnected for 10 seconds if (timeoutCount > 40 && !paused) { @@ -371,5 +379,6 @@ public final class DataLogManager { sysTimeEntry.append(WPIUtilJNI.getSystemTime(), WPIUtilJNI.now()); } } + newDataEvent.close(); } } 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 ce8a76c88a..139e2edc27 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.hal.ControlWord; +import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.MatchInfoData; import edu.wpi.first.networktables.BooleanPublisher; @@ -13,6 +14,7 @@ import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.util.EventVector; import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.util.datalog.BooleanArrayLogEntry; import edu.wpi.first.util.datalog.BooleanLogEntry; @@ -20,9 +22,6 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.FloatArrayLogEntry; import edu.wpi.first.util.datalog.IntegerArrayLogEntry; import java.nio.ByteBuffer; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.locks.Condition; -import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; /** Provide access to the network communication data to / from the Driver Station. */ @@ -37,16 +36,25 @@ public final class DriverStation { private static class HALJoystickAxes { public float[] m_axes; - public short m_count; + public int m_count; HALJoystickAxes(int count) { m_axes = new float[count]; } } + private static class HALJoystickAxesRaw { + public int[] m_axes; + public int m_count; + + HALJoystickAxesRaw(int count) { + m_axes = new int[count]; + } + } + private static class HALJoystickPOVs { public short[] m_povs; - public short m_count; + public int m_count; HALJoystickPOVs(int count) { m_povs = new short[count]; @@ -73,15 +81,6 @@ public final class DriverStation { private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0; private static double m_nextMessageTime; - private static class DriverStationTask implements Runnable { - DriverStationTask() {} - - @Override - public void run() { - DriverStation.run(); - } - } /* DriverStationTask */ - @SuppressWarnings("MemberName") private static class MatchDataSender { NetworkTable table; @@ -126,7 +125,7 @@ public final class DriverStation { } private void sendMatchData() { - AllianceStationID allianceID = HAL.getAllianceStation(); + AllianceStationID allianceID = DriverStationJNI.getAllianceStation(); boolean isRedAlliance = false; int stationNumber = 1; switch (allianceID) { @@ -172,7 +171,7 @@ public final class DriverStation { } finally { m_cacheDataMutex.unlock(); } - currentControlWord = HAL.nativeGetControlWord(); + currentControlWord = DriverStationJNI.nativeGetControlWord(); if (oldIsRedAlliance != isRedAlliance) { alliance.set(isRedAlliance); @@ -368,16 +367,22 @@ public final class DriverStation { // Joystick User Data private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts]; + private static HALJoystickAxesRaw[] m_joystickAxesRaw = new HALJoystickAxesRaw[kJoystickPorts]; private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts]; private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts]; private static MatchInfoData m_matchInfo = new MatchInfoData(); + private static ControlWord m_controlWord = new ControlWord(); + private static EventVector m_refreshEvents = new EventVector(); // Joystick Cached Data private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts]; + private static HALJoystickAxesRaw[] m_joystickAxesRawCache = + new HALJoystickAxesRaw[kJoystickPorts]; private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts]; private static HALJoystickButtons[] m_joystickButtonsCache = new HALJoystickButtons[kJoystickPorts]; private static MatchInfoData m_matchInfoCache = new MatchInfoData(); + private static ControlWord m_controlWordCache = new ControlWord(); // Joystick button rising/falling edge flags private static int[] m_joystickButtonsPressed = new int[kJoystickPorts]; @@ -389,31 +394,10 @@ public final class DriverStation { private static final MatchDataSender m_matchDataSender; private static DataLogSender m_dataLogSender; - // Internal Driver Station thread - private static Thread m_thread; - - private static volatile boolean m_threadKeepAlive = true; - private static final ReentrantLock m_cacheDataMutex = new ReentrantLock(); - private static final Lock m_waitForDataMutex; - private static final Condition m_waitForDataCond; - private static int m_waitForDataCount; - private static final ThreadLocal m_lastCount = ThreadLocal.withInitial(() -> 0); - private static boolean m_silenceJoystickWarning; - // Robot state status variables - private static boolean m_userInDisabled; - private static boolean m_userInAutonomous; - private static boolean m_userInTeleop; - private static boolean m_userInTest; - - // Control word variables - private static final ReentrantLock m_controlWordMutex = new ReentrantLock(); - private static final ControlWord m_controlWordCache; - private static long m_lastControlWordUpdate; - /** * DriverStation constructor. * @@ -424,42 +408,20 @@ public final class DriverStation { static { HAL.initialize(500, 0); - m_waitForDataCount = 0; - m_waitForDataMutex = new ReentrantLock(); - m_waitForDataCond = m_waitForDataMutex.newCondition(); for (int i = 0; i < kJoystickPorts; i++) { m_joystickButtons[i] = new HALJoystickButtons(); - m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes); - m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs); + m_joystickAxes[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes); + m_joystickAxesRaw[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes); + m_joystickPOVs[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs); m_joystickButtonsCache[i] = new HALJoystickButtons(); - m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes); - m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs); + m_joystickAxesCache[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes); + m_joystickAxesRawCache[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes); + m_joystickPOVsCache[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs); } - m_controlWordCache = new ControlWord(); - m_lastControlWordUpdate = 0; - m_matchDataSender = new MatchDataSender(); - - m_thread = new Thread(new DriverStationTask(), "FRCDriverStation"); - m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2); - - m_thread.start(); - } - - /** Kill the thread. */ - public static synchronized void release() { - m_threadKeepAlive = false; - if (m_thread != null) { - try { - m_thread.join(); - } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); - } - m_thread = null; - } } /** @@ -537,7 +499,8 @@ public final class DriverStation { } } } - HAL.sendError(isError, code, false, error, locString, traceString.toString(), true); + DriverStationJNI.sendError( + isError, code, false, error, locString, traceString.toString(), true); } /** @@ -666,7 +629,7 @@ public final class DriverStation { if (stick < 0 || stick >= kJoystickPorts) { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - if (axis < 0 || axis >= HAL.kMaxJoystickAxes) { + if (axis < 0 || axis >= DriverStationJNI.kMaxJoystickAxes) { throw new IllegalArgumentException("Joystick axis is out of range"); } @@ -699,7 +662,7 @@ public final class DriverStation { if (stick < 0 || stick >= kJoystickPorts) { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) { + if (pov < 0 || pov >= DriverStationJNI.kMaxJoystickPOVs) { throw new IllegalArgumentException("Joystick POV is out of range"); } @@ -760,10 +723,10 @@ public final class DriverStation { } /** - * Returns the number of POVs on a given joystick port. + * Returns the number of povs on a given joystick port. * * @param stick The joystick port number - * @return The number of POVs on the indicated joystick + * @return The number of povs on the indicated joystick */ public static int getStickPOVCount(int stick) { if (stick < 0 || stick >= kJoystickPorts) { @@ -808,7 +771,7 @@ public final class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - return HAL.getJoystickIsXbox((byte) stick) == 1; + return DriverStationJNI.getJoystickIsXbox((byte) stick) == 1; } /** @@ -822,7 +785,7 @@ public final class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - return HAL.getJoystickType((byte) stick); + return DriverStationJNI.getJoystickType((byte) stick); } /** @@ -836,7 +799,7 @@ public final class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - return HAL.getJoystickName((byte) stick); + return DriverStationJNI.getJoystickName((byte) stick); } /** @@ -851,7 +814,7 @@ public final class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - return HAL.getJoystickAxisType((byte) stick, (byte) axis); + return DriverStationJNI.getJoystickAxisType((byte) stick, (byte) axis); } /** @@ -875,12 +838,11 @@ public final class DriverStation { * @return True if the robot is enabled, false otherwise. */ public static boolean isEnabled() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached(); + return m_controlWord.getEnabled() && m_controlWord.getDSAttached(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } @@ -899,12 +861,11 @@ public final class DriverStation { * @return True if the robot is e-stopped, false otherwise. */ public static boolean isEStopped() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return m_controlWordCache.getEStop(); + return m_controlWord.getEStop(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } @@ -915,12 +876,11 @@ public final class DriverStation { * @return True if autonomous mode should be enabled, false otherwise. */ public static boolean isAutonomous() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return m_controlWordCache.getAutonomous(); + return m_controlWord.getAutonomous(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } @@ -931,12 +891,11 @@ public final class DriverStation { * @return True if autonomous should be set and the robot should be enabled. */ public static boolean isAutonomousEnabled() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled(); + return m_controlWord.getAutonomous() && m_controlWord.getEnabled(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } @@ -957,14 +916,13 @@ public final class DriverStation { * @return True if operator-controlled mode should be set and the robot should be enabled. */ public static boolean isTeleopEnabled() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return !m_controlWordCache.getAutonomous() - && !m_controlWordCache.getTest() - && m_controlWordCache.getEnabled(); + return !m_controlWord.getAutonomous() + && !m_controlWord.getTest() + && m_controlWord.getEnabled(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } @@ -975,12 +933,11 @@ public final class DriverStation { * @return True if test mode should be enabled, false otherwise. */ public static boolean isTest() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return m_controlWordCache.getTest(); + return m_controlWord.getTest(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } @@ -990,47 +947,25 @@ public final class DriverStation { * @return True if Driver Station is attached, false otherwise. */ public static boolean isDSAttached() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return m_controlWordCache.getDSAttached(); + return m_controlWord.getDSAttached(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } - /** - * Gets if a new control packet from the driver station arrived since the last time this function - * was called. - * - * @return True if the control data has been updated since the last call. - */ - public static boolean isNewControlData() { - m_waitForDataMutex.lock(); - try { - int currentCount = m_waitForDataCount; - if (m_lastCount.get() != currentCount) { - m_lastCount.set(currentCount); - return true; - } - } finally { - m_waitForDataMutex.unlock(); - } - return false; - } - /** * Gets if the driver station attached to a Field Management System. * * @return true if the robot is competing on a field being controlled by a Field Management System */ public static boolean isFMSAttached() { - m_controlWordMutex.lock(); + m_cacheDataMutex.lock(); try { - updateControlWord(false); - return m_controlWordCache.getFMSAttached(); + return m_controlWord.getFMSAttached(); } finally { - m_controlWordMutex.unlock(); + m_cacheDataMutex.unlock(); } } @@ -1121,7 +1056,7 @@ public final class DriverStation { * @return the current alliance */ public static Alliance getAlliance() { - AllianceStationID allianceStationID = HAL.getAllianceStation(); + AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation(); if (allianceStationID == null) { return Alliance.Invalid; } @@ -1148,7 +1083,7 @@ public final class DriverStation { * @return the location of the team's driver station controls: 1, 2, or 3 */ public static int getLocation() { - AllianceStationID allianceStationID = HAL.getAllianceStation(); + AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation(); if (allianceStationID == null) { return 0; } @@ -1170,68 +1105,6 @@ public final class DriverStation { } } - /** - * Wait for new data from the driver station. - * - *

Checks if new control data has arrived since the last waitForData call on the current - * thread. If new data has not arrived, returns immediately. - */ - public static void waitForData() { - waitForData(0); - } - - /** - * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data - * only. - * - *

Checks if new control data has arrived since the last waitForData call on the current - * thread. If new data has not arrived, returns immediately. - * - * @param timeoutSeconds The maximum time in seconds to wait. - * @return true if there is new data, otherwise false - */ - public static boolean waitForData(double timeoutSeconds) { - long startTimeMicroS = RobotController.getFPGATime(); - long timeoutMicroS = (long) (timeoutSeconds * 1e6); - m_waitForDataMutex.lock(); - try { - int currentCount = m_waitForDataCount; - if (m_lastCount.get() != currentCount) { - m_lastCount.set(currentCount); - return true; - } - while (m_waitForDataCount == currentCount) { - if (timeoutMicroS > 0) { - long nowMicroS = RobotController.getFPGATime(); - if (nowMicroS < startTimeMicroS + timeoutMicroS) { - // We still have time to wait - boolean signaled = - m_waitForDataCond.await( - startTimeMicroS + timeoutMicroS - nowMicroS, TimeUnit.MICROSECONDS); - if (!signaled) { - // Return false if a timeout happened - return false; - } - } else { - // Time has elapsed. - return false; - } - } else { - m_waitForDataCond.await(); - } - } - m_lastCount.set(m_waitForDataCount); - // Return true if we have received a proper signal - return true; - } catch (InterruptedException ex) { - // return false on a thread interrupt - Thread.currentThread().interrupt(); - return false; - } finally { - m_waitForDataMutex.unlock(); - } - } - /** * Return the approximate match time. The FMS does not send an official match time to the robots, * but does send an approximate match time. The value will count down the time remaining in the @@ -1242,58 +1115,7 @@ public final class DriverStation { * @return Time remaining in current match period (auto or teleop) in seconds */ public static double getMatchTime() { - return HAL.getMatchTime(); - } - - /** - * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic - * purposes only. - * - * @param entering If true, starting disabled code; if false, leaving disabled code - */ - public static void inDisabled(boolean entering) { - m_userInDisabled = entering; - } - - /** - * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic - * purposes only. - * - * @param entering If true, starting autonomous code; if false, leaving autonomous code - */ - public static void inAutonomous(boolean entering) { - m_userInAutonomous = entering; - } - - /** - * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic - * purposes only. - * - * @param entering If true, starting teleop code; if false, leaving teleop code - */ - public static void inTeleop(boolean entering) { - m_userInTeleop = entering; - } - - /** - * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic - * purposes only. - * - * @param entering If true, starting test code; if false, leaving test code - */ - public static void inTest(boolean entering) { - m_userInTest = entering; - } - - /** Forces waitForData() to return immediately. */ - public static void wakeupWaitForData() { - m_waitForDataMutex.lock(); - try { - m_waitForDataCount++; - m_waitForDataCond.signalAll(); - } finally { - m_waitForDataMutex.unlock(); - } + return DriverStationJNI.getMatchTime(); } /** @@ -1317,30 +1139,43 @@ public final class DriverStation { return !isFMSAttached() && m_silenceJoystickWarning; } + /** + * Refresh the passed in control word to contain the current control word cache. + * + * @param word Word to refresh. + */ + public static void refreshControlWordFromCache(ControlWord word) { + m_cacheDataMutex.lock(); + try { + word.update(m_controlWord); + } finally { + m_cacheDataMutex.unlock(); + } + } + /** * 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. */ - protected static void getData() { + public static void refreshData() { + DriverStationJNI.refreshDSData(); + // Get the status of all of the joysticks for (byte stick = 0; stick < kJoystickPorts; stick++) { m_joystickAxesCache[stick].m_count = - HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes); + DriverStationJNI.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes); + m_joystickAxesRawCache[stick].m_count = + DriverStationJNI.getJoystickAxesRaw(stick, m_joystickAxesRawCache[stick].m_axes); m_joystickPOVsCache[stick].m_count = - HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs); - m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer); + DriverStationJNI.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs); + m_joystickButtonsCache[stick].m_buttons = + DriverStationJNI.getJoystickButtons(stick, m_buttonCountBuffer); m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0); } - HAL.getMatchInfo(m_matchInfoCache); + DriverStationJNI.getMatchInfo(m_matchInfoCache); - m_controlWordMutex.lock(); - try { - // Force a control word update, to make sure the data is the newest. - updateControlWord(true); - } finally { - m_controlWordMutex.unlock(); - } + DriverStationJNI.getControlWord(m_controlWordCache); DataLogSender dataLogSender; // lock joystick mutex to swap cache data @@ -1361,6 +1196,10 @@ public final class DriverStation { m_joystickAxes = m_joystickAxesCache; m_joystickAxesCache = currentAxes; + HALJoystickAxesRaw[] currentAxesRaw = m_joystickAxesRaw; + m_joystickAxesRaw = m_joystickAxesRawCache; + m_joystickAxesRawCache = currentAxesRaw; + HALJoystickButtons[] currentButtons = m_joystickButtons; m_joystickButtons = m_joystickButtonsCache; m_joystickButtonsCache = currentButtons; @@ -1373,18 +1212,31 @@ public final class DriverStation { m_matchInfo = m_matchInfoCache; m_matchInfoCache = currentInfo; + ControlWord currentWord = m_controlWord; + m_controlWord = m_controlWordCache; + m_controlWordCache = currentWord; + dataLogSender = m_dataLogSender; } finally { m_cacheDataMutex.unlock(); } - wakeupWaitForData(); + m_refreshEvents.wakeup(); + m_matchDataSender.sendMatchData(); if (dataLogSender != null) { dataLogSender.send(WPIUtilJNI.now()); } } + public static void provideRefreshedDataEventHandle(int handle) { + m_refreshEvents.add(handle); + } + + public static void removeRefreshedDataEventHandle(int handle) { + m_refreshEvents.remove(handle); + } + /** * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm * the DS. @@ -1411,68 +1263,6 @@ public final class DriverStation { } } - /** Provides the service routine for the DS polling m_thread. */ - private static void run() { - int safetyCounter = 0; - while (m_threadKeepAlive) { - HAL.waitForDSData(); - getData(); - - if (isDisabled()) { - safetyCounter = 0; - } - - safetyCounter++; - if (safetyCounter >= 4) { - MotorSafety.checkMotors(); - safetyCounter = 0; - } - if (m_userInDisabled) { - HAL.observeUserProgramDisabled(); - } - if (m_userInAutonomous) { - HAL.observeUserProgramAutonomous(); - } - if (m_userInTeleop) { - HAL.observeUserProgramTeleop(); - } - if (m_userInTest) { - HAL.observeUserProgramTest(); - } - } - } - - /** - * Forces a control word cache update, and update the passed in control word. - * - * @param word Word to update. - */ - public static void updateControlWordFromCache(ControlWord word) { - m_controlWordMutex.lock(); - try { - updateControlWord(true); - word.update(m_controlWordCache); - } finally { - m_controlWordMutex.unlock(); - } - } - - /** - * Updates the data in the control word cache. Updates if the force parameter is set, or if 50ms - * have passed since the last update. - * - *

Must be called with m_controlWordMutex lock held. - * - * @param force True to force an update to the cache, otherwise update if 50ms have passed. - */ - private static void updateControlWord(boolean force) { - long now = System.currentTimeMillis(); - if (now - m_lastControlWordUpdate > 50 || force) { - HAL.getControlWord(m_controlWordCache); - m_lastControlWordUpdate = now; - } - } - /** * Starts logging DriverStation data to data log. Repeated calls are ignored. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java index 5650954a4a..f05bfb6de7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java @@ -4,7 +4,7 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj.event.EventLoop; import java.util.HashMap; @@ -248,7 +248,7 @@ public class GenericHID { */ public void setOutput(int outputNumber, boolean value) { m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1)); - HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); + DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); } /** @@ -258,7 +258,7 @@ public class GenericHID { */ public void setOutputs(int value) { m_outputs = value; - HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); + DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); } /** @@ -279,6 +279,6 @@ public class GenericHID { } else { m_rightRumble = (short) (value * 65535); } - HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); + DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index 19f81e15b5..1115130a61 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj; +import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.hal.HAL; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -253,10 +254,12 @@ public abstract class IterativeRobotBase extends RobotBase { } protected void loopFunc() { + DriverStation.refreshData(); m_watchdog.reset(); + m_word.refresh(); + // Get current mode - m_word.update(); Mode mode = Mode.kNone; if (m_word.isDisabled()) { mode = Mode.kDisabled; @@ -305,19 +308,19 @@ public abstract class IterativeRobotBase extends RobotBase { // Call the appropriate function depending upon the current robot mode if (mode == Mode.kDisabled) { - HAL.observeUserProgramDisabled(); + DriverStationJNI.observeUserProgramDisabled(); disabledPeriodic(); m_watchdog.addEpoch("disabledPeriodic()"); } else if (mode == Mode.kAutonomous) { - HAL.observeUserProgramAutonomous(); + DriverStationJNI.observeUserProgramAutonomous(); autonomousPeriodic(); m_watchdog.addEpoch("autonomousPeriodic()"); } else if (mode == Mode.kTeleop) { - HAL.observeUserProgramTeleop(); + DriverStationJNI.observeUserProgramTeleop(); teleopPeriodic(); m_watchdog.addEpoch("teleopPeriodic()"); } else { - HAL.observeUserProgramTest(); + DriverStationJNI.observeUserProgramTest(); testPeriodic(); m_watchdog.addEpoch("testPeriodic()"); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java index 5fb10b4ff8..a6b50b1ec7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java @@ -29,6 +29,7 @@ public abstract class MotorSafety { public MotorSafety() { synchronized (m_listMutex) { m_instanceList.add(this); + // TODO Threads } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index 07b0806248..1bf859366f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java @@ -274,15 +274,6 @@ public abstract class RobotBase implements AutoCloseable { return DriverStation.isTeleopEnabled(); } - /** - * Indicates if new data is available from the driver station. - * - * @return Has new data arrived over the network since the last time this function was called? - */ - public boolean isNewDataAvailable() { - return DriverStation.isNewControlData(); - } - /** Provide an alternate "main loop" via startCompetition(). */ public abstract void startCompetition(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 972df950ed..5a53b145f0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj; +import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; @@ -110,7 +111,7 @@ public class TimedRobot extends IterativeRobotBase { // Tell the DS that the robot is ready to be enabled System.out.println("********** Robot program startup complete **********"); - HAL.observeUserProgramStarting(); + DriverStationJNI.observeUserProgramStarting(); // Loop forever, calling the appropriate mode-dependent function while (true) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java new file mode 100644 index 0000000000..ca71c9f6c2 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java @@ -0,0 +1,112 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.internal; + +import edu.wpi.first.hal.DriverStationJNI; +import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.concurrent.atomic.AtomicBoolean; + +/* + * For internal use only. + */ +public class DriverStationModeThread implements AutoCloseable { + private final AtomicBoolean m_keepAlive = new AtomicBoolean(); + private final Thread m_thread; + + private boolean m_userInDisabled; + private boolean m_userInAutonomous; + private boolean m_userInTeleop; + private boolean m_userInTest; + + /** Internal use only. */ + public DriverStationModeThread() { + m_keepAlive.set(true); + m_thread = new Thread(this::run, "DriverStationMode"); + m_thread.start(); + } + + private void run() { + int handle = WPIUtilJNI.createEvent(true, false); + DriverStationJNI.provideNewDataEventHandle(handle); + + while (m_keepAlive.get()) { + try { + WPIUtilJNI.waitForObjectTimeout(handle, 0.1); + } catch (InterruptedException e) { + DriverStationJNI.removeNewDataEventHandle(handle); + WPIUtilJNI.destroyEvent(handle); + Thread.currentThread().interrupt(); + return; + } + DriverStation.refreshData(); + if (m_userInDisabled) { + DriverStationJNI.observeUserProgramDisabled(); + } + if (m_userInAutonomous) { + DriverStationJNI.observeUserProgramAutonomous(); + } + if (m_userInTeleop) { + DriverStationJNI.observeUserProgramTeleop(); + } + if (m_userInTest) { + DriverStationJNI.observeUserProgramTest(); + } + } + + DriverStationJNI.removeNewDataEventHandle(handle); + WPIUtilJNI.destroyEvent(handle); + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting disabled code; if false, leaving disabled code + */ + public void inDisabled(boolean entering) { + m_userInDisabled = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting autonomous code; if false, leaving autonomous code + */ + public void inAutonomous(boolean entering) { + m_userInAutonomous = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting teleop code; if false, leaving teleop code + */ + public void inTeleop(boolean entering) { + m_userInTeleop = entering; + } + + /** + * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic + * purposes only. + * + * @param entering If true, starting test code; if false, leaving test code + */ + public void inTest(boolean entering) { + m_userInTest = entering; + } + + @Override + public void close() { + m_keepAlive.set(false); + try { + m_thread.join(); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java index aa0bf84d66..a429e71c6f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java @@ -5,8 +5,10 @@ package edu.wpi.first.wpilibj.simulation; import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.hal.simulation.DriverStationDataJNI; import edu.wpi.first.hal.simulation.NotifyCallback; +import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.DriverStation; /** Class to control a simulated driver station. */ @@ -310,8 +312,17 @@ public final class DriverStationSim { /** Updates DriverStation data so that new values are visible to the user program. */ public static void notifyNewData() { + int handle = WPIUtilJNI.createEvent(false, false); + DriverStationJNI.provideNewDataEventHandle(handle); DriverStationDataJNI.notifyNewData(); - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(handle); + } catch (InterruptedException e) { + e.printStackTrace(); + } + DriverStationJNI.removeNewDataEventHandle(handle); + WPIUtilJNI.destroyEvent(handle); + DriverStation.refreshData(); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java index 855ace54e2..b2b22a2946 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java @@ -7,7 +7,7 @@ package edu.wpi.first.wpilibj.hal; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.hal.MatchInfoData; import edu.wpi.first.hal.simulation.DriverStationDataJNI; import edu.wpi.first.wpilibj.DriverStation.MatchType; @@ -20,7 +20,7 @@ class MatchInfoDataTest { DriverStationDataJNI.setMatchInfo("Event Name", "Game Message", 174, 191, matchType.ordinal()); MatchInfoData outMatchInfo = new MatchInfoData(); - HAL.getMatchInfo(outMatchInfo); + DriverStationJNI.getMatchInfo(outMatchInfo); assertAll( () -> assertEquals("Event Name", outMatchInfo.eventName), diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java index fb0a058f37..5bbf9aae57 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java @@ -137,6 +137,7 @@ class DriverStationSimTest { // B1 allianceStation = AllianceStationID.Blue1; DriverStationSim.setAllianceStationId(allianceStation); + DriverStationSim.notifyNewData(); assertEquals(allianceStation, DriverStationSim.getAllianceStationId()); assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance()); assertEquals(1, DriverStation.getLocation()); @@ -146,6 +147,7 @@ class DriverStationSimTest { // B2 allianceStation = AllianceStationID.Blue2; DriverStationSim.setAllianceStationId(allianceStation); + DriverStationSim.notifyNewData(); assertEquals(allianceStation, DriverStationSim.getAllianceStationId()); assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance()); assertEquals(2, DriverStation.getLocation()); @@ -155,6 +157,7 @@ class DriverStationSimTest { // B3 allianceStation = AllianceStationID.Blue3; DriverStationSim.setAllianceStationId(allianceStation); + DriverStationSim.notifyNewData(); assertEquals(allianceStation, DriverStationSim.getAllianceStationId()); assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance()); assertEquals(3, DriverStation.getLocation()); @@ -164,6 +167,7 @@ class DriverStationSimTest { // R1 allianceStation = AllianceStationID.Red1; DriverStationSim.setAllianceStationId(allianceStation); + DriverStationSim.notifyNewData(); assertEquals(allianceStation, DriverStationSim.getAllianceStationId()); assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance()); assertEquals(1, DriverStation.getLocation()); @@ -173,6 +177,7 @@ class DriverStationSimTest { // R2 allianceStation = AllianceStationID.Red2; DriverStationSim.setAllianceStationId(allianceStation); + DriverStationSim.notifyNewData(); assertEquals(allianceStation, DriverStationSim.getAllianceStationId()); assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance()); assertEquals(2, DriverStation.getLocation()); @@ -182,6 +187,7 @@ class DriverStationSimTest { // R3 allianceStation = AllianceStationID.Red3; DriverStationSim.setAllianceStationId(allianceStation); + DriverStationSim.notifyNewData(); assertEquals(allianceStation, DriverStationSim.getAllianceStationId()); assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance()); assertEquals(3, DriverStation.getLocation()); @@ -230,6 +236,7 @@ class DriverStationSimTest { try (CallbackStore cb = DriverStationSim.registerMatchTimeCallback(callback, false)) { final double testTime = 19.174; DriverStationSim.setMatchTime(testTime); + DriverStationSim.notifyNewData(); assertEquals(testTime, DriverStationSim.getMatchTime()); assertEquals(testTime, DriverStation.getMatchTime()); assertTrue(callback.wasTriggered()); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java index 76d13a4a89..bb40caae8d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java @@ -4,9 +4,11 @@ package edu.wpi.first.wpilibj.templates.educational; -import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.DriverStationJNI; +import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.internal.DriverStationModeThread; /** Educational robot base class. */ public class EducationalRobot extends RobotBase { @@ -34,40 +36,65 @@ public class EducationalRobot extends RobotBase { public void startCompetition() { robotInit(); + DriverStationModeThread modeThread = new DriverStationModeThread(); + + int event = WPIUtilJNI.createEvent(false, false); + + DriverStation.provideRefreshedDataEventHandle(event); + // Tell the DS that the robot is ready to be enabled - HAL.observeUserProgramStarting(); + DriverStationJNI.observeUserProgramStarting(); while (!Thread.currentThread().isInterrupted() && !m_exit) { if (isDisabled()) { - DriverStation.inDisabled(true); + modeThread.inDisabled(true); disabled(); - DriverStation.inDisabled(false); + modeThread.inDisabled(false); while (isDisabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else if (isAutonomous()) { - DriverStation.inAutonomous(true); + modeThread.inAutonomous(true); autonomous(); - DriverStation.inAutonomous(false); + modeThread.inAutonomous(false); while (isAutonomousEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else if (isTest()) { - DriverStation.inTest(true); + modeThread.inTest(true); test(); - DriverStation.inTest(false); + modeThread.inTest(false); while (isTest() && isEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else { - DriverStation.inTeleop(true); + modeThread.inTeleop(true); teleop(); - DriverStation.inTeleop(false); + modeThread.inTeleop(false); while (isTeleopEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } } + + DriverStation.removeRefreshedDataEventHandle(event); + modeThread.close(); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java index 787395c941..f08986476a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java @@ -4,11 +4,11 @@ package edu.wpi.first.wpilibj.templates.robotbaseskeleton; -import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.DriverStationJNI; +import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.internal.DriverStationModeThread; /** * The VM is configured to automatically run this class. If you change the name of this class or the @@ -31,44 +31,65 @@ public class Robot extends RobotBase { public void startCompetition() { robotInit(); + DriverStationModeThread modeThread = new DriverStationModeThread(); + + int event = WPIUtilJNI.createEvent(false, false); + + DriverStation.provideRefreshedDataEventHandle(event); + // Tell the DS that the robot is ready to be enabled - HAL.observeUserProgramStarting(); + DriverStationJNI.observeUserProgramStarting(); while (!Thread.currentThread().isInterrupted() && !m_exit) { if (isDisabled()) { - DriverStation.inDisabled(true); + modeThread.inDisabled(true); disabled(); - DriverStation.inDisabled(false); + modeThread.inDisabled(false); while (isDisabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else if (isAutonomous()) { - DriverStation.inAutonomous(true); + modeThread.inAutonomous(true); autonomous(); - DriverStation.inAutonomous(false); + modeThread.inAutonomous(false); while (isAutonomousEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else if (isTest()) { - LiveWindow.setEnabled(true); - Shuffleboard.enableActuatorWidgets(); - DriverStation.inTest(true); + modeThread.inTest(true); test(); - DriverStation.inTest(false); + modeThread.inTest(false); while (isTest() && isEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } - LiveWindow.setEnabled(false); - Shuffleboard.disableActuatorWidgets(); } else { - DriverStation.inTeleop(true); + modeThread.inTeleop(true); teleop(); - DriverStation.inTeleop(false); + modeThread.inTeleop(false); while (isTeleopEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } } + + DriverStation.removeRefreshedDataEventHandle(event); + modeThread.close(); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java index 8fed60df7e..5a84915d30 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java @@ -4,9 +4,11 @@ package edu.wpi.first.wpilibj.templates.romieducational; -import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.DriverStationJNI; +import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.internal.DriverStationModeThread; /** Educational robot base class. */ public class EducationalRobot extends RobotBase { @@ -34,40 +36,65 @@ public class EducationalRobot extends RobotBase { public void startCompetition() { robotInit(); + DriverStationModeThread modeThread = new DriverStationModeThread(); + + int event = WPIUtilJNI.createEvent(false, false); + + DriverStation.provideRefreshedDataEventHandle(event); + // Tell the DS that the robot is ready to be enabled - HAL.observeUserProgramStarting(); + DriverStationJNI.observeUserProgramStarting(); while (!Thread.currentThread().isInterrupted() && !m_exit) { if (isDisabled()) { - DriverStation.inDisabled(true); + modeThread.inDisabled(true); disabled(); - DriverStation.inDisabled(false); + modeThread.inDisabled(false); while (isDisabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else if (isAutonomous()) { - DriverStation.inAutonomous(true); + modeThread.inAutonomous(true); autonomous(); - DriverStation.inAutonomous(false); + modeThread.inAutonomous(false); while (isAutonomousEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else if (isTest()) { - DriverStation.inTest(true); + modeThread.inTest(true); test(); - DriverStation.inTest(false); + modeThread.inTest(false); while (isTest() && isEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } else { - DriverStation.inTeleop(true); + modeThread.inTeleop(true); teleop(); - DriverStation.inTeleop(false); + modeThread.inTeleop(false); while (isTeleopEnabled()) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(event); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } } } } + + DriverStation.removeRefreshedDataEventHandle(event); + modeThread.close(); } @Override diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java index b2e727dac3..cd67cbdb18 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java @@ -6,6 +6,8 @@ package edu.wpi.first.wpilibj; import static org.junit.Assert.assertEquals; +import edu.wpi.first.hal.DriverStationJNI; +import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.test.AbstractComsSetup; import java.util.logging.Logger; import org.junit.Test; @@ -24,13 +26,23 @@ public class DriverStationTest extends AbstractComsSetup { public void waitForDataTest() { long startTime = RobotController.getFPGATime(); + int handle = WPIUtilJNI.createEvent(false, false); + DriverStationJNI.provideNewDataEventHandle(handle); + // Wait for data 50 times for (int i = 0; i < 50; i++) { - DriverStation.waitForData(); + try { + WPIUtilJNI.waitForObject(handle); + } catch (InterruptedException e) { + e.printStackTrace(); + } } long endTime = RobotController.getFPGATime(); long difference = endTime - startTime; + DriverStationJNI.removeNewDataEventHandle(handle); + WPIUtilJNI.destroyEvent(handle); + assertEquals( "DriverStation waitForData did not wait long enough", TIMER_RUNTIME, diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java index 734115cdc3..056ff5ff1c 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.test; +import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.hal.HAL; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.MockDS; @@ -40,7 +41,7 @@ public abstract class AbstractComsSetup { try { // Set some implementations so that the static methods work properly HAL.initialize(500, 0); - HAL.observeUserProgramStarting(); + DriverStationJNI.observeUserProgramStarting(); DriverStation.getAlliance(); ds = new MockDS(); @@ -56,6 +57,7 @@ public abstract class AbstractComsSetup { // Wait until the robot is enabled before starting the tests int enableCounter = 0; + DriverStation.refreshData(); while (!DriverStation.isEnabled()) { if (enableCounter > 50) { // Robot did not enable properly after 5 seconds. @@ -69,6 +71,7 @@ public abstract class AbstractComsSetup { ex.printStackTrace(); } TestBench.out().println("Waiting for enable: " + enableCounter++); + DriverStation.refreshData(); } TestBench.out().println(); diff --git a/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java b/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java new file mode 100644 index 0000000000..c1b32a5bb5 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util; + +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.locks.ReentrantLock; + +public class EventVector { + private ReentrantLock m_lock = new ReentrantLock(); + private List m_events = new ArrayList<>(); + + /** + * Adds an event to the event vector. + * + * @param handle The event to add + */ + public void add(int handle) { + m_lock.lock(); + try { + m_events.add(handle); + } finally { + m_lock.unlock(); + } + } + + /** + * Removes an event from the event vector. + * + * @param handle The event to remove + */ + public void remove(int handle) { + m_lock.lock(); + try { + m_events.removeIf(x -> x.intValue() == handle); + } finally { + m_lock.unlock(); + } + } + + /** Wakes up all events in the event vector. */ + public void wakeup() { + m_lock.lock(); + try { + for (Integer eventHandle : m_events) { + WPIUtilJNI.setEvent(eventHandle); + } + } finally { + m_lock.unlock(); + } + } +} diff --git a/wpiutil/src/main/native/include/wpi/EventVector.h b/wpiutil/src/main/native/include/wpi/EventVector.h new file mode 100644 index 0000000000..fc54df709d --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/EventVector.h @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/SmallVector.h" +#include "wpi/Synchronization.h" +#include "wpi/mutex.h" + +namespace wpi { +struct EventVector { + wpi::mutex mutex; + wpi::SmallVector events; + + /** + * Adds an event to the event vector. + * + * @param handle the event to add + */ + void Add(WPI_EventHandle handle) { + std::scoped_lock lock{mutex}; + events.emplace_back(handle); + } + + /** + * Removes an event from the event vector. + * + * @param handle The event to remove + */ + void Remove(WPI_EventHandle handle) { + std::scoped_lock lock{mutex}; + auto it = std::find_if( + events.begin(), events.end(), + [=](const WPI_EventHandle fromHandle) { return fromHandle == handle; }); + if (it != events.end()) { + events.erase(it); + } + } + + /** + * Wakes up all events in the event vector. + */ + void Wakeup() { + std::scoped_lock lock{mutex}; + for (auto&& handle : events) { + wpi::SetEvent(handle); + } + } +}; +} // namespace wpi diff --git a/wpiutil/src/main/native/include/wpi/Synchronization.h b/wpiutil/src/main/native/include/wpi/Synchronization.h index 774f17ea9b..4ac3c8c811 100644 --- a/wpiutil/src/main/native/include/wpi/Synchronization.h +++ b/wpiutil/src/main/native/include/wpi/Synchronization.h @@ -4,9 +4,9 @@ #pragma once +#ifdef __cplusplus #include // NOLINT -#ifdef __cplusplus #include #include #endif