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/CMakeLists.txt b/wpilibc/simulation/CMakeLists.txt
index 1241fd3..39a7c6d 100644
--- a/wpilibc/simulation/CMakeLists.txt
+++ b/wpilibc/simulation/CMakeLists.txt
@@ -1,60 +1,26 @@
cmake_minimum_required(VERSION 2.8)
-project(WPILibSim)
+cmake_policy(SET CMP0015 NEW)
+project(wpilibcSim)
-if (WIN32)
- #temporary until we build dlls
- add_definitions(-DBUILDING_STATIC_LIBS)
-
- # XXX: should be set via CMake variables in configure.bat
- set(PTHREAD_INCLUDE_DIR "C:/Users/peter/gz-ws/pthread-w32/include")
- set(PTHREAD_LIBRARY "C:/Users/peter/gz-ws/pthread-w32/libs/x64/pthreadVC2.lib")
-endif()
-
-get_filename_component(HAL_API_INCLUDES ../../hal/include REALPATH)
-get_filename_component(NWT_API_INCLUDES ../../networktables/cpp/include REALPATH)
-
-
-# also on windows use sprintf_s instead of snprintf
-# TODO: find a more permenenant solution
-if (WIN32)
- add_definitions(-Dsnprintf=sprintf_s)
-endif()
-
-file(GLOB_RECURSE COM_SRC_FILES ../wpilibC++/src/*.cpp)
-
+file(GLOB_RECURSE COM_SRC_FILES ../shared/src/*.cpp
+ src/*.cpp)
set (INCLUDE_FOLDERS include
- ../wpilibC++/include
- ../../networktables/ntcore/include
+ ../shared/include
../../hal/include
+ ${NTCORE_INCLUDE_DIR}
${GZ_MSGS_INCLUDE_DIR}
${Boost_INCLUDE_DIR}
${GAZEBO_INCLUDE_DIRS})
-if (WIN32)
- #these paths will be fixed when a more permenant windows development solution is found
- set(INCLUDE_FOLDERS ${INCLUDE_FOLDERS}
- C:/Users/peter/gz-ws/protobuf-2.6.0-win64-vc12/src
- C:/Users/peter/gz-ws/sdformat/src/win/tinyxml
- C:/Users/peter/gz-ws/FreeImage-vc12-x64-release-debug/Source
- C:/Users/peter/gz-ws/tbb43_20141023oss/include
- ${PTHREAD_INCLUDE_DIR})
-endif()
-
include_directories(${INCLUDE_FOLDERS})
-link_directories(${GAZEBO_LIBRARY_DIRS})
+link_directories(${NTCORE_LIBDIR})
-if (WIN32)
- add_library(WPILibSim ${SRC_FILES} ${COM_SRC_FILES})
-else()
- add_library(WPILibSim SHARED ${SRC_FILES} ${COM_SRC_FILES})
-endif()
+add_library(${PROJECT_NAME} SHARED ${SRC_FILES} ${COM_SRC_FILES})
-target_link_libraries(WPILibSim gz_msgs ntcore ${PTHREAD_LIBRARY} ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} -fPIC) # NetworkTables
+target_link_libraries(${PROJECT_NAME} ntcore)
-if (WIN32)
- set_target_properties(${project} PROPERTIES LINK_FLAGS "/DEBUG")
-endif()
-
-#copy to eclipse plugin
+set_target_properties(${PROJECT_NAME}
+ PROPERTIES
+ LIBRARY_OUTPUT_DIRECTORY ${SIMULATION_INSTALL_DIR}/lib)
diff --git a/wpilibc/simulation/include/AnalogGyro.h b/wpilibc/simulation/include/AnalogGyro.h
new file mode 100644
index 0000000..e117330
--- /dev/null
+++ b/wpilibc/simulation/include/AnalogGyro.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "GyroBase.h"
+#include "simulation/SimGyro.h"
+
+#include <memory>
+
+class AnalogInput;
+class AnalogModule;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position.
+ * The AnalogGyro class tracks the robots heading based on the starting position. As the robot
+ * rotates the new heading is computed by integrating the rate of rotation returned
+ * by the sensor. When the class is instantiated, it does a short calibration routine
+ * where it samples the gyro while at rest to determine the default offset. This is
+ * subtracted from each sample to determine the heading. This gyro class must be used
+ * with a channel that is assigned one of the Analog accumulators from the FPGA. See
+ * AnalogInput for the current accumulator assignments.
+ */
+class AnalogGyro : public GyroBase
+{
+public:
+ static const uint32_t kOversampleBits;
+ static const uint32_t kAverageBits;
+ static const float kSamplesPerSecond;
+ static const float kCalibrationSampleTime;
+ static const float kDefaultVoltsPerDegreePerSecond;
+
+ explicit AnalogGyro(uint32_t channel);
+ virtual ~AnalogGyro() = default;
+ float GetAngle() const;
+ void Calibrate() override;
+ double GetRate() const;
+ void Reset();
+
+private:
+ void InitAnalogGyro(int channel);
+
+ SimGyro* impl;
+
+ std::shared_ptr<ITable> m_table;
+};
diff --git a/wpilibc/simulation/include/AnalogInput.h b/wpilibc/simulation/include/AnalogInput.h
index 59a3a85..8e857b4 100644
--- a/wpilibc/simulation/include/AnalogInput.h
+++ b/wpilibc/simulation/include/AnalogInput.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "simulation/SimFloatInput.h"
diff --git a/wpilibc/simulation/include/AnalogPotentiometer.h b/wpilibc/simulation/include/AnalogPotentiometer.h
index 786f36f..2dbcb3e 100644
--- a/wpilibc/simulation/include/AnalogPotentiometer.h
+++ b/wpilibc/simulation/include/AnalogPotentiometer.h
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
#pragma once
#include "AnalogInput.h"
diff --git a/wpilibc/simulation/include/Counter.h b/wpilibc/simulation/include/Counter.h
index 338efc0..2accacf 100644
--- a/wpilibc/simulation/include/Counter.h
+++ b/wpilibc/simulation/include/Counter.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "HAL/HAL.hpp"
diff --git a/wpilibc/simulation/include/CounterBase.h b/wpilibc/simulation/include/CounterBase.h
index 3352c29..683b681 100644
--- a/wpilibc/simulation/include/CounterBase.h
+++ b/wpilibc/simulation/include/CounterBase.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
/**
diff --git a/wpilibc/simulation/include/DigitalInput.h b/wpilibc/simulation/include/DigitalInput.h
index 1482e9c..92c4742 100644
--- a/wpilibc/simulation/include/DigitalInput.h
+++ b/wpilibc/simulation/include/DigitalInput.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "simulation/SimDigitalInput.h"
diff --git a/wpilibc/simulation/include/DoubleSolenoid.h b/wpilibc/simulation/include/DoubleSolenoid.h
index 122a439..ca38f1d 100644
--- a/wpilibc/simulation/include/DoubleSolenoid.h
+++ b/wpilibc/simulation/include/DoubleSolenoid.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "simulation/SimContinuousOutput.h"
diff --git a/wpilibc/simulation/include/DriverStation.h b/wpilibc/simulation/include/DriverStation.h
index f15985c..b96f1a7 100644
--- a/wpilibc/simulation/include/DriverStation.h
+++ b/wpilibc/simulation/include/DriverStation.h
@@ -1,112 +1,146 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
+#include "simulation/gz_msgs/msgs.h"
+
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
+
+#include <gazebo/transport/transport.hh>
#include "SensorBase.h"
#include "RobotState.h"
-#include "HAL/HAL.hpp"
-#include "HAL/cpp/Semaphore.hpp"
-#include "HAL/cpp/priority_mutex.h"
-#include "HAL/cpp/priority_condition_variable.h"
+#include <mutex>
#include <condition_variable>
-struct HALControlWord;
+struct HALCommonControlData;
class AnalogInput;
+using namespace gazebo;
+
/**
- * Provide access to the network communication data to / from the Driver
- * Station.
+ * Provide access to the network communication data to / from the Driver Station.
*/
-class DriverStation : public SensorBase, public RobotStateInterface {
- public:
- enum Alliance { kRed, kBlue, kInvalid };
+class DriverStation : public SensorBase, public RobotStateInterface
+{
+public:
+ enum Alliance
+ {
+ kRed,
+ kBlue,
+ kInvalid
+ };
- virtual ~DriverStation();
- static DriverStation &GetInstance();
- static void ReportError(std::string error);
+ virtual ~DriverStation() = default;
+ static DriverStation &GetInstance();
+ static void ReportError(std::string error);
+ static void ReportWarning(std::string error);
+ static void ReportError(bool is_error, int32_t code, const std::string& error, const std::string& location, const std::string& stack);
- static const uint32_t kJoystickPorts = 6;
+ static const uint32_t kBatteryChannel = 7;
+ static const uint32_t kJoystickPorts = 4;
+ static const uint32_t kJoystickAxes = 6;
- float GetStickAxis(uint32_t stick, uint32_t axis);
- int GetStickPOV(uint32_t stick, uint32_t pov);
- uint32_t GetStickButtons(uint32_t stick) const;
- bool GetStickButton(uint32_t stick, uint8_t button);
+ float GetStickAxis(uint32_t stick, uint32_t axis);
+ bool GetStickButton(uint32_t stick, uint32_t button);
+ short GetStickButtons(uint32_t stick);
- int GetStickAxisCount(uint32_t stick) const;
- int GetStickPOVCount(uint32_t stick) const;
- int GetStickButtonCount(uint32_t stick) const;
+ float GetAnalogIn(uint32_t channel);
+ bool GetDigitalIn(uint32_t channel);
+ void SetDigitalOut(uint32_t channel, bool value);
+ bool GetDigitalOut(uint32_t channel);
- bool GetJoystickIsXbox(uint32_t stick) const;
- int GetJoystickType(uint32_t stick) const;
- std::string GetJoystickName(uint32_t stick) const;
- int GetJoystickAxisType(uint32_t stick, uint8_t axis) const;
+ bool IsEnabled() const;
+ bool IsDisabled() const;
+ bool IsAutonomous() const;
+ bool IsOperatorControl() const;
+ bool IsTest() const;
+ bool IsFMSAttached() const;
- bool IsEnabled() const override;
- bool IsDisabled() const override;
- bool IsAutonomous() const override;
- bool IsOperatorControl() const override;
- bool IsTest() const override;
- bool IsDSAttached() const;
- bool IsNewControlData() const;
- bool IsFMSAttached() const;
- bool IsSysActive() const;
- bool IsSysBrownedOut() const;
+ uint32_t GetPacketNumber() const;
+ Alliance GetAlliance() const;
+ uint32_t GetLocation() const;
+ void WaitForData();
+ double GetMatchTime() const;
+ float GetBatteryVoltage() const;
+ uint16_t GetTeamNumber() const;
- Alliance GetAlliance() const;
- uint32_t GetLocation() const;
- void WaitForData();
- double GetMatchTime() const;
- float GetBatteryVoltage() const;
- /** 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 */
- void InDisabled(bool 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 */
- void InAutonomous(bool 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 */
- void InOperatorControl(bool 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 */
- void InTest(bool entering) { m_userInTest = entering; }
- protected:
- DriverStation();
+ void IncrementUpdateNumber()
+ {
+ m_updateNumber++;
+ }
- void GetData();
+ /** 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 */
+ void InDisabled(bool 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 */
+ void InAutonomous(bool 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 */
+ void InOperatorControl(bool 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 */
+ void InTest(bool entering)
+ {
+ m_userInTest = entering;
+ }
- private:
- static DriverStation *m_instance;
- void ReportJoystickUnpluggedError(std::string message);
- void Run();
+protected:
+ DriverStation();
- HALJoystickAxes m_joystickAxes[kJoystickPorts];
- HALJoystickPOVs m_joystickPOVs[kJoystickPorts];
- HALJoystickButtons m_joystickButtons[kJoystickPorts];
- HALJoystickDescriptor m_joystickDescriptor[kJoystickPorts];
- mutable Semaphore m_newControlData{Semaphore::kEmpty};
- mutable priority_condition_variable m_packetDataAvailableCond;
- priority_mutex m_packetDataAvailableMutex;
- std::condition_variable_any m_waitForDataCond;
- priority_mutex m_waitForDataMutex;
- bool m_userInDisabled = false;
- bool m_userInAutonomous = false;
- bool m_userInTeleop = false;
- bool m_userInTest = false;
- double m_nextMessageTime = 0;
+private:
+ static void InitTask(DriverStation *ds);
+ static DriverStation *m_instance;
+ static uint8_t m_updateNumber;
+ ///< TODO: Get rid of this and use the semaphore signaling
+ static const float kUpdatePeriod;
+
+ void stateCallback(const msgs::ConstDriverStationPtr &msg);
+ void joystickCallback(const msgs::ConstFRCJoystickPtr &msg, int i);
+ void joystickCallback0(const msgs::ConstFRCJoystickPtr &msg);
+ void joystickCallback1(const msgs::ConstFRCJoystickPtr &msg);
+ void joystickCallback2(const msgs::ConstFRCJoystickPtr &msg);
+ void joystickCallback3(const msgs::ConstFRCJoystickPtr &msg);
+ void joystickCallback4(const msgs::ConstFRCJoystickPtr &msg);
+ void joystickCallback5(const msgs::ConstFRCJoystickPtr &msg);
+
+ uint8_t m_digitalOut = 0;
+ std::condition_variable m_waitForDataCond;
+ std::mutex m_waitForDataMutex;
+ mutable std::recursive_mutex m_stateMutex;
+ std::recursive_mutex m_joystickMutex;
+ double m_approxMatchTimeOffset = 0;
+ bool m_userInDisabled = false;
+ bool m_userInAutonomous = false;
+ bool m_userInTeleop = false;
+ bool m_userInTest = false;
+
+ transport::SubscriberPtr stateSub;
+ transport::SubscriberPtr joysticksSub[6];
+ msgs::DriverStationPtr state;
+ msgs::FRCJoystickPtr joysticks[6];
};
diff --git a/wpilibc/simulation/include/Encoder.h b/wpilibc/simulation/include/Encoder.h
index 89b1b3a..22b8657 100644
--- a/wpilibc/simulation/include/Encoder.h
+++ b/wpilibc/simulation/include/Encoder.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "simulation/SimEncoder.h"
diff --git a/wpilibc/simulation/include/Gyro.h b/wpilibc/simulation/include/Gyro.h
deleted file mode 100644
index 7e4e40a..0000000
--- a/wpilibc/simulation/include/Gyro.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. 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. */
-/*----------------------------------------------------------------------------*/
-#pragma once
-
-#include "SensorBase.h"
-#include "PIDSource.h"
-#include "LiveWindow/LiveWindowSendable.h"
-#include "simulation/SimGyro.h"
-
-#include <memory>
-
-class AnalogInput;
-class AnalogModule;
-
-/**
- * Use a rate gyro to return the robots heading relative to a starting position.
- * The Gyro class tracks the robots heading based on the starting position. As the robot
- * rotates the new heading is computed by integrating the rate of rotation returned
- * by the sensor. When the class is instantiated, it does a short calibration routine
- * where it samples the gyro while at rest to determine the default offset. This is
- * subtracted from each sample to determine the heading. This gyro class must be used
- * with a channel that is assigned one of the Analog accumulators from the FPGA. See
- * AnalogInput for the current accumulator assignments.
- */
-class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable
-{
-public:
- static const uint32_t kOversampleBits;
- static const uint32_t kAverageBits;
- static const float kSamplesPerSecond;
- static const float kCalibrationSampleTime;
- static const float kDefaultVoltsPerDegreePerSecond;
-
- explicit Gyro(uint32_t channel);
- virtual ~Gyro() = default;
- virtual float GetAngle() const;
- virtual double GetRate() const;
- virtual void Reset();
-
- // PIDSource interface
- void SetPIDSourceType(PIDSourceType pidSource) override;
- double PIDGet() override;
-
- void UpdateTable() override;
- void StartLiveWindowMode() override;
- void StopLiveWindowMode() override;
- std::string GetSmartDashboardType() const override;
- void InitTable(std::shared_ptr<ITable> subTable) override;
- std::shared_ptr<ITable> GetTable() const override;
-
-private:
- void InitGyro(int channel);
-
- SimGyro* impl;
-
- std::shared_ptr<ITable> m_table;
-};
diff --git a/wpilibc/simulation/include/IterativeRobot.h b/wpilibc/simulation/include/IterativeRobot.h
index 7c9104d..8a4cd4d 100644
--- a/wpilibc/simulation/include/IterativeRobot.h
+++ b/wpilibc/simulation/include/IterativeRobot.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "Timer.h"
diff --git a/wpilibc/simulation/include/Jaguar.h b/wpilibc/simulation/include/Jaguar.h
index 1f522d9..27325af 100644
--- a/wpilibc/simulation/include/Jaguar.h
+++ b/wpilibc/simulation/include/Jaguar.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "SafePWM.h"
diff --git a/wpilibc/simulation/include/Joystick.h b/wpilibc/simulation/include/Joystick.h
index 0922359..e4e46da 100644
--- a/wpilibc/simulation/include/Joystick.h
+++ b/wpilibc/simulation/include/Joystick.h
@@ -1,16 +1,14 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
#ifndef JOYSTICK_H_
#define JOYSTICK_H_
-#include <cstdint>
#include <memory>
-#include <vector>
#include "GenericHID.h"
#include "ErrorBase.h"
@@ -18,101 +16,64 @@
/**
* Handle input from standard Joysticks connected to the Driver Station.
- * This class handles standard input that comes from the Driver Station. Each
- * time a value is requested
- * the most recent value is returned. There is a single class instance for each
- * joystick and the mapping
+ * This class handles standard input that comes from the Driver Station. Each time a value is requested
+ * the most recent value is returned. There is a single class instance for each joystick and the mapping
* of ports to hardware buttons depends on the code in the driver station.
*/
-class Joystick : public GenericHID, public ErrorBase {
- public:
- static const uint32_t kDefaultXAxis = 0;
- static const uint32_t kDefaultYAxis = 1;
- static const uint32_t kDefaultZAxis = 2;
- static const uint32_t kDefaultTwistAxis = 2;
- static const uint32_t kDefaultThrottleAxis = 3;
- typedef enum {
- kXAxis,
- kYAxis,
- kZAxis,
- kTwistAxis,
- kThrottleAxis,
- kNumAxisTypes
- } AxisType;
- static const uint32_t kDefaultTriggerButton = 1;
- static const uint32_t kDefaultTopButton = 2;
- typedef enum { kTriggerButton, kTopButton, kNumButtonTypes } ButtonType;
- typedef enum { kLeftRumble, kRightRumble } RumbleType;
- typedef enum {
- kUnknown = -1,
- kXInputUnknown = 0,
- kXInputGamepad = 1,
- kXInputWheel = 2,
- kXInputArcadeStick = 3,
- kXInputFlightStick = 4,
- kXInputDancePad = 5,
- kXInputGuitar = 6,
- kXInputGuitar2 = 7,
- kXInputDrumKit = 8,
- kXInputGuitar3 = 11,
- kXInputArcadePad = 19,
- kHIDJoystick = 20,
- kHIDGamepad = 21,
- kHIDDriving = 22,
- kHIDFlight = 23,
- kHID1stPerson = 24
- } HIDType;
- explicit Joystick(uint32_t port);
- Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes);
- virtual ~Joystick() = default;
+class Joystick : public GenericHID, public ErrorBase
+{
+public:
+ static const uint32_t kDefaultXAxis = 1;
+ static const uint32_t kDefaultYAxis = 2;
+ static const uint32_t kDefaultZAxis = 3;
+ static const uint32_t kDefaultTwistAxis = 4;
+ static const uint32_t kDefaultThrottleAxis = 3;
+ typedef enum
+ {
+ kXAxis, kYAxis, kZAxis, kTwistAxis, kThrottleAxis, kNumAxisTypes
+ } AxisType;
+ static const uint32_t kDefaultTriggerButton = 1;
+ static const uint32_t kDefaultTopButton = 2;
+ typedef enum
+ {
+ kTriggerButton, kTopButton, kNumButtonTypes
+ } ButtonType;
- Joystick(const Joystick&) = delete;
- Joystick& operator=(const Joystick&) = delete;
+ explicit Joystick(uint32_t port);
+ Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes);
+ virtual ~Joystick() = default;
- uint32_t GetAxisChannel(AxisType axis) const;
- void SetAxisChannel(AxisType axis, uint32_t channel);
+ Joystick(const Joystick&) = delete;
+ Joystick& operator=(const Joystick&) = delete;
- virtual float GetX(JoystickHand hand = kRightHand) const override;
- virtual float GetY(JoystickHand hand = kRightHand) const override;
- virtual float GetZ() const override;
- virtual float GetTwist() const override;
- virtual float GetThrottle() const override;
- virtual float GetAxis(AxisType axis) const;
- float GetRawAxis(uint32_t axis) const override;
+ uint32_t GetAxisChannel(AxisType axis);
+ void SetAxisChannel(AxisType axis, uint32_t channel);
- virtual bool GetTrigger(JoystickHand hand = kRightHand) const override;
- virtual bool GetTop(JoystickHand hand = kRightHand) const override;
- virtual bool GetBumper(JoystickHand hand = kRightHand) const override;
- virtual bool GetRawButton(uint32_t button) const override;
- virtual int GetPOV(uint32_t pov = 0) const override;
- bool GetButton(ButtonType button) const;
- static Joystick *GetStickForPort(uint32_t port);
+ virtual float GetX(JoystickHand hand = kRightHand) const override;
+ virtual float GetY(JoystickHand hand = kRightHand) const override;
+ virtual float GetZ() const override;
+ virtual float GetTwist() const override;
+ virtual float GetThrottle() const override;
+ virtual float GetAxis(AxisType axis) const;
+ float GetRawAxis(uint32_t axis) const override;
- virtual float GetMagnitude() const;
- virtual float GetDirectionRadians() const;
- virtual float GetDirectionDegrees() const;
+ virtual bool GetTrigger(JoystickHand hand = kRightHand) const override;
+ virtual bool GetTop(JoystickHand hand = kRightHand) const override;
+ virtual bool GetBumper(JoystickHand hand = kRightHand) const override;
+ virtual bool GetRawButton(uint32_t button) const override;
+ virtual int GetPOV(uint32_t pov = 1) const override;
+ bool GetButton(ButtonType button) const;
+ static Joystick* GetStickForPort(uint32_t port);
- bool GetIsXbox() const;
- Joystick::HIDType GetType() const;
- std::string GetName() const;
- int GetAxisType(uint8_t axis) const;
+ virtual float GetMagnitude() const;
+ virtual float GetDirectionRadians() const;
+ virtual float GetDirectionDegrees() const;
- int GetAxisCount() const;
- int GetButtonCount() const;
- int GetPOVCount() const;
-
- void SetRumble(RumbleType type, float value);
- void SetOutput(uint8_t outputNumber, bool value);
- void SetOutputs(uint32_t value);
-
- private:
- DriverStation &m_ds;
- uint32_t m_port;
- ::std::vector<uint32_t> m_axes;
- ::std::vector<uint32_t> m_buttons;
- uint32_t m_outputs = 0;
- uint16_t m_leftRumble = 0;
- uint16_t m_rightRumble = 0;
+private:
+ DriverStation &m_ds;
+ uint32_t m_port;
+ std::unique_ptr<uint32_t[]> m_axes;
+ std::unique_ptr<uint32_t[]> m_buttons;
};
#endif
diff --git a/wpilibc/simulation/include/MotorSafety.h b/wpilibc/simulation/include/MotorSafety.h
index c718806..82c77d8 100644
--- a/wpilibc/simulation/include/MotorSafety.h
+++ b/wpilibc/simulation/include/MotorSafety.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#define DEFAULT_SAFETY_EXPIRATION 0.1
diff --git a/wpilibc/simulation/include/MotorSafetyHelper.h b/wpilibc/simulation/include/MotorSafetyHelper.h
index bbe7658..f124605 100644
--- a/wpilibc/simulation/include/MotorSafetyHelper.h
+++ b/wpilibc/simulation/include/MotorSafetyHelper.h
@@ -1,9 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "ErrorBase.h"
diff --git a/wpilibc/simulation/include/Notifier.h b/wpilibc/simulation/include/Notifier.h
new file mode 100644
index 0000000..0160aa4
--- /dev/null
+++ b/wpilibc/simulation/include/Notifier.h
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <functional>
+#include <list>
+#include <thread>
+
+#include "ErrorBase.h"
+#include "HAL/cpp/priority_mutex.h"
+
+typedef std::function<void()> TimerEventHandler;
+
+class Notifier : public ErrorBase {
+ public:
+ explicit Notifier(TimerEventHandler handler);
+
+ template <typename Callable, typename Arg, typename... Args>
+ Notifier(Callable&& f, Arg&& arg, Args&&... args)
+ : Notifier(std::bind(std::forward<Callable>(f),
+ std::forward<Arg>(arg),
+ std::forward<Args>(args)...)) {}
+ virtual ~Notifier();
+
+ Notifier(const Notifier&) = delete;
+ Notifier& operator=(const Notifier&) = delete;
+
+ void StartSingle(double delay);
+ void StartPeriodic(double period);
+ void Stop();
+
+ private:
+ static std::list<Notifier*> timerQueue;
+ static priority_recursive_mutex queueMutex;
+ static priority_mutex halMutex;
+ static void *m_notifier;
+ static std::atomic<int> refcount;
+
+ // Process the timer queue on a timer event
+ static void ProcessQueue(uint32_t mask, void *params);
+
+ // Update the FPGA alarm since the queue has changed
+ static void UpdateAlarm();
+
+ // Insert the Notifier in the timer queue
+ void InsertInQueue(bool reschedule);
+
+ // Delete this Notifier from the timer queue
+ void DeleteFromQueue();
+
+ // Address of the handler
+ TimerEventHandler m_handler;
+ // The relative time (either periodic or single)
+ double m_period = 0;
+ // Absolute expiration time for the current event
+ double m_expirationTime = 0;
+ // True if this is a periodic event
+ bool m_periodic = false;
+ // Indicates if this entry is queued
+ bool m_queued = false;
+ // Held by interrupt manager task while handler call is in progress
+ priority_mutex m_handlerMutex;
+ static std::thread m_task;
+ static std::atomic<bool> m_stopped;
+ static void Run();
+};
diff --git a/wpilibc/simulation/include/PWM.h b/wpilibc/simulation/include/PWM.h
index 52007fb..bc0b7da 100644
--- a/wpilibc/simulation/include/PWM.h
+++ b/wpilibc/simulation/include/PWM.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "SensorBase.h"
diff --git a/wpilibc/simulation/include/Relay.h b/wpilibc/simulation/include/Relay.h
index abc9ad5..5a1c680 100644
--- a/wpilibc/simulation/include/Relay.h
+++ b/wpilibc/simulation/include/Relay.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "MotorSafety.h"
diff --git a/wpilibc/simulation/include/RobotBase.h b/wpilibc/simulation/include/RobotBase.h
index a88dbb2..ff9c015 100644
--- a/wpilibc/simulation/include/RobotBase.h
+++ b/wpilibc/simulation/include/RobotBase.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "Base.h"
@@ -47,6 +49,7 @@
RobotBase& operator=(const RobotBase&) = delete;
DriverStation &m_ds;
+ transport::SubscriberPtr time_sub;
private:
static RobotBase *m_instance;
diff --git a/wpilibc/simulation/include/RobotDrive.h b/wpilibc/simulation/include/RobotDrive.h
index a181e46..8e9f8ca 100644
--- a/wpilibc/simulation/include/RobotDrive.h
+++ b/wpilibc/simulation/include/RobotDrive.h
@@ -1,12 +1,15 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "ErrorBase.h"
#include <stdlib.h>
+#include <memory>
#include "MotorSafety.h"
#include "MotorSafetyHelper.h"
@@ -37,11 +40,17 @@
uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel);
RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor);
RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor);
+ RobotDrive(std::shared_ptr<SpeedController> leftMotor,
+ std::shared_ptr<SpeedController> rightMotor);
RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
SpeedController *frontRightMotor, SpeedController *rearRightMotor);
RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
SpeedController &frontRightMotor, SpeedController &rearRightMotor);
- virtual ~RobotDrive();
+ RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
+ std::shared_ptr<SpeedController> rearLeftMotor,
+ std::shared_ptr<SpeedController> frontRightMotor,
+ std::shared_ptr<SpeedController> rearRightMotor);
+ virtual ~RobotDrive() = default;
RobotDrive(const RobotDrive&) = delete;
RobotDrive& operator=(const RobotDrive&) = delete;
@@ -85,14 +94,14 @@
static const int32_t kMaxNumberOfMotors = 4;
- int32_t m_invertedMotors[kMaxNumberOfMotors];
+ int32_t m_invertedMotors[kMaxNumberOfMotors] = {1,1,1,1};
float m_sensitivity = 0.5;
double m_maxOutput = 1.0;
bool m_deleteSpeedControllers;
- SpeedController *m_frontLeftMotor = nullptr;
- SpeedController *m_frontRightMotor = nullptr;
- SpeedController *m_rearLeftMotor = nullptr;
- SpeedController *m_rearRightMotor = nullptr;
+ std::shared_ptr<SpeedController> m_frontLeftMotor;
+ std::shared_ptr<SpeedController> m_frontRightMotor;
+ std::shared_ptr<SpeedController> m_rearLeftMotor;
+ std::shared_ptr<SpeedController> m_rearRightMotor;
// FIXME: MotorSafetyHelper *m_safetyHelper;
private:
diff --git a/wpilibc/simulation/include/SafePWM.h b/wpilibc/simulation/include/SafePWM.h
index 577d853..4787cee 100644
--- a/wpilibc/simulation/include/SafePWM.h
+++ b/wpilibc/simulation/include/SafePWM.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "MotorSafety.h"
diff --git a/wpilibc/simulation/include/SampleRobot.h b/wpilibc/simulation/include/SampleRobot.h
index 38cb37d..a87bb4e 100644
--- a/wpilibc/simulation/include/SampleRobot.h
+++ b/wpilibc/simulation/include/SampleRobot.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "RobotBase.h"
diff --git a/wpilibc/simulation/include/Solenoid.h b/wpilibc/simulation/include/Solenoid.h
index b91da6e..4f6ac3a 100644
--- a/wpilibc/simulation/include/Solenoid.h
+++ b/wpilibc/simulation/include/Solenoid.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "simulation/SimContinuousOutput.h"
diff --git a/wpilibc/simulation/include/SpeedController.h b/wpilibc/simulation/include/SpeedController.h
index 96b6b17..e665d06 100644
--- a/wpilibc/simulation/include/SpeedController.h
+++ b/wpilibc/simulation/include/SpeedController.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "PIDOutput.h"
diff --git a/wpilibc/simulation/include/Talon.h b/wpilibc/simulation/include/Talon.h
index 4b7794a..d56d5ba 100644
--- a/wpilibc/simulation/include/Talon.h
+++ b/wpilibc/simulation/include/Talon.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "SafePWM.h"
diff --git a/wpilibc/simulation/include/Victor.h b/wpilibc/simulation/include/Victor.h
index b629bb4..ef3646e 100644
--- a/wpilibc/simulation/include/Victor.h
+++ b/wpilibc/simulation/include/Victor.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "SafePWM.h"
diff --git a/wpilibc/simulation/include/WPILib.h b/wpilibc/simulation/include/WPILib.h
index b3477d4..d4e9bf2 100644
--- a/wpilibc/simulation/include/WPILib.h
+++ b/wpilibc/simulation/include/WPILib.h
@@ -47,7 +47,7 @@
#include "Counter.h"
#include "DigitalInput.h"
#include "Encoder.h"
-#include "Gyro.h"
+#include "AnalogGyro.h"
#include "GenericHID.h"
#include "Joystick.h"
#include "PIDController.h"
diff --git a/wpilibc/simulation/include/simulation/MainNode.h b/wpilibc/simulation/include/simulation/MainNode.h
index a76ef50..416c7a5 100644
--- a/wpilibc/simulation/include/simulation/MainNode.h
+++ b/wpilibc/simulation/include/simulation/MainNode.h
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#ifndef _SIM_MAIN_NODE_H
#define _SIM_MAIN_NODE_H
@@ -39,10 +45,16 @@
transport::NodePtr main;
private:
MainNode() {
- gazebo::client::setup();
- main = transport::NodePtr(new transport::Node());
- main->Init("frc");
- gazebo::transport::run();
+ bool success = gazebo::client::setup();
+
+ if (success){
+ main = transport::NodePtr(new transport::Node());
+ main->Init("frc");
+ gazebo::transport::run();
+ }
+ else {
+ std::cout << "An error has occured setting up gazebo_client!" << std::endl;
+ }
}
};
diff --git a/wpilibc/simulation/include/simulation/SimContinuousOutput.h b/wpilibc/simulation/include/simulation/SimContinuousOutput.h
index 06a28e9..2c0d5da 100644
--- a/wpilibc/simulation/include/simulation/SimContinuousOutput.h
+++ b/wpilibc/simulation/include/simulation/SimContinuousOutput.h
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#ifndef _SIM_SPEED_CONTROLLER_H
diff --git a/wpilibc/simulation/include/simulation/SimDigitalInput.h b/wpilibc/simulation/include/simulation/SimDigitalInput.h
index c85c19f..ca14b0f 100644
--- a/wpilibc/simulation/include/simulation/SimDigitalInput.h
+++ b/wpilibc/simulation/include/simulation/SimDigitalInput.h
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#ifndef _SIM_DIGITAL_INPUT_H
diff --git a/wpilibc/simulation/include/simulation/SimEncoder.h b/wpilibc/simulation/include/simulation/SimEncoder.h
index 9f37723..254cbe4 100644
--- a/wpilibc/simulation/include/simulation/SimEncoder.h
+++ b/wpilibc/simulation/include/simulation/SimEncoder.h
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#ifndef _SIM_ENCODER_H
diff --git a/wpilibc/simulation/include/simulation/SimFloatInput.h b/wpilibc/simulation/include/simulation/SimFloatInput.h
index 6271b96..bdc1c4c 100644
--- a/wpilibc/simulation/include/simulation/SimFloatInput.h
+++ b/wpilibc/simulation/include/simulation/SimFloatInput.h
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#ifndef _SIM_FLOAT_INPUT_H
diff --git a/wpilibc/simulation/include/simulation/SimGyro.h b/wpilibc/simulation/include/simulation/SimGyro.h
index fcb81f6..85d9d27 100644
--- a/wpilibc/simulation/include/simulation/SimGyro.h
+++ b/wpilibc/simulation/include/simulation/SimGyro.h
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#ifndef _SIM_GYRO_H
diff --git a/wpilibc/simulation/include/simulation/simTime.h b/wpilibc/simulation/include/simulation/simTime.h
index 20fe0c5..bd87ffa 100644
--- a/wpilibc/simulation/include/simulation/simTime.h
+++ b/wpilibc/simulation/include/simulation/simTime.h
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
#pragma once
diff --git a/wpilibc/simulation/src/AnalogGyro.cpp b/wpilibc/simulation/src/AnalogGyro.cpp
new file mode 100644
index 0000000..32196a8
--- /dev/null
+++ b/wpilibc/simulation/src/AnalogGyro.cpp
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogGyro.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+const uint32_t AnalogGyro::kOversampleBits = 10;
+const uint32_t AnalogGyro::kAverageBits = 0;
+const float AnalogGyro::kSamplesPerSecond = 50.0;
+const float AnalogGyro::kCalibrationSampleTime = 5.0;
+const float AnalogGyro::kDefaultVoltsPerDegreePerSecond = 0.007;
+
+/**
+ * Initialize the gyro.
+ * Calibrate the gyro by running for a number of samples and computing the center value for this
+ * part. Then use the center value as the Accumulator center value for subsequent measurements.
+ * It's important to make sure that the robot is not moving while the centering calculations are
+ * in progress, this is typically done when the robot is first turned on while it's sitting at
+ * rest before the competition starts.
+ */
+void AnalogGyro::InitAnalogGyro(int channel)
+{
+ SetPIDSourceType(PIDSourceType::kDisplacement);
+
+ char buffer[50];
+ int n = sprintf(buffer, "analog/%d", channel);
+ impl = new SimGyro(buffer);
+
+ LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this);
+}
+
+/**
+ * AnalogGyro constructor with only a channel..
+ *
+ * @param channel The analog channel the gyro is connected to.
+ */
+AnalogGyro::AnalogGyro(uint32_t channel)
+{
+ InitAnalogGyro(channel);
+}
+
+/**
+ * Reset the gyro.
+ * Resets the gyro to a heading of zero. This can be used if there is significant
+ * drift in the gyro and it needs to be recalibrated after it has been running.
+ */
+void AnalogGyro::Reset()
+{
+ impl->Reset();
+}
+
+void AnalogGyro::Calibrate(){
+ Reset();
+}
+
+/**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by the oversampling rate, the
+ * gyro type and the A/D calibration values.
+ * The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
+ * want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
+ *
+ * @return the current heading of the robot in degrees. This heading is based on integration
+ * of the returned rate from the gyro.
+ */
+float AnalogGyro::GetAngle() const
+{
+ return impl->GetAngle();
+}
+
+
+/**
+ * Return the rate of rotation of the gyro
+ *
+ * The rate is based on the most recent reading of the gyro analog value
+ *
+ * @return the current rate in degrees per second
+ */
+double AnalogGyro::GetRate() const
+{
+ return impl->GetVelocity();
+}
diff --git a/wpilibc/simulation/src/AnalogInput.cpp b/wpilibc/simulation/src/AnalogInput.cpp
index 71c48c5..7979edf 100644
--- a/wpilibc/simulation/src/AnalogInput.cpp
+++ b/wpilibc/simulation/src/AnalogInput.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "AnalogInput.h"
diff --git a/wpilibc/simulation/src/AnalogPotentiometer.cpp b/wpilibc/simulation/src/AnalogPotentiometer.cpp
index fa312c1..c71fda9 100644
--- a/wpilibc/simulation/src/AnalogPotentiometer.cpp
+++ b/wpilibc/simulation/src/AnalogPotentiometer.cpp
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#include "AnalogPotentiometer.h"
diff --git a/wpilibc/simulation/src/DigitalInput.cpp b/wpilibc/simulation/src/DigitalInput.cpp
index e7ae36d..5911fb3 100644
--- a/wpilibc/simulation/src/DigitalInput.cpp
+++ b/wpilibc/simulation/src/DigitalInput.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "DigitalInput.h"
diff --git a/wpilibc/simulation/src/DoubleSolenoid.cpp b/wpilibc/simulation/src/DoubleSolenoid.cpp
index d1d0917..6180fb4 100644
--- a/wpilibc/simulation/src/DoubleSolenoid.cpp
+++ b/wpilibc/simulation/src/DoubleSolenoid.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "DoubleSolenoid.h"
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);
}
diff --git a/wpilibc/simulation/src/Encoder.cpp b/wpilibc/simulation/src/Encoder.cpp
index 790868e..565a8eb 100644
--- a/wpilibc/simulation/src/Encoder.cpp
+++ b/wpilibc/simulation/src/Encoder.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Encoder.h"
diff --git a/wpilibc/simulation/src/Gyro.cpp b/wpilibc/simulation/src/Gyro.cpp
deleted file mode 100644
index a28bb9b..0000000
--- a/wpilibc/simulation/src/Gyro.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "Gyro.h"
-#include "Timer.h"
-#include "WPIErrors.h"
-#include "LiveWindow/LiveWindow.h"
-
-const uint32_t Gyro::kOversampleBits = 10;
-const uint32_t Gyro::kAverageBits = 0;
-const float Gyro::kSamplesPerSecond = 50.0;
-const float Gyro::kCalibrationSampleTime = 5.0;
-const float Gyro::kDefaultVoltsPerDegreePerSecond = 0.007;
-
-/**
- * Initialize the gyro.
- * Calibrate the gyro by running for a number of samples and computing the center value for this
- * part. Then use the center value as the Accumulator center value for subsequent measurements.
- * It's important to make sure that the robot is not moving while the centering calculations are
- * in progress, this is typically done when the robot is first turned on while it's sitting at
- * rest before the competition starts.
- */
-void Gyro::InitGyro(int channel)
-{
- SetPIDSourceType(PIDSourceType::kDisplacement);
-
- char buffer[50];
- int n = sprintf(buffer, "analog/%d", channel);
- impl = new SimGyro(buffer);
-
- LiveWindow::GetInstance()->AddSensor("Gyro", channel, this);
-}
-
-/**
- * Gyro constructor with only a channel..
- *
- * @param channel The analog channel the gyro is connected to.
- */
-Gyro::Gyro(uint32_t channel)
-{
- InitGyro(channel);
-}
-
-/**
- * Reset the gyro.
- * Resets the gyro to a heading of zero. This can be used if there is significant
- * drift in the gyro and it needs to be recalibrated after it has been running.
- */
-void Gyro::Reset()
-{
- impl->Reset();
-}
-
-/**
- * Return the actual angle in degrees that the robot is currently facing.
- *
- * The angle is based on the current accumulator value corrected by the oversampling rate, the
- * gyro type and the A/D calibration values.
- * The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
- * want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
- *
- * @return the current heading of the robot in degrees. This heading is based on integration
- * of the returned rate from the gyro.
- */
-float Gyro::GetAngle() const
-{
- return impl->GetAngle();
-}
-
-
-/**
- * Return the rate of rotation of the gyro
- *
- * The rate is based on the most recent reading of the gyro analog value
- *
- * @return the current rate in degrees per second
- */
-double Gyro::GetRate() const
-{
- return impl->GetVelocity();
-}
-
-void Gyro::SetPIDSourceType(PIDSourceType pidSource)
-{
- m_pidSource = pidSource;
-}
-
-/**
- * Get the angle in degrees for the PIDSource base object.
- *
- * @return The angle in degrees.
- */
-double Gyro::PIDGet()
-{
- switch(GetPIDSourceType()){
- case PIDSourceType::kRate:
- return GetRate();
- case PIDSourceType::kDisplacement:
- return GetAngle();
- default:
- return 0;
- }
-}
-
-void Gyro::UpdateTable() {
- if (m_table != nullptr) {
- m_table->PutNumber("Value", GetAngle());
- }
-}
-
-void Gyro::StartLiveWindowMode() {
-
-}
-
-void Gyro::StopLiveWindowMode() {
-
-}
-
-std::string Gyro::GetSmartDashboardType() const {
- return "Gyro";
-}
-
-void Gyro::InitTable(std::shared_ptr<ITable> subTable) {
- m_table = subTable;
- UpdateTable();
-}
-
-std::shared_ptr<ITable> Gyro::GetTable() const {
- return m_table;
-}
diff --git a/wpilibc/simulation/src/IterativeRobot.cpp b/wpilibc/simulation/src/IterativeRobot.cpp
index f2511df..d78e9f3 100644
--- a/wpilibc/simulation/src/IterativeRobot.cpp
+++ b/wpilibc/simulation/src/IterativeRobot.cpp
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* 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 "IterativeRobot.h"
#include "DriverStation.h"
diff --git a/wpilibc/simulation/src/Jaguar.cpp b/wpilibc/simulation/src/Jaguar.cpp
index fab109d..148f629 100644
--- a/wpilibc/simulation/src/Jaguar.cpp
+++ b/wpilibc/simulation/src/Jaguar.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
/*----------------------------------------------------------------------------*/
diff --git a/wpilibc/simulation/src/Joystick.cpp b/wpilibc/simulation/src/Joystick.cpp
index a7a299c..3c32e86 100644
--- a/wpilibc/simulation/src/Joystick.cpp
+++ b/wpilibc/simulation/src/Joystick.cpp
@@ -1,16 +1,15 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Joystick.h"
#include "DriverStation.h"
-//#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include <math.h>
-#include <string.h>
+#include <memory>
const uint32_t Joystick::kDefaultXAxis;
const uint32_t Joystick::kDefaultYAxis;
@@ -26,21 +25,19 @@
* Construct an instance of a joystick.
* The joystick index is the usb port on the drivers station.
*
- * @param port The port on the driver station that the joystick is plugged into
- * (0-5).
+ * @param port The port on the driver station that the joystick is plugged into.
*/
Joystick::Joystick(uint32_t port)
- : Joystick(port, kNumAxisTypes, kNumButtonTypes) {
- m_axes[kXAxis] = kDefaultXAxis;
- m_axes[kYAxis] = kDefaultYAxis;
- m_axes[kZAxis] = kDefaultZAxis;
- m_axes[kTwistAxis] = kDefaultTwistAxis;
- m_axes[kThrottleAxis] = kDefaultThrottleAxis;
+ : Joystick(port, kNumAxisTypes, kNumButtonTypes)
+{
+ m_axes[kXAxis] = kDefaultXAxis;
+ m_axes[kYAxis] = kDefaultYAxis;
+ m_axes[kZAxis] = kDefaultZAxis;
+ m_axes[kTwistAxis] = kDefaultTwistAxis;
+ m_axes[kThrottleAxis] = kDefaultThrottleAxis;
- m_buttons[kTriggerButton] = kDefaultTriggerButton;
- m_buttons[kTopButton] = kDefaultTopButton;
-
- HALReport(HALUsageReporting::kResourceType_Joystick, port);
+ m_buttons[kTriggerButton] = kDefaultTriggerButton;
+ m_buttons[kTopButton] = kDefaultTopButton;
}
/**
@@ -53,108 +50,111 @@
* @param numAxisTypes The number of axis types in the enum.
* @param numButtonTypes The number of button types in the enum.
*/
-Joystick::Joystick(uint32_t port, uint32_t numAxisTypes,
- uint32_t numButtonTypes)
- : m_ds(DriverStation::GetInstance()),
- m_port(port),
- m_axes(numAxisTypes),
- m_buttons(numButtonTypes) {
- if (!joySticksInitialized) {
- for (auto& joystick : joysticks) joystick = nullptr;
- joySticksInitialized = true;
- }
- if (m_port >= DriverStation::kJoystickPorts) {
- wpi_setWPIError(BadJoystickIndex);
- } else {
- joysticks[m_port] = this;
- }
+Joystick::Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes)
+ : m_port (port),
+ m_ds(DriverStation::GetInstance())
+{
+ if ( !joySticksInitialized )
+ {
+ for (unsigned i = 0; i < DriverStation::kJoystickPorts; i++)
+ joysticks[i] = nullptr;
+ joySticksInitialized = true;
+ }
+ joysticks[m_port] = this;
+
+ m_axes = std::make_unique<uint32_t[]>(numAxisTypes);
+ m_buttons = std::make_unique<uint32_t[]>(numButtonTypes);
}
-Joystick *Joystick::GetStickForPort(uint32_t port) {
- Joystick *stick = joysticks[port];
- if (stick == nullptr) {
- stick = new Joystick(port);
- joysticks[port] = stick;
- }
- return stick;
+Joystick * Joystick::GetStickForPort(uint32_t port)
+{
+ Joystick *stick = joysticks[port];
+ if (stick == nullptr)
+ {
+ stick = new Joystick(port);
+ joysticks[port] = stick;
+ }
+ return stick;
}
/**
* Get the X value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
- * @param hand This parameter is ignored for the Joystick class and is only here
- * to complete the GenericHID interface.
*/
-float Joystick::GetX(JoystickHand hand) const {
- return GetRawAxis(m_axes[kXAxis]);
+float Joystick::GetX(JoystickHand hand) const
+{
+ return GetRawAxis(m_axes[kXAxis]);
}
/**
* Get the Y value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
- * @param hand This parameter is ignored for the Joystick class and is only here
- * to complete the GenericHID interface.
*/
-float Joystick::GetY(JoystickHand hand) const {
- return GetRawAxis(m_axes[kYAxis]);
+float Joystick::GetY(JoystickHand hand) const
+{
+ return GetRawAxis(m_axes[kYAxis]);
}
/**
* Get the Z value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
-float Joystick::GetZ() const { return GetRawAxis(m_axes[kZAxis]); }
+float Joystick::GetZ() const
+{
+ return GetRawAxis(m_axes[kZAxis]);
+}
/**
* Get the twist value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
-float Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); }
+float Joystick::GetTwist() const
+{
+ return GetRawAxis(m_axes[kTwistAxis]);
+}
/**
* Get the throttle value of the current joystick.
* This depends on the mapping of the joystick connected to the current port.
*/
-float Joystick::GetThrottle() const {
- return GetRawAxis(m_axes[kThrottleAxis]);
+float Joystick::GetThrottle() const
+{
+ return GetRawAxis(m_axes[kThrottleAxis]);
}
/**
* Get the value of the axis.
*
- * @param axis The axis to read, starting at 0.
+ * @param axis The axis to read [1-6].
* @return The value of the axis.
*/
-float Joystick::GetRawAxis(uint32_t axis) const {
- return m_ds.GetStickAxis(m_port, axis);
+float Joystick::GetRawAxis(uint32_t axis) const
+{
+ return m_ds.GetStickAxis(m_port, axis);
}
/**
* For the current joystick, return the axis determined by the argument.
*
- * This is for cases where the joystick axis is returned programatically,
- * otherwise one of the
+ * This is for cases where the joystick axis is returned programatically, otherwise one of the
* previous functions would be preferable (for example GetX()).
*
* @param axis The axis to read.
* @return The value of the axis.
*/
-float Joystick::GetAxis(AxisType axis) const {
- switch (axis) {
- case kXAxis:
- return this->GetX();
- case kYAxis:
- return this->GetY();
- case kZAxis:
- return this->GetZ();
- case kTwistAxis:
- return this->GetTwist();
- case kThrottleAxis:
- return this->GetThrottle();
- default:
- wpi_setWPIError(BadJoystickAxis);
- return 0.0;
- }
+float Joystick::GetAxis(AxisType axis) const
+{
+ switch(axis)
+ {
+ case kXAxis: return this->GetX();
+ case kYAxis: return this->GetY();
+ case kZAxis: return this->GetZ();
+ case kTwistAxis: return this->GetTwist();
+ case kThrottleAxis: return this->GetThrottle();
+ default:
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
}
/**
@@ -162,12 +162,12 @@
*
* Look up which button has been assigned to the trigger and read its state.
*
- * @param hand This parameter is ignored for the Joystick class and is only here
- * to complete the GenericHID interface.
+ * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @return The state of the trigger.
*/
-bool Joystick::GetTrigger(JoystickHand hand) const {
- return GetRawButton(m_buttons[kTriggerButton]);
+bool Joystick::GetTrigger(JoystickHand hand) const
+{
+ return GetRawButton(m_buttons[kTriggerButton]);
}
/**
@@ -175,45 +175,45 @@
*
* Look up which button has been assigned to the top and read its state.
*
- * @param hand This parameter is ignored for the Joystick class and is only here
- * to complete the GenericHID interface.
+ * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @return The state of the top button.
*/
-bool Joystick::GetTop(JoystickHand hand) const {
- return GetRawButton(m_buttons[kTopButton]);
+bool Joystick::GetTop(JoystickHand hand) const
+{
+ return GetRawButton(m_buttons[kTopButton]);
}
/**
* This is not supported for the Joystick.
* This method is only here to complete the GenericHID interface.
*/
-bool Joystick::GetBumper(JoystickHand hand) const {
- // Joysticks don't have bumpers.
- return false;
+bool Joystick::GetBumper(JoystickHand hand) const
+{
+ // Joysticks don't have bumpers.
+ return false;
}
/**
- * Get the button value (starting at button 1)
+ * Get the button value for buttons 1 through 12.
*
- * The buttons are returned in a single 16 bit value with one bit representing
- * the state
+ * The buttons are returned in a single 16 bit value with one bit representing the state
* of each button. The appropriate button is returned as a boolean value.
*
- * @param button The button number to be read (starting at 1)
+ * @param button The button number to be read.
* @return The state of the button.
**/
-bool Joystick::GetRawButton(uint32_t button) const {
- return m_ds.GetStickButton(m_port, button);
+bool Joystick::GetRawButton(uint32_t button) const
+{
+ return m_ds.GetStickButton(m_port, button);
}
/**
- * Get the state of a POV on the joystick.
- *
- * @param pov The index of the POV to read (starting at 0)
- * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
- */
+* Get the state of a POV on the joystick.
+*
+* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+*/
int Joystick::GetPOV(uint32_t pov) const {
- return m_ds.GetStickPOV(m_port, pov);
+ return 0; // TODO
}
/**
@@ -224,75 +224,27 @@
* @param button The type of button to read.
* @return The state of the button.
*/
-bool Joystick::GetButton(ButtonType button) const {
- switch (button) {
- case kTriggerButton:
- return GetTrigger();
- case kTopButton:
- return GetTop();
- default:
- return false;
- }
+bool Joystick::GetButton(ButtonType button) const
+{
+ switch (button)
+ {
+ case kTriggerButton: return GetTrigger();
+ case kTopButton: return GetTop();
+ default:
+ return false;
+ }
}
/**
- * Get the number of axis for a joystick
- *
- * @return the number of axis for the current joystick
- */
-int Joystick::GetAxisCount() const { return m_ds.GetStickAxisCount(m_port); }
-
-/**
- * Get the value of isXbox for the joystick.
- *
- * @return A boolean that is true if the joystick is an xbox controller.
- */
-bool Joystick::GetIsXbox() const { return m_ds.GetJoystickIsXbox(m_port); }
-
-/**
- * Get the HID type of the controller.
- *
- * @return the HID type of the controller.
- */
-Joystick::HIDType Joystick::GetType() const {
- return static_cast<HIDType>(m_ds.GetJoystickType(m_port));
-}
-
-/**
- * Get the name of the joystick.
- *
- * @return the name of the controller.
- */
-std::string Joystick::GetName() const { return m_ds.GetJoystickName(m_port); }
-
-// int Joystick::GetAxisType(uint8_t axis) const
-//{
-// return m_ds.GetJoystickAxisType(m_port, axis);
-//}
-
-/**
- * Get the number of axis for a joystick
- *
-* @return the number of buttons on the current joystick
- */
-int Joystick::GetButtonCount() const {
- return m_ds.GetStickButtonCount(m_port);
-}
-
-/**
- * Get the number of axis for a joystick
- *
- * @return then umber of POVs for the current joystick
- */
-int Joystick::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); }
-
-/**
* Get the channel currently associated with the specified axis.
*
* @param axis The axis to look up the channel for.
* @return The channel fr the axis.
*/
-uint32_t Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; }
+uint32_t Joystick::GetAxisChannel(AxisType axis)
+{
+ return m_axes[axis];
+}
/**
* Set the channel associated with a specified axis.
@@ -300,8 +252,9 @@
* @param axis The axis to set the channel for.
* @param channel The channel to set the axis to.
*/
-void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) {
- m_axes[axis] = channel;
+void Joystick::SetAxisChannel(AxisType axis, uint32_t channel)
+{
+ m_axes[axis] = channel;
}
/**
@@ -311,7 +264,7 @@
* @return The magnitude of the direction vector
*/
float Joystick::GetMagnitude() const {
- return sqrt(pow(GetX(), 2) + pow(GetY(), 2));
+ return sqrt(pow(GetX(),2) + pow(GetY(),2) );
}
/**
@@ -320,58 +273,19 @@
*
* @return The direction of the vector in radians
*/
-float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); }
+float Joystick::GetDirectionRadians() const {
+ return atan2(GetX(), -GetY());
+}
/**
* Get the direction of the vector formed by the joystick and its origin
* in degrees
*
- * uses acos(-1) to represent Pi due to absence of readily accessible Pi
+ * uses acos(-1) to represent Pi due to absence of readily accessable Pi
* constant in C++
*
* @return The direction of the vector in degrees
*/
float Joystick::GetDirectionDegrees() const {
- return (180 / acos(-1)) * GetDirectionRadians();
-}
-
-/**
- * Set the rumble output for the joystick. The DS currently supports 2 rumble
- * values,
- * left rumble and right rumble
- * @param type Which rumble value to set
- * @param value The normalized value (0 to 1) to set the rumble to
- */
-void Joystick::SetRumble(RumbleType type, float value) {
- if (value < 0)
- value = 0;
- else if (value > 1)
- value = 1;
- if (type == kLeftRumble)
- m_leftRumble = value * 65535;
- else
- m_rightRumble = value * 65535;
- HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
-}
-
-/**
- * Set a single HID output value for the joystick.
- * @param outputNumber The index of the output to set (1-32)
- * @param value The value to set the output to
- */
-
-void Joystick::SetOutput(uint8_t outputNumber, bool value) {
- m_outputs =
- (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
-
- HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
-}
-
-/**
- * Set all HID output values for the joystick.
- * @param value The 32 bit output value (1 bit for each output)
- */
-void Joystick::SetOutputs(uint32_t value) {
- m_outputs = value;
- HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+ return (180/acos(-1))*GetDirectionRadians();
}
diff --git a/wpilibc/simulation/src/MotorSafetyHelper.cpp b/wpilibc/simulation/src/MotorSafetyHelper.cpp
index 87f5352..47db177 100644
--- a/wpilibc/simulation/src/MotorSafetyHelper.cpp
+++ b/wpilibc/simulation/src/MotorSafetyHelper.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "MotorSafetyHelper.h"
diff --git a/wpilibc/simulation/src/Notifier.cpp b/wpilibc/simulation/src/Notifier.cpp
index 25daaf0..eb19487 100644
--- a/wpilibc/simulation/src/Notifier.cpp
+++ b/wpilibc/simulation/src/Notifier.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Notifier.h"
@@ -9,7 +10,7 @@
#include "Utility.h"
#include "WPIErrors.h"
-Notifier *Notifier::timerQueueHead = nullptr;
+std::list<Notifier*> Notifier::timerQueue;
priority_recursive_mutex Notifier::queueMutex;
std::atomic<int> Notifier::refcount{0};
std::thread Notifier::m_task;
@@ -20,16 +21,14 @@
* @param handler The handler is called at the notification time which is set
* using StartSingle or StartPeriodic.
*/
-Notifier::Notifier(TimerEventHandler handler, void *param)
+Notifier::Notifier(TimerEventHandler handler)
{
if (handler == nullptr)
wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
m_handler = handler;
- m_param = param;
m_periodic = false;
m_expirationTime = 0;
m_period = 0;
- m_nextEvent = nullptr;
m_queued = false;
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
@@ -89,13 +88,20 @@
{
std::lock_guard<priority_recursive_mutex> sync(queueMutex);
double currentTime = GetClock();
- current = timerQueueHead;
- if (current == nullptr || current->m_expirationTime > currentTime)
+
+ if (timerQueue.empty())
+ {
+ break;
+ }
+ current = timerQueue.front();
+ if (current->m_expirationTime > currentTime)
{
break; // no more timer events to process
}
- // need to process this entry
- timerQueueHead = current->m_nextEvent;
+ // remove next entry before processing it
+ timerQueue.pop_front();
+
+ current->m_queued = false;
if (current->m_periodic)
{
// if periodic, requeue the event
@@ -112,7 +118,7 @@
current->m_handlerMutex.lock();
}
- current->m_handler(current->m_param); // call the event handler
+ current->m_handler(); // call the event handler
current->m_handlerMutex.unlock();
}
// reschedule the first item in the queue
@@ -139,32 +145,34 @@
{
m_expirationTime = GetClock() + m_period;
}
- if (timerQueueHead == nullptr || timerQueueHead->m_expirationTime >= this->m_expirationTime)
+
+ // Attempt to insert new entry into queue
+ for (auto i = timerQueue.begin(); i != timerQueue.end(); i++)
{
- // the queue is empty or greater than the new entry
- // the new entry becomes the first element
- this->m_nextEvent = timerQueueHead;
- timerQueueHead = this;
+ if ((*i)->m_expirationTime > m_expirationTime)
+ {
+ timerQueue.insert(i, this);
+ m_queued = true;
+ }
+ }
+
+ /* If the new entry wasn't queued, either the queue was empty or the first
+ * element was greater than the new entry.
+ */
+ if (!m_queued)
+ {
+ timerQueue.push_front(this);
+
if (!reschedule)
{
- // since the first element changed, update alarm, unless we already plan to
+ /* Since the first element changed, update alarm, unless we already
+ * plan to
+ */
UpdateAlarm();
}
+
+ m_queued = true;
}
- else
- {
- for (Notifier **npp = &(timerQueueHead->m_nextEvent); ; npp = &(*npp)->m_nextEvent)
- {
- Notifier *n = *npp;
- if (n == nullptr || n->m_expirationTime > this->m_expirationTime)
- {
- *npp = this;
- this->m_nextEvent = n;
- break;
- }
- }
- }
- m_queued = true;
}
/**
@@ -179,23 +187,16 @@
if (m_queued)
{
m_queued = false;
- wpi_assert(timerQueueHead != nullptr);
- if (timerQueueHead == this)
+ wpi_assert(!timerQueue.empty());
+ if (timerQueue.front() == this)
{
// remove the first item in the list - update the alarm
- timerQueueHead = this->m_nextEvent;
+ timerQueue.pop_front();
UpdateAlarm();
}
else
{
- for (Notifier *n = timerQueueHead; n != nullptr; n = n->m_nextEvent)
- {
- if (n->m_nextEvent == this)
- {
- // this element is the next element from *n from the queue
- n->m_nextEvent = this->m_nextEvent; // point around this one
- }
- }
+ timerQueue.remove(this);
}
}
}
@@ -249,9 +250,19 @@
void Notifier::Run() {
while (!m_stopped) {
Notifier::ProcessQueue(0, nullptr);
- if (timerQueueHead != nullptr)
+ bool isEmpty;
{
- Wait(timerQueueHead->m_expirationTime - GetClock());
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ isEmpty = timerQueue.empty();
+ }
+ if (!isEmpty)
+ {
+ double expirationTime;
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ expirationTime = timerQueue.front()->m_expirationTime;
+ }
+ Wait(expirationTime - GetClock());
}
else
{
diff --git a/wpilibc/simulation/src/PIDController.cpp b/wpilibc/simulation/src/PIDController.cpp
index 03beb5c..d3c18b5 100644
--- a/wpilibc/simulation/src/PIDController.cpp
+++ b/wpilibc/simulation/src/PIDController.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "PIDController.h"
@@ -74,7 +75,7 @@
m_enabled = false;
m_setpoint = 0;
- m_prevInput = 0;
+ m_prevError = 0;
m_totalError = 0;
m_tolerance = .05;
@@ -84,7 +85,7 @@
m_pidOutput = output;
m_period = period;
- m_controlLoop = std::make_unique<Notifier>(PIDController::CallCalculate, this);
+ m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_controlLoop->StartPeriodic(m_period);
static int32_t instances = 0;
@@ -98,30 +99,16 @@
}
/**
- * Call the Calculate method as a non-static method. This avoids having to prepend
- * all local variables in that method with the class pointer. This way the "this"
- * pointer will be set up and class variables can be called more easily.
- * This method is static and called by the Notifier class.
- * @param controller the address of the PID controller object to use in the background loop
+ * Read the input, calculate the output accordingly, and write to the output.
+ * This should only be called by the Notifier.
*/
-void PIDController::CallCalculate(void *controller)
-{
- PIDController *control = (PIDController*) controller;
- control->Calculate();
-}
-
- /**
- * Read the input, calculate the output accordingly, and write to the output.
- * This should only be called by the Notifier indirectly through CallCalculate
- * and is created during initialization.
- */
void PIDController::Calculate()
{
bool enabled;
PIDSource *pidInput;
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
if (m_pidInput == 0) return;
if (m_pidOutput == 0) return;
enabled = m_enabled;
@@ -135,7 +122,7 @@
PIDOutput *pidOutput;
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_error = m_setpoint - input;
if (m_continuous)
{
@@ -168,7 +155,8 @@
}
}
- m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
+ m_result = m_D * m_error + m_P * m_totalError +
+ CalculateFeedForward();
}
else {
if (m_I != 0) {
@@ -186,9 +174,10 @@
}
}
- m_result = m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
+ m_result = m_P * m_error + m_I * m_totalError +
+ m_D * (m_error - m_prevError) + CalculateFeedForward();
}
- m_prevInput = input;
+ m_prevError = m_error;
if (m_result > m_maximumOutput) m_result = m_maximumOutput;
else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
@@ -202,6 +191,33 @@
}
/**
+ * Calculate the feed forward term
+ *
+ * Both of the provided feed forward calculations are velocity feed forwards.
+ * If a different feed forward calculation is desired, the user can override
+ * this function and provide his or her own. This function does no
+ * synchronization because the PIDController class only calls it in synchronized
+ * code, so be careful if calling it oneself.
+ *
+ * If a velocity PID controller is being used, the F term should be set to 1
+ * over the maximum setpoint for the output. If a position PID controller is
+ * being used, the F term should be set to 1 over the maximum speed for the
+ * output measured in setpoint units per this controller's update period (see
+ * the default period in this class's constructor).
+ */
+double PIDController::CalculateFeedForward() {
+ if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+ return m_F * GetSetpoint();
+ }
+ else {
+ double temp = m_F * GetDeltaSetpoint();
+ m_prevSetpoint = m_setpoint;
+ m_setpointTimer.Reset();
+ return temp;
+ }
+}
+
+/**
* Set the PID Controller gain parameters.
* Set the proportional, integral, and differential coefficients.
* @param p Proportional coefficient
@@ -211,7 +227,7 @@
void PIDController::SetPID(double p, double i, double d)
{
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -235,7 +251,7 @@
void PIDController::SetPID(double p, double i, double d, double f)
{
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -256,7 +272,7 @@
*/
double PIDController::GetP() const
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_P;
}
@@ -266,7 +282,7 @@
*/
double PIDController::GetI() const
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_I;
}
@@ -276,7 +292,7 @@
*/
double PIDController::GetD() const
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_D;
}
@@ -286,7 +302,7 @@
*/
double PIDController::GetF() const
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_F;
}
@@ -297,7 +313,7 @@
*/
float PIDController::Get() const
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_result;
}
@@ -310,7 +326,7 @@
*/
void PIDController::SetContinuous(bool continuous)
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_continuous = continuous;
}
@@ -323,7 +339,7 @@
void PIDController::SetInputRange(float minimumInput, float maximumInput)
{
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
@@ -339,7 +355,7 @@
*/
void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
@@ -351,7 +367,8 @@
void PIDController::SetSetpoint(float setpoint)
{
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
if (m_maximumInput > m_minimumInput)
{
if (setpoint > m_maximumInput)
@@ -378,11 +395,21 @@
*/
double PIDController::GetSetpoint() const
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_setpoint;
}
/**
+ * Returns the change in setpoint over time of the PIDController
+ * @return the change in setpoint over time
+ */
+double PIDController::GetDeltaSetpoint() const
+{
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
+/**
* Retruns the current difference of the input from the setpoint
* @return the current error
*/
@@ -390,7 +417,7 @@
{
double pidInput;
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
pidInput = m_pidInput->PIDGet();
}
return GetSetpoint() - pidInput;
@@ -420,7 +447,7 @@
float PIDController::GetAvgError() const {
float avgError = 0;
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
// Don't divide by zero.
if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
}
@@ -434,7 +461,7 @@
*/
void PIDController::SetTolerance(float percent)
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -446,7 +473,7 @@
*/
void PIDController::SetPercentTolerance(float percent)
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -458,7 +485,7 @@
*/
void PIDController::SetAbsoluteTolerance(float absTolerance)
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
@@ -473,6 +500,7 @@
* @param bufLength Number of previous cycles to average. Defaults to 1.
*/
void PIDController::SetToleranceBuffer(unsigned bufLength) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_bufLength = bufLength;
// Cut the buffer down to size if needed.
@@ -491,9 +519,9 @@
*/
bool PIDController::OnTarget() const
{
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ if (m_buf.size() == 0) return false;
double error = GetError();
-
- std::lock_guard<priority_mutex> sync(m_mutex);
switch (m_toleranceType) {
case kPercentTolerance:
return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
@@ -513,7 +541,7 @@
void PIDController::Enable()
{
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_enabled = true;
}
@@ -528,7 +556,7 @@
void PIDController::Disable()
{
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_pidOutput->PIDWrite(0);
m_enabled = false;
}
@@ -543,7 +571,7 @@
*/
bool PIDController::IsEnabled() const
{
- std::lock_guard<priority_mutex> lock(m_mutex);
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_enabled;
}
@@ -554,8 +582,8 @@
{
Disable();
- std::lock_guard<priority_mutex> lock(m_mutex);
- m_prevInput = 0;
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_prevError = 0;
m_totalError = 0;
m_result = 0;
}
diff --git a/wpilibc/simulation/src/PWM.cpp b/wpilibc/simulation/src/PWM.cpp
index dfb8bd9..19b30c3 100644
--- a/wpilibc/simulation/src/PWM.cpp
+++ b/wpilibc/simulation/src/PWM.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "PWM.h"
diff --git a/wpilibc/simulation/src/Relay.cpp b/wpilibc/simulation/src/Relay.cpp
index f35d782..7582b52 100644
--- a/wpilibc/simulation/src/Relay.cpp
+++ b/wpilibc/simulation/src/Relay.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Relay.h"
diff --git a/wpilibc/simulation/src/RobotBase.cpp b/wpilibc/simulation/src/RobotBase.cpp
index 1fa5ac7..d5afb7f 100644
--- a/wpilibc/simulation/src/RobotBase.cpp
+++ b/wpilibc/simulation/src/RobotBase.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "RobotBase.h"
@@ -34,7 +35,7 @@
RobotBase::RobotBase() : m_ds(DriverStation::GetInstance())
{
RobotState::SetImplementation(DriverStation::GetInstance());
- transport::SubscriberPtr time_pub = MainNode::Subscribe("time", &wpilib::internal::time_callback);
+ time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback);
}
/**
diff --git a/wpilibc/simulation/src/RobotDrive.cpp b/wpilibc/simulation/src/RobotDrive.cpp
index 852d0bf..b6216d0 100644
--- a/wpilibc/simulation/src/RobotDrive.cpp
+++ b/wpilibc/simulation/src/RobotDrive.cpp
@@ -1,15 +1,15 @@
/*----------------------------------------------------------------------------*/
-/* 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 "RobotDrive.h"
-
//#include "CANJaguar.h"
#include "GenericHID.h"
#include "Joystick.h"
-#include "Jaguar.h"
+#include "Talon.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <math.h>
@@ -19,6 +19,10 @@
const int32_t RobotDrive::kMaxNumberOfMotors;
+static auto make_shared_nodelete(SpeedController *ptr) {
+ return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
+}
+
/*
* Driving functions
* These functions provide an interface to multiple motors that is used for C programming
@@ -39,19 +43,15 @@
/** Constructor for RobotDrive with 2 motors specified with channel numbers.
* Set up parameters for a two wheel drive system where the
* left and right motor pwm channels are specified in the call.
- * This call assumes Jaguars for controlling the motors.
+ * This call assumes Talosn for controlling the motors.
* @param leftMotorChannel The PWM channel number that drives the left motor.
* @param rightMotorChannel The PWM channel number that drives the right motor.
*/
RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
{
InitRobotDrive();
- m_rearLeftMotor = new Jaguar(leftMotorChannel);
- m_rearRightMotor = new Jaguar(rightMotorChannel);
- for (int32_t i=0; i < kMaxNumberOfMotors; i++)
- {
- m_invertedMotors[i] = 1;
- }
+ m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
+ m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
@@ -60,7 +60,7 @@
* Constructor for RobotDrive with 4 motors specified with channel numbers.
* Set up parameters for a four wheel drive system where all four motor
* pwm channels are specified in the call.
- * This call assumes Jaguars for controlling the motors.
+ * This call assumes Talons for controlling the motors.
* @param frontLeftMotor Front left motor channel number
* @param rearLeftMotor Rear Left motor channel number
* @param frontRightMotor Front right motor channel number
@@ -70,14 +70,10 @@
uint32_t frontRightMotor, uint32_t rearRightMotor)
{
InitRobotDrive();
- m_rearLeftMotor = new Jaguar(rearLeftMotor);
- m_rearRightMotor = new Jaguar(rearRightMotor);
- m_frontLeftMotor = new Jaguar(frontLeftMotor);
- m_frontRightMotor = new Jaguar(frontRightMotor);
- for (int32_t i=0; i < kMaxNumberOfMotors; i++)
- {
- m_invertedMotors[i] = 1;
- }
+ m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
+ m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
+ m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
+ m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
@@ -90,34 +86,36 @@
* @param leftMotor The left SpeedController object used to drive the robot.
* @param rightMotor the right SpeedController object used to drive the robot.
*/
-RobotDrive::RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor)
-{
+RobotDrive::RobotDrive(SpeedController *leftMotor,
+ SpeedController *rightMotor) {
InitRobotDrive();
- if (leftMotor == nullptr || rightMotor == nullptr)
- {
+ if (leftMotor == nullptr || rightMotor == nullptr) {
wpi_setWPIError(NullParameter);
m_rearLeftMotor = m_rearRightMotor = nullptr;
return;
}
- m_rearLeftMotor = leftMotor;
- m_rearRightMotor = rightMotor;
- for (int32_t i=0; i < kMaxNumberOfMotors; i++)
- {
- m_invertedMotors[i] = 1;
- }
- m_deleteSpeedControllers = false;
+ m_rearLeftMotor = make_shared_nodelete(leftMotor);
+ m_rearRightMotor = make_shared_nodelete(rightMotor);
}
-RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor)
-{
+//TODO: Change to rvalue references & move syntax.
+RobotDrive::RobotDrive(SpeedController &leftMotor,
+ SpeedController &rightMotor) {
InitRobotDrive();
- m_rearLeftMotor = &leftMotor;
- m_rearRightMotor = &rightMotor;
- for (int32_t i=0; i < kMaxNumberOfMotors; i++)
- {
- m_invertedMotors[i] = 1;
+ m_rearLeftMotor = make_shared_nodelete(&leftMotor);
+ m_rearRightMotor = make_shared_nodelete(&rightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
+ std::shared_ptr<SpeedController> rightMotor) {
+ InitRobotDrive();
+ if (leftMotor == nullptr || rightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ m_rearLeftMotor = m_rearRightMotor = nullptr;
+ return;
}
- m_deleteSpeedControllers = false;
+ m_rearLeftMotor = leftMotor;
+ m_rearRightMotor = rightMotor;
}
/**
@@ -128,12 +126,40 @@
* @param rearRightMotor The back right SpeedController object used to drive the robot.
* @param frontRightMotor The front right SpeedController object used to drive the robot.
*/
-RobotDrive::RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
- SpeedController *frontRightMotor, SpeedController *rearRightMotor)
-{
+RobotDrive::RobotDrive(SpeedController *frontLeftMotor,
+ SpeedController *rearLeftMotor,
+ SpeedController *frontRightMotor,
+ SpeedController *rearRightMotor) {
InitRobotDrive();
- if (frontLeftMotor == nullptr || rearLeftMotor == nullptr || frontRightMotor == nullptr || rearRightMotor == nullptr)
- {
+ if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+ frontRightMotor == nullptr || rearRightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
+ m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
+ m_frontRightMotor = make_shared_nodelete(frontRightMotor);
+ m_rearRightMotor = make_shared_nodelete(rearRightMotor);
+}
+
+RobotDrive::RobotDrive(SpeedController &frontLeftMotor,
+ SpeedController &rearLeftMotor,
+ SpeedController &frontRightMotor,
+ SpeedController &rearRightMotor) {
+ InitRobotDrive();
+ m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
+ m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
+ m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
+ m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
+ std::shared_ptr<SpeedController> rearLeftMotor,
+ std::shared_ptr<SpeedController> frontRightMotor,
+ std::shared_ptr<SpeedController> rearRightMotor) {
+ InitRobotDrive();
+ if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+ frontRightMotor == nullptr || rearRightMotor == nullptr) {
wpi_setWPIError(NullParameter);
return;
}
@@ -141,42 +167,6 @@
m_rearLeftMotor = rearLeftMotor;
m_frontRightMotor = frontRightMotor;
m_rearRightMotor = rearRightMotor;
- for (int32_t i=0; i < kMaxNumberOfMotors; i++)
- {
- m_invertedMotors[i] = 1;
- }
- m_deleteSpeedControllers = false;
-}
-
-RobotDrive::RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
- SpeedController &frontRightMotor, SpeedController &rearRightMotor)
-{
- InitRobotDrive();
- m_frontLeftMotor = &frontLeftMotor;
- m_rearLeftMotor = &rearLeftMotor;
- m_frontRightMotor = &frontRightMotor;
- m_rearRightMotor = &rearRightMotor;
- for (int32_t i=0; i < kMaxNumberOfMotors; i++)
- {
- m_invertedMotors[i] = 1;
- }
- m_deleteSpeedControllers = false;
-}
-
-/**
- * RobotDrive destructor.
- * Deletes motor objects that were not passed in and created internally only.
- **/
-RobotDrive::~RobotDrive()
-{
- if (m_deleteSpeedControllers)
- {
- delete m_frontLeftMotor;
- delete m_rearLeftMotor;
- delete m_frontRightMotor;
- delete m_rearRightMotor;
- }
- // FIXME: delete m_safetyHelper;
}
/**
diff --git a/wpilibc/simulation/src/SafePWM.cpp b/wpilibc/simulation/src/SafePWM.cpp
index 8ba0b68..57bd729 100644
--- a/wpilibc/simulation/src/SafePWM.cpp
+++ b/wpilibc/simulation/src/SafePWM.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "SafePWM.h"
diff --git a/wpilibc/simulation/src/SampleRobot.cpp b/wpilibc/simulation/src/SampleRobot.cpp
index b8f3181..21dcbfa 100644
--- a/wpilibc/simulation/src/SampleRobot.cpp
+++ b/wpilibc/simulation/src/SampleRobot.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "SampleRobot.h"
diff --git a/wpilibc/simulation/src/SensorBase.cpp b/wpilibc/simulation/src/SensorBase.cpp
index fa3086f..de18bcf 100644
--- a/wpilibc/simulation/src/SensorBase.cpp
+++ b/wpilibc/simulation/src/SensorBase.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "SensorBase.h"
diff --git a/wpilibc/simulation/src/Solenoid.cpp b/wpilibc/simulation/src/Solenoid.cpp
index 387194d..a9f4e9b 100644
--- a/wpilibc/simulation/src/Solenoid.cpp
+++ b/wpilibc/simulation/src/Solenoid.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Solenoid.h"
diff --git a/wpilibc/simulation/src/Talon.cpp b/wpilibc/simulation/src/Talon.cpp
index 0e9505d..9a6d806 100644
--- a/wpilibc/simulation/src/Talon.cpp
+++ b/wpilibc/simulation/src/Talon.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Talon.h"
diff --git a/wpilibc/simulation/src/Timer.cpp b/wpilibc/simulation/src/Timer.cpp
index b4734db..8bef9f2 100644
--- a/wpilibc/simulation/src/Timer.cpp
+++ b/wpilibc/simulation/src/Timer.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Timer.h"
diff --git a/wpilibc/simulation/src/Utility.cpp b/wpilibc/simulation/src/Utility.cpp
index b0710f1..4b49725 100644
--- a/wpilibc/simulation/src/Utility.cpp
+++ b/wpilibc/simulation/src/Utility.cpp
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
@@ -16,7 +22,7 @@
#include <cstdio>
#include <cstdlib>
#include <cstring>
-#if defined(UNIX)
+#if not defined(_WIN32)
#include <execinfo.h>
#include <cxxabi.h>
#endif
@@ -58,29 +64,27 @@
* This allows breakpoints to be set on an assert.
* The users don't call this, but instead use the wpi_assert macros in Utility.h.
*/
-bool wpi_assert_impl(bool conditionValue, const std::string &conditionText,
- const std::string &message, const std::string &fileName,
- uint32_t lineNumber, const std::string &funcName) {
+bool wpi_assert_impl(bool conditionValue, const char *conditionText,
+ const char *message, const char *fileName,
+ uint32_t lineNumber, const char *funcName) {
if (!conditionValue) {
- // Error string buffer
- std::stringstream error;
+ std::stringstream errorStream;
- // If an error message was specified, include it
- // Build error string
- if (message.size()) {
- error << "Assertion failed: \"" << message << "\", \"" << conditionText
- << "\" failed in " << funcName + "() in " << fileName << " at line "
- << lineNumber;
+ errorStream << "Assertion \"" << conditionText << "\" ";
+ errorStream << "on line " << lineNumber << " ";
+ errorStream << "of " << basename(fileName) << " ";
+
+ if (message[0] != '\0') {
+ errorStream << "failed: " << message << std::endl;
} else {
- error << "Assertion failed: \"" << conditionText << "\" in " << funcName
- << "() in " << fileName << " at line " << lineNumber;
+ errorStream << "failed." << std::endl;
}
// Print to console and send to remote dashboard
- std::cout << "\n\n>>>>" << error.str();
-
+ std::cout << "\n\n>>>>" << errorStream.str();
wpi_handleTracing();
}
+
return conditionValue;
}
@@ -161,13 +165,13 @@
*
* @return The current time in microseconds according to the FPGA (since FPGA reset).
*/
-uint32_t GetFPGATime()
+uint64_t GetFPGATime()
{
return wpilib::internal::simTime * 1e6;
}
//TODO: implement symbol demangling and backtrace on windows
-#if defined(UNIX)
+#if not defined(_WIN32)
/**
* Demangle a C++ symbol, used for printing stack traces.
diff --git a/wpilibc/simulation/src/Victor.cpp b/wpilibc/simulation/src/Victor.cpp
index 54ba61f..72e33bc 100644
--- a/wpilibc/simulation/src/Victor.cpp
+++ b/wpilibc/simulation/src/Victor.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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 "Victor.h"
diff --git a/wpilibc/simulation/src/simulation/SimEncoder.cpp b/wpilibc/simulation/src/simulation/SimEncoder.cpp
index b72b8f1..0b5ed20 100644
--- a/wpilibc/simulation/src/simulation/SimEncoder.cpp
+++ b/wpilibc/simulation/src/simulation/SimEncoder.cpp
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#include "simulation/SimEncoder.h"
#include "simulation/MainNode.h"
diff --git a/wpilibc/simulation/src/simulation/SimGyro.cpp b/wpilibc/simulation/src/simulation/SimGyro.cpp
index c173c05..c93434d 100644
--- a/wpilibc/simulation/src/simulation/SimGyro.cpp
+++ b/wpilibc/simulation/src/simulation/SimGyro.cpp
@@ -1,3 +1,9 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 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 the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
#include "simulation/SimGyro.h"
#include "simulation/MainNode.h"