diff --git a/wpilibc/athena/include/DriverStation.h b/wpilibc/athena/include/DriverStation.h index 338503381e..7e7dee66d1 100644 --- a/wpilibc/athena/include/DriverStation.h +++ b/wpilibc/athena/include/DriverStation.h @@ -12,7 +12,6 @@ #include #include -#include "HAL/cpp/Semaphore.h" #include "HAL/cpp/priority_condition_variable.h" #include "HAL/cpp/priority_mutex.h" #include "RobotState.h" @@ -102,30 +101,41 @@ class DriverStation : public SensorBase, public RobotStateInterface { void Run(); void UpdateControlWord(bool force, HAL_ControlWord& controlWord) const; + // Joystick User Data std::unique_ptr m_joystickAxes; std::unique_ptr m_joystickPOVs; std::unique_ptr m_joystickButtons; std::unique_ptr m_joystickDescriptor; - // Cached Data + // Joystick Cached Data std::unique_ptr m_joystickAxesCache; std::unique_ptr m_joystickPOVsCache; std::unique_ptr m_joystickButtonsCache; std::unique_ptr m_joystickDescriptorCache; + // Internal Driver Station thread Task m_task; std::atomic m_isRunning{false}; - mutable Semaphore m_newControlData{Semaphore::kEmpty}; - bool m_updatedControlLoopData = false; + + // WPILib WaitForData control variables + bool m_waitForDataPredicate = false; std::condition_variable_any m_waitForDataCond; priority_mutex m_waitForDataMutex; + + mutable std::atomic m_newControlData{false}; + mutable priority_mutex m_joystickDataMutex; + + // Robot state status variables bool m_userInDisabled = false; bool m_userInAutonomous = false; bool m_userInTeleop = false; bool m_userInTest = false; + + // Control word variables mutable HAL_ControlWord m_controlWordCache; mutable std::chrono::steady_clock::time_point m_lastControlWordUpdate; mutable priority_mutex m_controlWordMutex; + double m_nextMessageTime = 0; }; diff --git a/wpilibc/athena/src/DriverStation.cpp b/wpilibc/athena/src/DriverStation.cpp index 980e95c83b..54fbfd6474 100644 --- a/wpilibc/athena/src/DriverStation.cpp +++ b/wpilibc/athena/src/DriverStation.cpp @@ -348,7 +348,7 @@ bool DriverStation::IsDSAttached() const { * @return True if the control data has been updated since the last call. */ bool DriverStation::IsNewControlData() const { - return m_newControlData.tryTake() == false; + return m_newControlData.exchange(false); } /** @@ -476,7 +476,7 @@ bool DriverStation::WaitForData(double timeout) { #endif std::unique_lock lock(m_waitForDataMutex); - while (!m_updatedControlLoopData) { + while (!m_waitForDataPredicate) { if (timeout > 0) { auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime); if (timedOut == std::cv_status::timeout) { @@ -486,7 +486,7 @@ bool DriverStation::WaitForData(double timeout) { m_waitForDataCond.wait(lock); } } - m_updatedControlLoopData = false; + m_waitForDataPredicate = false; return true; } @@ -547,8 +547,6 @@ void DriverStation::GetData() { m_joystickPOVs.swap(m_joystickPOVsCache); m_joystickButtons.swap(m_joystickButtonsCache); m_joystickDescriptor.swap(m_joystickDescriptorCache); - - m_newControlData.give(); } /** @@ -621,9 +619,13 @@ void DriverStation::Run() { while (m_isRunning) { HAL_WaitForDSData(); GetData(); + // notify IsNewControlData variables + m_newControlData = true; + + // notify WaitForData block { std::lock_guard lock(m_waitForDataMutex); - m_updatedControlLoopData = true; + m_waitForDataPredicate = true; } m_waitForDataCond.notify_all(); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java index fbd701abab..d43b3b0787 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java @@ -8,10 +8,12 @@ package edu.wpi.first.wpilibj; 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; +import java.util.concurrent.atomic.AtomicBoolean; import edu.wpi.first.wpilibj.hal.AllianceStationID; import edu.wpi.first.wpilibj.hal.ControlWord; @@ -77,39 +79,43 @@ public class DriverStation implements RobotState.Interface { private static DriverStation instance = new DriverStation(); + // Joystick User Data private HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts]; private HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts]; private HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts]; + // Joystick Cached Data private HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts]; private HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts]; private HALJoystickButtons[] m_joystickButtonsCache = new HALJoystickButtons[kJoystickPorts]; // preallocated byte buffer for button count private ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1); - private int[] m_joystickIsXbox = new int[kJoystickPorts]; - private int[] m_joystickType = new int[kJoystickPorts]; - private String[] m_joystickName = new String[kJoystickPorts]; - private int[][] m_joystickAxisType = new int[kJoystickPorts][HAL.kMaxJoystickAxes]; - + // Internal Driver Station thread private Thread m_thread; + private final Lock m_dataMutex; private final Condition m_dataCond; - private final Object m_newControlDataMutex; - private final Object m_joystickMutex; private volatile boolean m_threadKeepAlive = true; - private final Object m_controlWordMutex; - private ControlWord m_controlWordCache; - private long m_lastControlWordUpdate; + // WPILib WaitForData control variables + private boolean m_waitForDataPredicate; + private AtomicBoolean m_newControlData; + + private final Object m_joystickMutex; + + // Robot state status variables private boolean m_userInDisabled = false; private boolean m_userInAutonomous = false; private boolean m_userInTeleop = false; private boolean m_userInTest = false; - private boolean m_updatedControlLoopData; - private boolean m_newControlData; + + // Control word variables + private final Object m_controlWordMutex; + private ControlWord m_controlWordCache; + private long m_lastControlWordUpdate; /** * Gets an instance of the DriverStation @@ -130,7 +136,7 @@ public class DriverStation implements RobotState.Interface { m_dataMutex = new ReentrantLock(); m_dataCond = m_dataMutex.newCondition(); m_joystickMutex = new Object(); - m_newControlDataMutex = new Object(); + m_newControlData = new AtomicBoolean(false); for (int i = 0; i < kJoystickPorts; i++) { m_joystickButtons[i] = new HALJoystickButtons(); m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes); @@ -538,11 +544,7 @@ public class DriverStation implements RobotState.Interface { * @return True if the control data has been updated since the last call. */ public boolean isNewControlData() { - synchronized (m_newControlDataMutex) { - boolean result = m_newControlData; - m_newControlData = false; - return result; - } + return m_newControlData.getAndSet(false); } /** @@ -652,7 +654,7 @@ public class DriverStation implements RobotState.Interface { m_dataMutex.lock(); try { try { - while (!m_updatedControlLoopData) { + while (!m_waitForDataPredicate) { if (timeout > 0) { long now = Utility.getFPGATime(); if (now < startTime + timeoutMicros) { @@ -671,7 +673,7 @@ public class DriverStation implements RobotState.Interface { m_dataCond.await(); } } - m_updatedControlLoopData = false; + m_waitForDataPredicate = false; // Return true if we have received a proper signal return true; } catch (InterruptedException ex) { @@ -781,10 +783,6 @@ public class DriverStation implements RobotState.Interface { m_joystickPOVs = m_joystickPOVsCache; m_joystickPOVsCache = currentPOVs; } - //Lock new control data mutex and set new control data. - synchronized (m_newControlDataMutex) { - m_newControlData = true; - } } /** @@ -821,11 +819,14 @@ public class DriverStation implements RobotState.Interface { getData(); m_dataMutex.lock(); try { - m_updatedControlLoopData = true; + m_waitForDataPredicate = true; m_dataCond.signalAll(); } finally { m_dataMutex.unlock(); } + // notify isNewControlData variable + m_newControlData.set(true); + if (++safetyCounter >= 4) { MotorSafetyHelper.checkMotors(); safetyCounter = 0;