Squashed 'third_party/allwpilib_2016/' changes from 7f61816..3ce6feb
3ce6feb Updated release number for the new release
e054bbc This adds StopMotor() to the SpeedController interface for C++ and Java. For Java, this is as simple as just adding it, as all motors already have an implementation from MotorSafety that is correctly resolved. For C++, I had to override StopMotor in the classes that descend from SafePWM and explicitly call the SafePWM version. RobotDrive now calls StopMotor on each of its SpeedControllers, instead of calling Disable or setting the motor to 0.0 as it was doing previously.
a15b9dc Merge "More updates to the Gyro test fixing potential null pointer exception"
21b7213 Added Config routine to allow enabling/disabling of limit switch and soft limits. This improves upon the ConfigLimitMode routine, which does not allow certain combinations of enable/disabled limit features. Also keeps parity with LV and Java.
1b45237 Merge "Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically."
1096b15 Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically.
7da21fa More updates to the Gyro test fixing potential null pointer exception
ede5862 Rate-limit duplicate error messages to avoid flooding console.
c22284d Merge "artf4818: Fix CAN Talon JNI references with underscores."
cac3741 Merge "Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now."
097aa8c Fixed the gyro deviation over time test
1d647b3 artf4818: Fix CAN Talon JNI references with underscores.
368cfc7 Merge "Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs."
833e459 Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now.
6c096a3 Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs.
dd7eb0f Fixed robot drive for C++ Simulation
258a622 Merge "Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405"
b1386c6 Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405
a58de40 Merge "Removed publishing of java sim jar"
792d0d3 PDP Classes should support any PDP address
35df955 Merge "Remove maven local as a possible search location"
a0ce9ee Another improvement to HAL-joy getting to ensure it works in future RIO image updates.
0f02c31 Removed publishing of java sim jar
8435ac7 DriverStation::GetJoystickName(): Make work for stick>0.
b4cf4f4 Remove maven local as a possible search location
c3000c3 Merge "Fix HALGetJoystickDescriptor()."
4dec0b4 Merge "Fixed Simulation C++ API"
abc9c27 Fixed Simulation C++ API
b8ae9ec Fix HALGetJoystickDescriptor().
a60f874 Artf4800: Fixes HALGetJoystick*** Segfault
010b584 Merge "fix sim_ds launch script"
4429e16 Merge "Added build dir specification for sim javadoc to not overwrite athena javadoc"
ec9349b Initialized the m_sensors variable to fix artf4798.
9745af8 Added build dir specification for sim javadoc to not overwrite athena javadoc
4da8702 fix sim_ds launch script
05acf79 Fix C++ PIDController SetToleranceBuffer and OnTarget locking.
94a6b05 Merge "Fix onTarget() so that it returns false until there are any values retrieved"
d06053d Fix onTarget() so that it returns false until there are any values retrieved
74927cc Correctly set smart dashboard type for AnalogGyro and ADXRS450_Gyro.
070752f Merge "Fixed sim_ds script library path"
21a8bab Merge "PIDController feed forward term can now be calculated by the end user"
56bd6da Fixed sim_ds script library path
07710f1 Merge "Fixing install script... again"
e1cb61f Use absolute path for NT persistent storage.
09c7482 Fixing install script... again
a3b8bec PIDController feed forward term can now be calculated by the end user
790adb0 artf2612: Update license in source files.
042671c Merge "Removed gz_msgs from wpilibcSim"
c111690 Ultrasonic: replace linked list with std::set.
d71a8ed Removed gz_msgs from wpilibcSim
37259f7 Merge "Replaced linked list in Notifier with std::list"
cd17e7a Merge "Renamed Gyro to AnalogGyro to match athena API"
c5c8a87 Replaced linked list in Notifier with std::list
89405d8 Renamed Gyro to AnalogGyro to match athena API
4a19490 Merge "Adds CANTalon to LiveWindow"
c4a3567 Merge "Fixing the frcsim installer script"
295648f Adds CANTalon to LiveWindow
1b964a2 Merge "Fixes CAN devices in C++ library not showing in the livewindow"
7ba5cee Merge "HAL: Use extern "C" in implementation files."
d17d242 Fixes CAN devices in C++ library not showing in the livewindow
25a771a Added linear digital filters
7349c2c Fixing the frcsim installer script
c82122c Merge "Default bufLength for PIDController in Java should be 1"
58f3f97 Merge "Adds WaitResult to Java waitForInterrupt"
bc8ed12 HAL: Use extern "C" in implementation files.
4cac89e Default bufLength for PIDController in Java should be 1
64fcdcc Keep track of FPGA time rollovers with 64-bit time.
d30b283 Merge "Change C++ Notifier to allow std::function callback."
6ee3052 Merge "Rewrite C++ Notifier to use HAL multi-notifier support."
f5d09e2 Merge "Rewrite Java Notifier and update Interrupt JNI."
d0274aa Merge "Readded styleguide accidentally removed in the reorg"
68311ad Merge "Artf4179: Allow alternate I2C addresses for ADXL345_I2C"
dee12d4 Readded styleguide accidentally removed in the reorg
fa100df Fixed some typos in the comments of MotorEncoderFixture.java, a method name in CANMotorEncoderFixture.java, and the README files
3397b5c Adds WaitResult to Java waitForInterrupt
5f0dffd Artf4177: Use read byte count for ReadString
8564f33 Artf4179: Allow alternate I2C addresses for ADXL345_I2C
e52b52d Change C++ Notifier to allow std::function callback.
40b29e7 Rewrite C++ Notifier to use HAL multi-notifier support.
d126f45 Rewrite Java Notifier and update Interrupt JNI.
557805a Merge "finishing up FRCSim installer"
911b64b finishing up FRCSim installer
e24fe6f Merge "Artf4776 Fixes First DIO PWM usage errors"
f8f9284 Merge "Artf4774 Fixes HAL getHALErrorMessage missing error"
84428d5 Merge "Prevent double free in DigitalGlitchFilter"
a00a5ff Merge "Set correct error message"
9aeee98 Prevent double free in DigitalGlitchFilter
5d2186c working on install process for FRCSim 2016
7fdb616 Merge "This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places."
f67ebca Improved READMEs
8a7c019 Artf4776 Fixes First DIO PWM usage errors
1cd2f9a Added libnipalu to make vision programs link properly
375b92a This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places.
8d0a990 Set correct error message
b65401f Artf4774 Fixes HAL getHALErrorMessage missing error
5f918be Condition java sim build on -PmakeSim flag
53bd180 Merge "Add SPARK and SD540 motor controllers"
66cbe69 Fixed double free of DriverStation.
431f345 Repaired simulation build on linux
611593c Add Cmake wrappers and unzip desktop ntcore builds
51a18cd Add SPARK and SD540 motor controllers
c05e883 Merge changes I55ce71c6,I803680c1
2b80029 Rewrite CANTalon JNI layer.
ef4c45b Last feature addition for CANTalon java/C++ user-facing API.
Change-Id: Ia3a124978a426991890b6f8abbe07d34d75ba38d
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 3ce6feb8acdeca46e93a55280fb6ace3a4d5bcd6
diff --git a/wpilibc/simulation/src/DriverStation.cpp b/wpilibc/simulation/src/DriverStation.cpp
index 39802ab..274cb18 100644
--- a/wpilibc/simulation/src/DriverStation.cpp
+++ b/wpilibc/simulation/src/DriverStation.cpp
@@ -1,227 +1,82 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "DriverStation.h"
-#include "AnalogInput.h"
#include "Timer.h"
-#include "NetworkCommunication/FRCComm.h"
-#include "MotorSafetyHelper.h"
+#include "simulation/MainNode.h"
+//#include "MotorSafetyHelper.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <string.h>
#include "Log.hpp"
+#include "boost/mem_fn.hpp"
// set the logging level
TLogLevel dsLogLevel = logDEBUG;
-const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
#define DS_LOG(level) \
- if (level > dsLogLevel) ; \
- else Log().Get(level)
+ if (level > dsLogLevel) ; \
+ else Log().Get(level)
+const uint32_t DriverStation::kBatteryChannel;
const uint32_t DriverStation::kJoystickPorts;
+const uint32_t DriverStation::kJoystickAxes;
+const float DriverStation::kUpdatePeriod = 0.02;
+uint8_t DriverStation::m_updateNumber = 0;
/**
- * DriverStation constructor.
+ * DriverStation contructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation() {
- // All joysticks should default to having zero axes, povs and buttons, so
- // uninitialized memory doesn't get sent to speed controllers.
- for (unsigned int i = 0; i < kJoystickPorts; i++) {
- m_joystickAxes[i].count = 0;
- m_joystickPOVs[i].count = 0;
- m_joystickButtons[i].count = 0;
- m_joystickDescriptor[i].isXbox = 0;
- m_joystickDescriptor[i].type = -1;
- m_joystickDescriptor[i].name[0] = '\0';
- }
- // Register that semaphore with the network communications task.
- // It will signal when new packet data is available.
- HALSetNewDataSem(m_packetDataAvailableCond.native_handle());
+ state = msgs::DriverStationPtr(new msgs::DriverStation());
+ stateSub = MainNode::Subscribe("~/ds/state",
+ &DriverStation::stateCallback, this);
+ // TODO: for loop + boost bind
+ joysticks[0] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+ joysticksSub[0] = MainNode::Subscribe("~/ds/joysticks/0",
+ &DriverStation::joystickCallback0, this);
+ joysticks[1] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+ joysticksSub[1] = MainNode::Subscribe("~/ds/joysticks/1",
+ &DriverStation::joystickCallback1, this);
+ joysticks[2] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+ joysticksSub[2] = MainNode::Subscribe("~/ds/joysticks/2",
+ &DriverStation::joystickCallback2, this);
+ joysticks[3] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+ joysticksSub[3] = MainNode::Subscribe("~/ds/joysticks/5",
+ &DriverStation::joystickCallback3, this);
+ joysticks[4] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+ joysticksSub[4] = MainNode::Subscribe("~/ds/joysticks/4",
+ &DriverStation::joystickCallback4, this);
+ joysticks[5] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+ joysticksSub[5] = MainNode::Subscribe("~/ds/joysticks/5",
+ &DriverStation::joystickCallback5, this);
- AddToSingletonList();
-
-}
-
-void DriverStation::Run() {
- int period = 0;
- while (true) {
- {
- std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
- m_packetDataAvailableCond.wait(lock);
- }
- GetData();
- m_waitForDataCond.notify_all();
-
- if (++period >= 4) {
- MotorSafetyHelper::CheckMotors();
- period = 0;
- }
- if (m_userInDisabled) HALNetworkCommunicationObserveUserProgramDisabled();
- if (m_userInAutonomous) HALNetworkCommunicationObserveUserProgramAutonomous();
- if (m_userInTeleop) HALNetworkCommunicationObserveUserProgramTeleop();
- if (m_userInTest) HALNetworkCommunicationObserveUserProgramTest();
- }
+ AddToSingletonList();
}
/**
- * Return a reference to the singleton DriverStation.
- * @return Pointer to the DS instance
+ * Return a pointer to the singleton DriverStation.
*/
-DriverStation &DriverStation::GetInstance() {
- static DriverStation instance;
- return instance;
+DriverStation& DriverStation::GetInstance()
+{
+ static DriverStation instance;
+ return instance;
}
/**
- * 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 DriverStation::GetData() {
- // Get the status of all of the joysticks
- for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
- HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
- HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
- HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
- HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
- }
- m_newControlData.give();
-}
-
-/**
- * Read the battery voltage.
+ * Read the battery voltage. Hardcoded to 12 volts for Simulation.
*
- * @return The battery voltage in Volts.
+ * @return The battery voltage.
*/
-float DriverStation::GetBatteryVoltage() const {
- int32_t status = 0;
- float voltage = getVinVoltage(&status);
- wpi_setErrorWithContext(status, "getVinVoltage");
-
- return voltage;
-}
-
-/**
- * Reports errors related to unplugged joysticks
- * Throttles the errors so that they don't overwhelm the DS
- */
-void DriverStation::ReportJoystickUnpluggedError(std::string message) {
- double currentTime = Timer::GetFPGATimestamp();
- if (currentTime > m_nextMessageTime) {
- ReportError(message);
- m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
- }
-}
-
-/**
- * Returns the number of axes on a given joystick port
- *
- * @param stick The joystick port number
- * @return The number of axes on the indicated joystick
- */
-int DriverStation::GetStickAxisCount(uint32_t stick) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return 0;
- }
- HALJoystickAxes joystickAxes;
- HALGetJoystickAxes(stick, &joystickAxes);
- return joystickAxes.count;
-}
-
-/**
- * Returns the name of the joystick at the given port
- *
- * @param stick The joystick port number
- * @return The name of the joystick at the given port
- */
-std::string DriverStation::GetJoystickName(uint32_t stick) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- }
- std::string retVal(m_joystickDescriptor[0].name);
- return retVal;
-}
-
-/**
- * Returns the type of joystick at a given port
- *
- * @param stick The joystick port number
- * @return The HID type of joystick at the given port
- */
-int DriverStation::GetJoystickType(uint32_t stick) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return -1;
- }
- return (int)m_joystickDescriptor[stick].type;
-}
-
-/**
- * Returns a boolean indicating if the controller is an xbox controller.
- *
- * @param stick The joystick port number
- * @return A boolean that is true if the controller is an xbox controller.
- */
-bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return false;
- }
- return (bool)m_joystickDescriptor[stick].isXbox;
-}
-
-/**
- * Returns the types of Axes on a given joystick port
- *
- * @param stick The joystick port number and the target axis
- * @return What type of axis the axis is reporting to be
- */
-int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return -1;
- }
- return m_joystickDescriptor[stick].axisTypes[axis];
-}
-
-/**
- * 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
- */
-int DriverStation::GetStickPOVCount(uint32_t stick) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return 0;
- }
- HALJoystickPOVs joystickPOVs;
- HALGetJoystickPOVs(stick, &joystickPOVs);
- return joystickPOVs.count;
-}
-
-/**
- * Returns the number of buttons on a given joystick port
- *
- * @param stick The joystick port number
- * @return The number of buttons on the indicated joystick
- */
-int DriverStation::GetStickButtonCount(uint32_t stick) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return 0;
- }
- HALJoystickButtons joystickButtons;
- HALGetJoystickButtons(stick, &joystickButtons);
- return joystickButtons.count;
+float DriverStation::GetBatteryVoltage() const
+{
+ return 12.0; // 12 volts all the time!
}
/**
@@ -232,287 +87,310 @@
* @param axis The analog axis value to read from the joystick.
* @return The value of the axis on the joystick.
*/
-float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return 0;
- }
+float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis)
+{
+ if (axis < 0 || axis > (kJoystickAxes - 1))
+ {
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
+ if (stick < 0 || stick > 5)
+ {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0.0;
+ }
- if (axis >= m_joystickAxes[stick].count) {
- if (axis >= kMaxJoystickAxes) {
- wpi_setWPIError(BadJoystickAxis);
- }
- else {
- ReportJoystickUnpluggedError(
- "WARNING: Joystick Axis missing, check if all controllers are "
- "plugged in\n");
- }
- return 0.0f;
- }
-
- int8_t value = m_joystickAxes[stick].axes[axis];
-
- if (value < 0) {
- return value / 128.0f;
- } else {
- return value / 127.0f;
- }
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size())
+ {
+ return 0.0;
+ }
+ return joysticks[stick]->axes(axis);
}
/**
- * Get the state of a POV on the joystick.
+ * The state of a specific button (1 - 12) on the joystick.
+ * This method only works in simulation, but is more efficient than GetStickButtons.
*
- * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ * @param stick The joystick to read.
+ * @param button The button number to check.
+ * @return If the button is pressed.
*/
-int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return -1;
- }
+bool DriverStation::GetStickButton(uint32_t stick, uint32_t button)
+{
+ if (stick < 0 || stick >= 6)
+ {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5");
+ return false;
+ }
- if (pov >= m_joystickPOVs[stick].count) {
- if (pov >= kMaxJoystickPOVs) {
- wpi_setWPIError(BadJoystickAxis);
- }
- else {
- ReportJoystickUnpluggedError(
- "WARNING: Joystick POV missing, check if all controllers are plugged "
- "in\n");
- }
- return -1;
- }
-
- return m_joystickPOVs[stick].povs[pov];
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ if (joysticks[stick] == nullptr || button >= joysticks[stick]->buttons().size())
+ {
+ return false;
+ }
+ return joysticks[stick]->buttons(button-1);
}
/**
* The state of the buttons on the joystick.
+ * 12 buttons (4 msb are unused) from the joystick.
*
* @param stick The joystick to read.
* @return The state of the buttons on the joystick.
*/
-uint32_t DriverStation::GetStickButtons(uint32_t stick) const {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return 0;
- }
+short DriverStation::GetStickButtons(uint32_t stick)
+{
+ if (stick < 0 || stick >= 6)
+ {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5");
+ return false;
+ }
+ short btns = 0, btnid;
- return m_joystickButtons[stick].buttons;
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ msgs::FRCJoystickPtr joy = joysticks[stick];
+ for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++)
+ {
+ if (joysticks[stick]->buttons(btnid))
+ {
+ btns |= (1 << btnid);
+ }
+ }
+ return btns;
}
+// 5V divided by 10 bits
+#define kDSAnalogInScaling ((float)(5.0 / 1023.0))
+
/**
- * The state of one joystick button. Button indexes begin at 1.
+ * Get an analog voltage from the Driver Station.
+ * The analog values are returned as voltage values for the Driver Station analog inputs.
+ * These inputs are typically used for advanced operator interfaces consisting of potentiometers
+ * or resistor networks representing values on a rotary switch.
*
- * @param stick The joystick to read.
- * @param button The button index, beginning at 1.
- * @return The state of the joystick button.
+ * @param channel The analog input channel on the driver station to read from. Valid range is 1 - 4.
+ * @return The analog voltage on the input.
*/
-bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) {
- if (stick >= kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- return false;
- }
-
- if (button > m_joystickButtons[stick].count) {
- ReportJoystickUnpluggedError(
- "WARNING: Joystick Button missing, check if all controllers are "
- "plugged in\n");
- return false;
- }
- if (button == 0) {
- ReportJoystickUnpluggedError(
- "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
- return false;
- }
- return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
+float DriverStation::GetAnalogIn(uint32_t channel)
+{
+ wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn");
+ return 0.0;
}
/**
- * Check if the DS has enabled the robot
- * @return True if the robot is enabled and the DS is connected
+ * Get values from the digital inputs on the Driver Station.
+ * Return digital values from the Drivers Station. These values are typically used for buttons
+ * and switches on advanced operator interfaces.
+ * @param channel The digital input to get. Valid range is 1 - 8.
*/
-bool DriverStation::IsEnabled() const {
- HALControlWord controlWord;
- memset(&controlWord, 0, sizeof(controlWord));
- HALGetControlWord(&controlWord);
- return controlWord.enabled && controlWord.dsAttached;
+bool DriverStation::GetDigitalIn(uint32_t channel)
+{
+ wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn");
+ return false;
}
/**
- * Check if the robot is disabled
- * @return True if the robot is explicitly disabled or the DS is not connected
+ * Set a value for the digital outputs on the Driver Station.
+ *
+ * Control digital outputs on the Drivers Station. These values are typically used for
+ * giving feedback on a custom operator station such as LEDs.
+ *
+ * @param channel The digital output to set. Valid range is 1 - 8.
+ * @param value The state to set the digital output.
*/
-bool DriverStation::IsDisabled() const {
- HALControlWord controlWord;
- memset(&controlWord, 0, sizeof(controlWord));
- HALGetControlWord(&controlWord);
- return !(controlWord.enabled && controlWord.dsAttached);
+void DriverStation::SetDigitalOut(uint32_t channel, bool value)
+{
+ wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut");
}
/**
- * Check if the DS is commanding autonomous mode
- * @return True if the robot is being commanded to be in autonomous mode
+ * Get a value that was set for the digital outputs on the Driver Station.
+ * @param channel The digital ouput to monitor. Valid range is 1 through 8.
+ * @return A digital value being output on the Drivers Station.
*/
-bool DriverStation::IsAutonomous() const {
- HALControlWord controlWord;
- memset(&controlWord, 0, sizeof(controlWord));
- HALGetControlWord(&controlWord);
- return controlWord.autonomous;
+bool DriverStation::GetDigitalOut(uint32_t channel)
+{
+ wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut");
+ return false;
}
-/**
- * Check if the DS is commanding teleop mode
- * @return True if the robot is being commanded to be in teleop mode
- */
-bool DriverStation::IsOperatorControl() const {
- HALControlWord controlWord;
- memset(&controlWord, 0, sizeof(controlWord));
- HALGetControlWord(&controlWord);
- return !(controlWord.autonomous || controlWord.test);
+bool DriverStation::IsEnabled() const
+{
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ return state != nullptr ? state->enabled() : false;
}
-/**
- * Check if the DS is commanding test mode
- * @return True if the robot is being commanded to be in test mode
- */
-bool DriverStation::IsTest() const {
- HALControlWord controlWord;
- HALGetControlWord(&controlWord);
- return controlWord.test;
+bool DriverStation::IsDisabled() const
+{
+ return !IsEnabled();
}
-/**
- * Check if the DS is attached
- * @return True if the DS is connected to the robot
- */
-bool DriverStation::IsDSAttached() const {
- HALControlWord controlWord;
- memset(&controlWord, 0, sizeof(controlWord));
- HALGetControlWord(&controlWord);
- return controlWord.dsAttached;
+bool DriverStation::IsAutonomous() const
+{
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ return state != nullptr ?
+ state->state() == msgs::DriverStation_State_AUTO : false;
}
-/**
- * @return always true in simulation
- */
-bool DriverStation::IsSysActive() const {
- return true;
+bool DriverStation::IsOperatorControl() const
+{
+ return !(IsAutonomous() || IsTest());
}
-/**
- * @return always false in simulation
- */
-bool DriverStation::IsSysBrownedOut() const {
- return false;
-}
-
-/**
- * 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 get the intended behaviour.
- * @return True if the control data has been updated since the last call.
- */
-bool DriverStation::IsNewControlData() const {
- return m_newControlData.tryTake() == false;
+bool DriverStation::IsTest() const
+{
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ return state != nullptr ?
+ state->state() == msgs::DriverStation_State_TEST : false;
}
/**
* Is 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
+ * Note: This does not work with the Blue DS.
+ * @return True if the robot is competing on a field being controlled by a Field Management System
*/
-bool DriverStation::IsFMSAttached() const {
- HALControlWord controlWord;
- HALGetControlWord(&controlWord);
- return controlWord.fmsAttached;
+bool DriverStation::IsFMSAttached() const
+{
+ return false; // No FMS in simulation
}
/**
* Return the alliance that the driver station says it is on.
* This could return kRed or kBlue
- * @return The Alliance enum (kRed, kBlue or kInvalid)
+ * @return The Alliance enum
*/
-DriverStation::Alliance DriverStation::GetAlliance() const {
- HALAllianceStationID allianceStationID;
- HALGetAllianceStation(&allianceStationID);
- switch (allianceStationID) {
- case kHALAllianceStationID_red1:
- case kHALAllianceStationID_red2:
- case kHALAllianceStationID_red3:
- return kRed;
- case kHALAllianceStationID_blue1:
- case kHALAllianceStationID_blue2:
- case kHALAllianceStationID_blue3:
- return kBlue;
- default:
- return kInvalid;
- }
+DriverStation::Alliance DriverStation::GetAlliance() const
+{
+ // if (m_controlData->dsID_Alliance == 'R') return kRed;
+ // if (m_controlData->dsID_Alliance == 'B') return kBlue;
+ // wpi_assert(false);
+ return kInvalid; // TODO: Support alliance colors
}
/**
* Return the driver station location on the field
* This could return 1, 2, or 3
- * @return The location of the driver station (1-3, 0 for invalid)
+ * @return The location of the driver station
*/
-uint32_t DriverStation::GetLocation() const {
- HALAllianceStationID allianceStationID;
- HALGetAllianceStation(&allianceStationID);
- switch (allianceStationID) {
- case kHALAllianceStationID_red1:
- case kHALAllianceStationID_blue1:
- return 1;
- case kHALAllianceStationID_red2:
- case kHALAllianceStationID_blue2:
- return 2;
- case kHALAllianceStationID_red3:
- case kHALAllianceStationID_blue3:
- return 3;
- default:
- return 0;
- }
+uint32_t DriverStation::GetLocation() const
+{
+ return -1; // TODO: Support locations
}
/**
* 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
+ * This is a good way to delay processing until there is new driver station data to act on
*/
-void DriverStation::WaitForData() {
- std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
- m_waitForDataCond.wait(lock);
+void DriverStation::WaitForData()
+{
+ std::unique_lock<std::mutex> lock(m_waitForDataMutex);
+ m_waitForDataCond.wait(lock);
}
/**
* 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 current period (auto or
- * teleop).
- * Warning: This is not an official time (so it cannot be used to dispute ref
- * calls or guarantee that a function
- * will trigger before the match ends)
- * The Practice Match function of the DS approximates the behaviour seen on the
- * field.
- * @return Time remaining in current match period (auto or teleop)
+ * The FMS does not currently send the official match time to the robots
+ * This returns the time since the enable signal sent from the Driver Station
+ * At the beginning of autonomous, the time is reset to 0.0 seconds
+ * At the beginning of teleop, the time is reset to +15.0 seconds
+ * If the robot is disabled, this returns 0.0 seconds
+ * Warning: This is not an official time (so it cannot be used to argue with referees)
+ * @return Match time in seconds since the beginning of autonomous
*/
-double DriverStation::GetMatchTime() const {
- float matchTime;
- HALGetMatchTime(&matchTime);
- return (double)matchTime;
+double DriverStation::GetMatchTime() const
+{
+ if (m_approxMatchTimeOffset < 0.0)
+ return 0.0;
+ return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset;
}
/**
* Report an error to the DriverStation messages window.
* The error is also printed to the program console.
*/
-void DriverStation::ReportError(std::string error) {
- std::cout << error << std::endl;
+void DriverStation::ReportError(std::string error)
+{
+ std::cout << error << std::endl;
+}
- HALControlWord controlWord;
- HALGetControlWord(&controlWord);
- if (controlWord.dsAttached) {
- HALSetErrorData(error.c_str(), error.size(), 0);
- }
+/**
+ * Report a warning to the DriverStation messages window.
+ * The warning is also printed to the program console.
+ */
+void DriverStation::ReportWarning(std::string error)
+{
+ std::cout << error << std::endl;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(bool is_error, int32_t code,
+ const std::string& error,
+ const std::string& location,
+ const std::string& stack)
+{
+ if (!location.empty())
+ std::cout << (is_error ? "Error" : "Warning") << " at " << location << ": ";
+ std::cout << error << std::endl;
+ if (!stack.empty())
+ std::cout << stack << std::endl;
+}
+
+/**
+ * Return the team number that the Driver Station is configured for
+ * @return The team number
+ */
+uint16_t DriverStation::GetTeamNumber() const
+{
+ return 348;
+}
+
+void DriverStation::stateCallback(const msgs::ConstDriverStationPtr &msg)
+{
+ {
+ std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+ *state = *msg;
+ }
+ m_waitForDataCond.notify_all();
+}
+
+void DriverStation::joystickCallback(const msgs::ConstFRCJoystickPtr &msg,
+ int i)
+{
+ std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+ *(joysticks[i]) = *msg;
+}
+
+void DriverStation::joystickCallback0(const msgs::ConstFRCJoystickPtr &msg)
+{
+ joystickCallback(msg, 0);
+}
+
+void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr &msg)
+{
+ joystickCallback(msg, 1);
+}
+
+void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr &msg)
+{
+ joystickCallback(msg, 2);
+}
+
+void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr &msg)
+{
+ joystickCallback(msg, 3);
+}
+
+void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr &msg)
+{
+ joystickCallback(msg, 4);
+}
+
+void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr &msg)
+{
+ joystickCallback(msg, 5);
}