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/Athena/include/ADXL345_I2C.h b/wpilibc/Athena/include/ADXL345_I2C.h
index e55e007..e0327f7 100644
--- a/wpilibc/Athena/include/ADXL345_I2C.h
+++ b/wpilibc/Athena/include/ADXL345_I2C.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 "interfaces/Accelerometer.h"
@@ -51,7 +52,7 @@
};
public:
- explicit ADXL345_I2C(Port port, Range range = kRange_2G);
+ explicit ADXL345_I2C(Port port, Range range = kRange_2G, int deviceAddress = kAddress);
virtual ~ADXL345_I2C() = default;
ADXL345_I2C(const ADXL345_I2C&) = delete;
diff --git a/wpilibc/Athena/include/ADXL345_SPI.h b/wpilibc/Athena/include/ADXL345_SPI.h
index 1310133..ceb28cd 100644
--- a/wpilibc/Athena/include/ADXL345_SPI.h
+++ b/wpilibc/Athena/include/ADXL345_SPI.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 "interfaces/Accelerometer.h"
diff --git a/wpilibc/Athena/include/ADXL362.h b/wpilibc/Athena/include/ADXL362.h
index ea47686..d4c2e0b 100644
--- a/wpilibc/Athena/include/ADXL362.h
+++ b/wpilibc/Athena/include/ADXL362.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 "interfaces/Accelerometer.h"
diff --git a/wpilibc/Athena/include/ADXRS450_Gyro.h b/wpilibc/Athena/include/ADXRS450_Gyro.h
index 42a7a59..7717f57 100644
--- a/wpilibc/Athena/include/ADXRS450_Gyro.h
+++ b/wpilibc/Athena/include/ADXRS450_Gyro.h
@@ -1,9 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2015. All Rights Reserved. */
+/* Copyright (c) FIRST 2015-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"
@@ -35,8 +36,6 @@
void Reset() override;
void Calibrate() override;
- std::string GetSmartDashboardType() const override;
-
private:
SPI m_spi;
diff --git a/wpilibc/Athena/include/AnalogAccelerometer.h b/wpilibc/Athena/include/AnalogAccelerometer.h
index ad6d6dd..86f7019 100644
--- a/wpilibc/Athena/include/AnalogAccelerometer.h
+++ b/wpilibc/Athena/include/AnalogAccelerometer.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 "AnalogInput.h"
diff --git a/wpilibc/Athena/include/AnalogGyro.h b/wpilibc/Athena/include/AnalogGyro.h
index 4ce0ccb..acb5b0f 100644
--- a/wpilibc/Athena/include/AnalogGyro.h
+++ b/wpilibc/Athena/include/AnalogGyro.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 "GyroBase.h"
@@ -53,8 +54,6 @@
virtual void InitGyro();
void Calibrate() override;
- std::string GetSmartDashboardType() const override;
-
protected:
std::shared_ptr<AnalogInput> m_analog;
diff --git a/wpilibc/Athena/include/AnalogInput.h b/wpilibc/Athena/include/AnalogInput.h
index d47d1f2..6217e2d 100644
--- a/wpilibc/Athena/include/AnalogInput.h
+++ b/wpilibc/Athena/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 "HAL/HAL.hpp"
diff --git a/wpilibc/Athena/include/AnalogOutput.h b/wpilibc/Athena/include/AnalogOutput.h
index c30b5c7..d7bd30e 100644
--- a/wpilibc/Athena/include/AnalogOutput.h
+++ b/wpilibc/Athena/include/AnalogOutput.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/Athena/include/AnalogPotentiometer.h b/wpilibc/Athena/include/AnalogPotentiometer.h
index 7b7b12a..3c6daa5 100644
--- a/wpilibc/Athena/include/AnalogPotentiometer.h
+++ b/wpilibc/Athena/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. */
+/*----------------------------------------------------------------------------*/
+
#include "AnalogInput.h"
#include "interfaces/Potentiometer.h"
#include "LiveWindow/LiveWindowSendable.h"
diff --git a/wpilibc/Athena/include/AnalogTrigger.h b/wpilibc/Athena/include/AnalogTrigger.h
index 7fdd202..811cc07 100644
--- a/wpilibc/Athena/include/AnalogTrigger.h
+++ b/wpilibc/Athena/include/AnalogTrigger.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 "HAL/HAL.hpp"
diff --git a/wpilibc/Athena/include/AnalogTriggerOutput.h b/wpilibc/Athena/include/AnalogTriggerOutput.h
index 27ad3b8..976d30c 100644
--- a/wpilibc/Athena/include/AnalogTriggerOutput.h
+++ b/wpilibc/Athena/include/AnalogTriggerOutput.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 "DigitalSource.h"
diff --git a/wpilibc/Athena/include/BuiltInAccelerometer.h b/wpilibc/Athena/include/BuiltInAccelerometer.h
index b6b5f8e..d201b61 100644
--- a/wpilibc/Athena/include/BuiltInAccelerometer.h
+++ b/wpilibc/Athena/include/BuiltInAccelerometer.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "interfaces/Accelerometer.h"
diff --git a/wpilibc/Athena/include/CANJaguar.h b/wpilibc/Athena/include/CANJaguar.h
index 5d05d47..c456e2a 100644
--- a/wpilibc/Athena/include/CANJaguar.h
+++ b/wpilibc/Athena/include/CANJaguar.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2009. All Rights Reserved. */
+/* Copyright (c) FIRST 2009-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"
@@ -228,6 +230,7 @@
mutable std::atomic<bool> m_receivedStatusMessage2{false};
bool m_controlEnabled = false;
+ bool m_stopped = false;
void verify();
diff --git a/wpilibc/Athena/include/CANSpeedController.h b/wpilibc/Athena/include/CANSpeedController.h
index 73ed379..5d76026 100644
--- a/wpilibc/Athena/include/CANSpeedController.h
+++ b/wpilibc/Athena/include/CANSpeedController.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "SpeedController.h"
@@ -20,23 +22,11 @@
kSpeed = 2,
kPosition = 3,
kVoltage = 4,
- kFollower = 5 // Not supported in Jaguar.
+ kFollower = 5, // Not supported in Jaguar.
+ kMotionProfile = 6, // Not supported in Jaguar.
};
// Helper function for the ControlMode enum
- std::string GetModeName(ControlMode mode) {
- switch(mode) {
- case kPercentVbus: return "PercentVbus";
- case kCurrent: return "Current";
- case kSpeed: return "Speed";
- case kPosition: return "Position";
- case kVoltage: return "Voltage";
- case kFollower: return "Follower";
- default: return "[unknown control mode]";
- }
- }
-
- // Helper function for the ControlMode enum
virtual bool IsModePID(ControlMode mode) const = 0;
enum Faults {
@@ -75,6 +65,7 @@
virtual float Get() const = 0;
virtual void Set(float value, uint8_t syncGroup = 0) = 0;
+ virtual void StopMotor() = 0;
virtual void Disable() = 0;
virtual void SetP(double p) = 0;
virtual void SetI(double i) = 0;
diff --git a/wpilibc/Athena/include/CANTalon.h b/wpilibc/Athena/include/CANTalon.h
index 15ebbe2..8ea9ae1 100644
--- a/wpilibc/Athena/include/CANTalon.h
+++ b/wpilibc/Athena/include/CANTalon.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
+
#pragma once
#include "SafePWM.h"
@@ -42,9 +44,9 @@
* Depending on the sensor type, Talon can determine if sensor is plugged in ot not.
*/
enum FeedbackDeviceStatus {
- FeedbackStatusUnknown = 0, //!< Sensor status could not be determined. Not all sensors can do this.
- FeedbackStatusPresent = 1, //!< Sensor is present and working okay.
- FeedbackStatusNotPresent = 2, //!< Sensor is not present, not plugged in, not powered, etc...
+ FeedbackStatusUnknown = 0, //!< Sensor status could not be determined. Not all sensors can do this.
+ FeedbackStatusPresent = 1, //!< Sensor is present and working okay.
+ FeedbackStatusNotPresent = 2, //!< Sensor is not present, not plugged in, not powered, etc...
};
enum StatusFrameRate {
StatusFrameRateGeneral = 0,
@@ -53,6 +55,151 @@
StatusFrameRateAnalogTempVbat = 3,
StatusFrameRatePulseWidthMeas = 4,
};
+ /**
+ * Enumerated types for Motion Control Set Values.
+ * When in Motion Profile control mode, these constants are paseed
+ * into set() to manipulate the motion profile executer.
+ * When changing modes, be sure to read the value back using getMotionProfileStatus()
+ * to ensure changes in output take effect before performing buffering actions.
+ * Disable will signal Talon to put motor output into neutral drive.
+ * Talon will stop processing motion profile points. This means the buffer is
+ * effectively disconnected from the executer, allowing the robot to gracefully
+ * clear and push new traj points. isUnderrun will get cleared.
+ * The active trajectory is also cleared.
+ * Enable will signal Talon to pop a trajectory point from it's buffer and process it.
+ * If the active trajectory is empty, Talon will shift in the next point.
+ * If the active traj is empty, and so is the buffer, the motor drive is neutral and
+ * isUnderrun is set. When active traj times out, and buffer has at least one point,
+ * Talon shifts in next one, and isUnderrun is cleared. When active traj times out,
+ * and buffer is empty, Talon keeps processing active traj and sets IsUnderrun.
+ * Hold will signal Talon keep processing the active trajectory indefinitely.
+ * If the active traj is cleared, Talon will neutral motor drive. Otherwise
+ * Talon will keep processing the active traj but it will not shift in
+ * points from the buffer. This means the buffer is effectively disconnected
+ * from the executer, allowing the robot to gracefully clear and push
+ * new traj points.
+ * isUnderrun is set if active traj is empty, otherwise it is cleared.
+ * isLast signal is also cleared.
+ *
+ * Typical workflow:
+ * set(Disable),
+ * Confirm Disable takes effect,
+ * clear buffer and push buffer points,
+ * set(Enable) when enough points have been pushed to ensure no underruns,
+ * wait for MP to finish or decide abort,
+ * If MP finished gracefully set(Hold) to hold position servo and disconnect buffer,
+ * If MP is being aborted set(Disable) to neutral the motor and disconnect buffer,
+ * Confirm mode takes effect,
+ * clear buffer and push buffer points, and rinse-repeat.
+ */
+ enum SetValueMotionProfile {
+ SetValueMotionProfileDisable = 0,
+ SetValueMotionProfileEnable = 1,
+ SetValueMotionProfileHold = 2,
+ };
+ /**
+ * Motion Profile Trajectory Point
+ * This is simply a data transer object.
+ */
+ struct TrajectoryPoint {
+ double position; //!< The position to servo to.
+ double velocity; //!< The velocity to feed-forward.
+ /**
+ * Time in milliseconds to process this point.
+ * Value should be between 1ms and 255ms. If value is zero
+ * then Talon will default to 1ms. If value exceeds 255ms API will cap it.
+ */
+ unsigned int timeDurMs;
+ /**
+ * Which slot to get PIDF gains.
+ * PID is used for position servo.
+ * F is used as the Kv constant for velocity feed-forward.
+ * Typically this is hardcoded to the a particular slot, but you are free
+ * gain schedule if need be.
+ */
+ unsigned int profileSlotSelect;
+ /**
+ * Set to true to only perform the velocity feed-forward and not perform
+ * position servo. This is useful when learning how the position servo
+ * changes the motor response. The same could be accomplish by clearing the
+ * PID gains, however this is synchronous the streaming, and doesn't require restoing
+ * gains when finished.
+ *
+ * Additionaly setting this basically gives you direct control of the motor output
+ * since motor output = targetVelocity X Kv, where Kv is our Fgain.
+ * This means you can also scheduling straight-throttle curves without relying on
+ * a sensor.
+ */
+ bool velocityOnly;
+ /**
+ * Set to true to signal Talon that this is the final point, so do not
+ * attempt to pop another trajectory point from out of the Talon buffer.
+ * Instead continue processing this way point. Typically the velocity
+ * member variable should be zero so that the motor doesn't spin indefinitely.
+ */
+ bool isLastPoint;
+ /**
+ * Set to true to signal Talon to zero the selected sensor.
+ * When generating MPs, one simple method is to make the first target position zero,
+ * and the final target position the target distance from the current position.
+ * Then when you fire the MP, the current position gets set to zero.
+ * If this is the intent, you can set zeroPos on the first trajectory point.
+ *
+ * Otherwise you can leave this false for all points, and offset the positions
+ * of all trajectory points so they are correct.
+ */
+ bool zeroPos;
+ };
+ /**
+ * Motion Profile Status
+ * This is simply a data transer object.
+ */
+ struct MotionProfileStatus {
+ /**
+ * The available empty slots in the trajectory buffer.
+ *
+ * The robot API holds a "top buffer" of trajectory points, so your applicaion
+ * can dump several points at once. The API will then stream them into the Talon's
+ * low-level buffer, allowing the Talon to act on them.
+ */
+ unsigned int topBufferRem;
+ /**
+ * The number of points in the top trajectory buffer.
+ */
+ unsigned int topBufferCnt;
+ /**
+ * The number of points in the low level Talon buffer.
+ */
+ unsigned int btmBufferCnt;
+ /**
+ * Set if isUnderrun ever gets set.
+ * Only is cleared by clearMotionProfileHasUnderrun() to ensure
+ * robot logic can react or instrument it.
+ * @see clearMotionProfileHasUnderrun()
+ */
+ bool hasUnderrun;
+ /**
+ * This is set if Talon needs to shift a point from its buffer into
+ * the active trajectory point however the buffer is empty. This gets cleared
+ * automatically when is resolved.
+ */
+ bool isUnderrun;
+ /**
+ * True if the active trajectory point has not empty, false otherwise.
+ * The members in activePoint are only valid if this signal is set.
+ */
+ bool activePointValid;
+ /**
+ * The number of points in the low level Talon buffer.
+ */
+ TrajectoryPoint activePoint;
+ /**
+ * The current output mode of the motion profile executer (disabled, enabled, or hold).
+ * When changing the set() value in MP mode, it's important to check this signal to
+ * confirm the change takes effect before interacting with the top buffer.
+ */
+ SetValueMotionProfile outputEnable;
+ };
explicit CANTalon(int deviceNumber);
explicit CANTalon(int deviceNumber, int controlPeriodMs);
DEFAULT_MOVE_CONSTRUCTOR(CANTalon);
@@ -139,6 +286,9 @@
virtual void ConfigLimitMode(LimitMode mode) override;
virtual void ConfigForwardLimit(double forwardLimitPosition) override;
virtual void ConfigReverseLimit(double reverseLimitPosition) override;
+ void ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, bool bReverseLimitSwitchEn);
+ void ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn);
+ void ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn);
/**
* Change the fwd limit switch setting to normally open or closed.
* Talon will disable momentarilly if the Talon's current setting
@@ -167,9 +317,9 @@
/**
* Enables Talon SRX to automatically zero the Sensor Position whenever an
* edge is detected on the index signal.
- * @param enable boolean input, pass true to enable feature or false to disable.
- * @param risingEdge boolean input, pass true to clear the position on rising edge,
- * pass false to clear the position on falling edge.
+ * @param enable boolean input, pass true to enable feature or false to disable.
+ * @param risingEdge boolean input, pass true to clear the position on rising edge,
+ * pass false to clear the position on falling edge.
*/
void EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge);
void ConfigSetParameter(uint32_t paramEnum, double value);
@@ -193,6 +343,69 @@
bool IsEnabled() const override;
double GetSetpoint() const override;
+
+ /**
+ * Calling application can opt to speed up the handshaking between the robot API and the Talon to increase the
+ * download rate of the Talon's Motion Profile. Ideally the period should be no more than half the period
+ * of a trajectory point.
+ */
+ void ChangeMotionControlFramePeriod(int periodMs);
+
+ /**
+ * Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top).
+ * Be sure to check GetMotionProfileStatus() to know when the buffer is actually cleared.
+ */
+ void ClearMotionProfileTrajectories();
+
+ /**
+ * Retrieve just the buffer count for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and ideal
+ * if caller needs to quickly poll the progress of trajectory points being emptied
+ * into Talon's RAM. Otherwise just use GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+ int GetMotionProfileTopLevelBufferCount();
+
+ /**
+ * Push another trajectory point into the top level buffer (which is emptied into
+ * the Talon's bottom buffer as room allows).
+ * @param trajPt the trajectory point to insert into buffer.
+ * @return true if trajectory point push ok. CTR_BufferFull if buffer is full
+ * due to kMotionProfileTopBufferCapacity.
+ */
+ bool PushMotionProfileTrajectory(const TrajectoryPoint & trajPt);
+
+ /**
+ * @return true if api-level (top) buffer is full.
+ */
+ bool IsMotionProfileTopLevelBufferFull();
+
+ /**
+ * This must be called periodically to funnel the trajectory points from the API's top level buffer to
+ * the Talon's bottom level buffer. Recommendation is to call this twice as fast as the executation rate of the motion profile.
+ * So if MP is running with 20ms trajectory points, try calling this routine every 10ms. All motion profile functions are thread-safe
+ * through the use of a mutex, so there is no harm in having the caller utilize threading.
+ */
+ void ProcessMotionProfileBuffer();
+
+ /**
+ * Retrieve all status information.
+ * Since this all comes from one CAN frame, its ideal to have one routine to retrieve the frame once and decode everything.
+ * @param [out] motionProfileStatus contains all progress information on the currently running MP.
+ */
+ void GetMotionProfileStatus(MotionProfileStatus & motionProfileStatus);
+
+ /**
+ * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another point,
+ * but the low level buffer is empty.
+ *
+ * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set until
+ * Robot Application clears it with this routine, which ensures Robot Application
+ * gets a chance to instrument or react. Caller could also check the isUnderrun flag
+ * which automatically clears when fault condition is removed.
+ */
+ void ClearMotionProfileHasUnderrun();
+
// LiveWindow stuff.
void ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) override;
@@ -216,6 +429,7 @@
kPositionMode = 1,
kSpeedMode = 2,
kCurrentMode = 3,
+ kMotionProfileMode = 6,
kDisabled = 15
};
@@ -226,6 +440,7 @@
// actually test this.
bool m_controlEnabled = true;
+ bool m_stopped = false;
ControlMode m_controlMode = kPercentVbus;
TalonControlMode m_sendMode;
@@ -255,10 +470,10 @@
static const unsigned int kDelayForSolicitedSignalsUs = 4000;
/**
* @param devToLookup FeedbackDevice to lookup the scalar for. Because Talon
- * allows multiple sensors to be attached simultaneously, caller must
- * specify which sensor to lookup.
- * @return The number of native Talon units per rotation of the selected sensor.
- * Zero if the necessary sensor information is not available.
+ * allows multiple sensors to be attached simultaneously, caller must
+ * specify which sensor to lookup.
+ * @return The number of native Talon units per rotation of the selected sensor.
+ * Zero if the necessary sensor information is not available.
* @see ConfigEncoderCodesPerRev
* @see ConfigPotentiometerTurns
*/
@@ -271,40 +486,40 @@
*/
void ApplyControlMode(CANSpeedController::ControlMode mode);
/**
- * @param fullRotations double precision value representing number of rotations of selected feedback sensor.
- * If user has never called the config routine for the selected sensor, then the caller
- * is likely passing rotations in engineering units already, in which case it is returned
- * as is.
- * @see ConfigPotentiometerTurns
- * @see ConfigEncoderCodesPerRev
+ * @param fullRotations double precision value representing number of rotations of selected feedback sensor.
+ * If user has never called the config routine for the selected sensor, then the caller
+ * is likely passing rotations in engineering units already, in which case it is returned
+ * as is.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
* @return fullRotations in native engineering units of the Talon SRX firmware.
*/
int32_t ScaleRotationsToNativeUnits(FeedbackDevice devToLookup, double fullRotations) const;
/**
- * @param rpm double precision value representing number of rotations per minute of selected feedback sensor.
- * If user has never called the config routine for the selected sensor, then the caller
- * is likely passing rotations in engineering units already, in which case it is returned
- * as is.
- * @see ConfigPotentiometerTurns
- * @see ConfigEncoderCodesPerRev
+ * @param rpm double precision value representing number of rotations per minute of selected feedback sensor.
+ * If user has never called the config routine for the selected sensor, then the caller
+ * is likely passing rotations in engineering units already, in which case it is returned
+ * as is.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
* @return sensor velocity in native engineering units of the Talon SRX firmware.
*/
int32_t ScaleVelocityToNativeUnits(FeedbackDevice devToLookup, double rpm) const;
/**
- * @param nativePos integral position of the feedback sensor in native Talon SRX units.
- * If user has never called the config routine for the selected sensor, then the return
- * will be in TALON SRX units as well to match the behavior in the 2015 season.
- * @see ConfigPotentiometerTurns
- * @see ConfigEncoderCodesPerRev
+ * @param nativePos integral position of the feedback sensor in native Talon SRX units.
+ * If user has never called the config routine for the selected sensor, then the return
+ * will be in TALON SRX units as well to match the behavior in the 2015 season.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
* @return double precision number of rotations, unless config was never performed.
*/
double ScaleNativeUnitsToRotations(FeedbackDevice devToLookup, int32_t nativePos) const;
/**
- * @param nativeVel integral velocity of the feedback sensor in native Talon SRX units.
- * If user has never called the config routine for the selected sensor, then the return
- * will be in TALON SRX units as well to match the behavior in the 2015 season.
- * @see ConfigPotentiometerTurns
- * @see ConfigEncoderCodesPerRev
+ * @param nativeVel integral velocity of the feedback sensor in native Talon SRX units.
+ * If user has never called the config routine for the selected sensor, then the return
+ * will be in TALON SRX units as well to match the behavior in the 2015 season.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
* @return double precision of sensor velocity in RPM, unless config was never performed.
*/
double ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, int32_t nativeVel) const;
diff --git a/wpilibc/Athena/include/CameraServer.h b/wpilibc/Athena/include/CameraServer.h
index 17ece99..bb0c314 100644
--- a/wpilibc/Athena/include/CameraServer.h
+++ b/wpilibc/Athena/include/CameraServer.h
@@ -1,9 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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
#include "USBCamera.h"
diff --git a/wpilibc/Athena/include/Compressor.h b/wpilibc/Athena/include/Compressor.h
index d5962c3..0d56ef7 100644
--- a/wpilibc/Athena/include/Compressor.h
+++ b/wpilibc/Athena/include/Compressor.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#ifndef Compressor_H_
diff --git a/wpilibc/Athena/include/ControllerPower.h b/wpilibc/Athena/include/ControllerPower.h
index 7384e60..18367b5 100644
--- a/wpilibc/Athena/include/ControllerPower.h
+++ b/wpilibc/Athena/include/ControllerPower.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __CONTROLLER_POWER_H__
diff --git a/wpilibc/Athena/include/Counter.h b/wpilibc/Athena/include/Counter.h
index 4066de8..3cb55b5 100644
--- a/wpilibc/Athena/include/Counter.h
+++ b/wpilibc/Athena/include/Counter.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 "HAL/HAL.hpp"
diff --git a/wpilibc/Athena/include/CounterBase.h b/wpilibc/Athena/include/CounterBase.h
index 68896a8..633b795 100644
--- a/wpilibc/Athena/include/CounterBase.h
+++ b/wpilibc/Athena/include/CounterBase.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 <stdint.h>
diff --git a/wpilibc/Athena/include/DigitalGlitchFilter.h b/wpilibc/Athena/include/DigitalGlitchFilter.h
index a6f9f90..0f9e676 100644
--- a/wpilibc/Athena/include/DigitalGlitchFilter.h
+++ b/wpilibc/Athena/include/DigitalGlitchFilter.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2015. All Rights Reserved. */
+/* Copyright (c) FIRST 2015-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 <array>
diff --git a/wpilibc/Athena/include/DigitalInput.h b/wpilibc/Athena/include/DigitalInput.h
index 79cfcc5..96a70a0 100644
--- a/wpilibc/Athena/include/DigitalInput.h
+++ b/wpilibc/Athena/include/DigitalInput.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 "DigitalSource.h"
diff --git a/wpilibc/Athena/include/DigitalOutput.h b/wpilibc/Athena/include/DigitalOutput.h
index 367fb80..e8bb514 100644
--- a/wpilibc/Athena/include/DigitalOutput.h
+++ b/wpilibc/Athena/include/DigitalOutput.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 "DigitalSource.h"
diff --git a/wpilibc/Athena/include/DigitalSource.h b/wpilibc/Athena/include/DigitalSource.h
index c716715..d85102b 100644
--- a/wpilibc/Athena/include/DigitalSource.h
+++ b/wpilibc/Athena/include/DigitalSource.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 "InterruptableSensorBase.h"
diff --git a/wpilibc/Athena/include/DoubleSolenoid.h b/wpilibc/Athena/include/DoubleSolenoid.h
index fd20397..9d1f033 100644
--- a/wpilibc/Athena/include/DoubleSolenoid.h
+++ b/wpilibc/Athena/include/DoubleSolenoid.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 "SolenoidBase.h"
diff --git a/wpilibc/Athena/include/DriverStation.h b/wpilibc/Athena/include/DriverStation.h
index 23ee095..b98c6ec 100644
--- a/wpilibc/Athena/include/DriverStation.h
+++ b/wpilibc/Athena/include/DriverStation.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"
@@ -29,6 +31,10 @@
virtual ~DriverStation();
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;
@@ -95,6 +101,7 @@
private:
static DriverStation *m_instance;
void ReportJoystickUnpluggedError(std::string message);
+ void ReportJoystickUnpluggedWarning(std::string message);
void Run();
HALJoystickAxes m_joystickAxes[kJoystickPorts];
diff --git a/wpilibc/Athena/include/Encoder.h b/wpilibc/Athena/include/Encoder.h
index 76bfaf9..5c692f0 100644
--- a/wpilibc/Athena/include/Encoder.h
+++ b/wpilibc/Athena/include/Encoder.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 "HAL/HAL.hpp"
diff --git a/wpilibc/Athena/include/GearTooth.h b/wpilibc/Athena/include/GearTooth.h
index 3889f9c..070f397 100644
--- a/wpilibc/Athena/include/GearTooth.h
+++ b/wpilibc/Athena/include/GearTooth.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 "Counter.h"
diff --git a/wpilibc/Athena/include/I2C.h b/wpilibc/Athena/include/I2C.h
index 89041e7..3267032 100644
--- a/wpilibc/Athena/include/I2C.h
+++ b/wpilibc/Athena/include/I2C.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 "SensorBase.h"
diff --git a/wpilibc/Athena/include/Internal/HardwareHLReporting.h b/wpilibc/Athena/include/Internal/HardwareHLReporting.h
index a91a20d..b411ecf 100644
--- a/wpilibc/Athena/include/Internal/HardwareHLReporting.h
+++ b/wpilibc/Athena/include/Internal/HardwareHLReporting.h
@@ -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 "HLUsageReporting.h"
diff --git a/wpilibc/Athena/include/InterruptableSensorBase.h b/wpilibc/Athena/include/InterruptableSensorBase.h
index d77ae2f..708089b 100644
--- a/wpilibc/Athena/include/InterruptableSensorBase.h
+++ b/wpilibc/Athena/include/InterruptableSensorBase.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 "HAL/HAL.hpp"
diff --git a/wpilibc/Athena/include/IterativeRobot.h b/wpilibc/Athena/include/IterativeRobot.h
index ccea4f8..acba235 100644
--- a/wpilibc/Athena/include/IterativeRobot.h
+++ b/wpilibc/Athena/include/IterativeRobot.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 "Timer.h"
diff --git a/wpilibc/Athena/include/Jaguar.h b/wpilibc/Athena/include/Jaguar.h
index feb00c7..c129559 100644
--- a/wpilibc/Athena/include/Jaguar.h
+++ b/wpilibc/Athena/include/Jaguar.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 "SafePWM.h"
@@ -20,6 +21,7 @@
virtual void Set(float value, uint8_t syncGroup = 0) override;
virtual float Get() const override;
virtual void Disable() override;
+ virtual void StopMotor() override;
virtual void PIDWrite(float output) override;
virtual void SetInverted(bool isInverted) override;
diff --git a/wpilibc/Athena/include/Joystick.h b/wpilibc/Athena/include/Joystick.h
index b8ea292..fdd4d7d 100644
--- a/wpilibc/Athena/include/Joystick.h
+++ b/wpilibc/Athena/include/Joystick.h
@@ -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. */
/*----------------------------------------------------------------------------*/
#ifndef JOYSTICK_H_
diff --git a/wpilibc/Athena/include/MotorSafety.h b/wpilibc/Athena/include/MotorSafety.h
index 6963ae7..373fddf 100644
--- a/wpilibc/Athena/include/MotorSafety.h
+++ b/wpilibc/Athena/include/MotorSafety.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
#define DEFAULT_SAFETY_EXPIRATION 0.1
diff --git a/wpilibc/Athena/include/MotorSafetyHelper.h b/wpilibc/Athena/include/MotorSafetyHelper.h
index bbe7658..f124605 100644
--- a/wpilibc/Athena/include/MotorSafetyHelper.h
+++ b/wpilibc/Athena/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/Athena/include/Notifier.h b/wpilibc/Athena/include/Notifier.h
new file mode 100644
index 0000000..070853f
--- /dev/null
+++ b/wpilibc/Athena/include/Notifier.h
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <functional>
+
+#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:
+ // update the HAL alarm
+ void UpdateAlarm();
+ // HAL callback
+ static void Notify(uint64_t currentTimeInt, void *param);
+
+ // held while updating process information
+ priority_mutex m_processMutex;
+ // HAL handle
+ void *m_notifier;
+ // address of the handler
+ TimerEventHandler m_handler;
+ // the absolute expiration time
+ double m_expirationTime = 0;
+ // the relative time (either periodic or single)
+ double m_period = 0;
+ // true if this is a periodic event
+ bool m_periodic = false;
+
+ // held by interrupt manager task while handler call is in progress
+ priority_mutex m_handlerMutex;
+};
diff --git a/wpilibc/Athena/include/PWM.h b/wpilibc/Athena/include/PWM.h
index b7120c0..8f9976a 100644
--- a/wpilibc/Athena/include/PWM.h
+++ b/wpilibc/Athena/include/PWM.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 "SensorBase.h"
diff --git a/wpilibc/Athena/include/PowerDistributionPanel.h b/wpilibc/Athena/include/PowerDistributionPanel.h
index 9a1bd7c..b9769ec 100644
--- a/wpilibc/Athena/include/PowerDistributionPanel.h
+++ b/wpilibc/Athena/include/PowerDistributionPanel.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved.
- */
+/* 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 $(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/Athena/include/Preferences.h b/wpilibc/Athena/include/Preferences.h
index a2c3bc4..1e1beff 100644
--- a/wpilibc/Athena/include/Preferences.h
+++ b/wpilibc/Athena/include/Preferences.h
@@ -1,9 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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/Athena/include/Relay.h b/wpilibc/Athena/include/Relay.h
index f582221..a9bd26a 100644
--- a/wpilibc/Athena/include/Relay.h
+++ b/wpilibc/Athena/include/Relay.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 "MotorSafety.h"
diff --git a/wpilibc/Athena/include/RobotBase.h b/wpilibc/Athena/include/RobotBase.h
index 2fcd146..b6626b9 100644
--- a/wpilibc/Athena/include/RobotBase.h
+++ b/wpilibc/Athena/include/RobotBase.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 "Base.h"
diff --git a/wpilibc/Athena/include/RobotDrive.h b/wpilibc/Athena/include/RobotDrive.h
index d91e80d..547d06a 100644
--- a/wpilibc/Athena/include/RobotDrive.h
+++ b/wpilibc/Athena/include/RobotDrive.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/Athena/include/SD540.h b/wpilibc/Athena/include/SD540.h
new file mode 100644
index 0000000..9e98678
--- /dev/null
+++ b/wpilibc/Athena/include/SD540.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "SafePWM.h"
+#include "SpeedController.h"
+#include "PIDOutput.h"
+
+/**
+ * Mindsensors SD540 Speed Controller
+ */
+class SD540 : public SafePWM, public SpeedController {
+ public:
+ explicit SD540(uint32_t channel);
+ virtual ~SD540() = default;
+ virtual void Set(float value, uint8_t syncGroup = 0) override;
+ virtual float Get() const override;
+ virtual void Disable() override;
+ virtual void StopMotor() override;
+
+ virtual void PIDWrite(float output) override;
+
+ virtual void SetInverted(bool isInverted) override;
+ virtual bool GetInverted() const override;
+
+ private:
+ bool m_isInverted = false;
+};
diff --git a/wpilibc/Athena/include/SPI.h b/wpilibc/Athena/include/SPI.h
index 7b33dd7..b6d68f3 100644
--- a/wpilibc/Athena/include/SPI.h
+++ b/wpilibc/Athena/include/SPI.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 "HAL/HAL.hpp"
diff --git a/wpilibc/Athena/include/SafePWM.h b/wpilibc/Athena/include/SafePWM.h
index f364626..3c908c7 100644
--- a/wpilibc/Athena/include/SafePWM.h
+++ b/wpilibc/Athena/include/SafePWM.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 "MotorSafety.h"
diff --git a/wpilibc/Athena/include/SampleRobot.h b/wpilibc/Athena/include/SampleRobot.h
index 64ca32c..61f8bc0 100644
--- a/wpilibc/Athena/include/SampleRobot.h
+++ b/wpilibc/Athena/include/SampleRobot.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 "RobotBase.h"
diff --git a/wpilibc/Athena/include/SerialPort.h b/wpilibc/Athena/include/SerialPort.h
index 3cd1c1c..e94af87 100644
--- a/wpilibc/Athena/include/SerialPort.h
+++ b/wpilibc/Athena/include/SerialPort.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/Athena/include/Servo.h b/wpilibc/Athena/include/Servo.h
index 040d9d1..f33a82e 100644
--- a/wpilibc/Athena/include/Servo.h
+++ b/wpilibc/Athena/include/Servo.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 "SafePWM.h"
diff --git a/wpilibc/Athena/include/Solenoid.h b/wpilibc/Athena/include/Solenoid.h
index dc3d53a..d032930 100644
--- a/wpilibc/Athena/include/Solenoid.h
+++ b/wpilibc/Athena/include/Solenoid.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 "SolenoidBase.h"
diff --git a/wpilibc/Athena/include/SolenoidBase.h b/wpilibc/Athena/include/SolenoidBase.h
index 159beeb..0a71de2 100644
--- a/wpilibc/Athena/include/SolenoidBase.h
+++ b/wpilibc/Athena/include/SolenoidBase.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 "Resource.h"
diff --git a/wpilibc/Athena/include/Spark.h b/wpilibc/Athena/include/Spark.h
new file mode 100644
index 0000000..d4859ce
--- /dev/null
+++ b/wpilibc/Athena/include/Spark.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "SafePWM.h"
+#include "SpeedController.h"
+#include "PIDOutput.h"
+
+/**
+ * REV Robotics Speed Controller
+ */
+class Spark : public SafePWM, public SpeedController {
+ public:
+ explicit Spark(uint32_t channel);
+ virtual ~Spark() = default;
+ virtual void Set(float value, uint8_t syncGroup = 0) override;
+ virtual float Get() const override;
+ virtual void Disable() override;
+ virtual void StopMotor() override;
+
+ virtual void PIDWrite(float output) override;
+
+ virtual void SetInverted(bool isInverted) override;
+ virtual bool GetInverted() const override;
+
+ private:
+ bool m_isInverted = false;
+};
diff --git a/wpilibc/Athena/include/SpeedController.h b/wpilibc/Athena/include/SpeedController.h
index a87ed2b..47ba843 100644
--- a/wpilibc/Athena/include/SpeedController.h
+++ b/wpilibc/Athena/include/SpeedController.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 "HAL/HAL.hpp"
@@ -47,4 +48,9 @@
* @return isInverted The state of inversion, true is inverted.
*/
virtual bool GetInverted() const = 0;
+
+ /**
+ * Common interface to stop the motor until Set is called again.
+ */
+ virtual void StopMotor() = 0;
};
diff --git a/wpilibc/Athena/include/Talon.h b/wpilibc/Athena/include/Talon.h
index 38764b8..0a88012 100644
--- a/wpilibc/Athena/include/Talon.h
+++ b/wpilibc/Athena/include/Talon.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 "SafePWM.h"
@@ -24,6 +25,7 @@
virtual void PIDWrite(float output) override;
virtual void SetInverted(bool isInverted) override;
virtual bool GetInverted() const override;
+ virtual void StopMotor() override;
private:
bool m_isInverted = false;
diff --git a/wpilibc/Athena/include/TalonSRX.h b/wpilibc/Athena/include/TalonSRX.h
index 8819723..61b1fd8 100644
--- a/wpilibc/Athena/include/TalonSRX.h
+++ b/wpilibc/Athena/include/TalonSRX.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 "SafePWM.h"
@@ -21,6 +22,7 @@
virtual void Set(float value, uint8_t syncGroup = 0) override;
virtual float Get() const override;
virtual void Disable() override;
+ virtual void StopMotor() override;
virtual void PIDWrite(float output) override;
virtual void SetInverted(bool isInverted) override;
diff --git a/wpilibc/Athena/include/USBCamera.h b/wpilibc/Athena/include/USBCamera.h
index d02c5d3..522e889 100644
--- a/wpilibc/Athena/include/USBCamera.h
+++ b/wpilibc/Athena/include/USBCamera.h
@@ -1,9 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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
#include "ErrorBase.h"
diff --git a/wpilibc/Athena/include/Ultrasonic.h b/wpilibc/Athena/include/Ultrasonic.h
index 9f36171..d586173 100644
--- a/wpilibc/Athena/include/Ultrasonic.h
+++ b/wpilibc/Athena/include/Ultrasonic.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 "SensorBase.h"
@@ -12,10 +13,8 @@
#include "PIDSource.h"
#include "LiveWindow/LiveWindowSendable.h"
#include <atomic>
-
-#include "HAL/cpp/priority_mutex.h"
-
#include <memory>
+#include <set>
class DigitalInput;
class DigitalOutput;
@@ -80,24 +79,22 @@
static void UltrasonicChecker();
- static constexpr double kPingTime =
- 10 * 1e-6; ///< Time (sec) for the ping trigger pulse.
- static const uint32_t kPriority =
- 64; ///< Priority that the ultrasonic round robin task runs.
- static constexpr double kMaxUltrasonicTime =
- 0.1; ///< Max time (ms) between readings.
+ // Time (sec) for the ping trigger pulse.
+ static constexpr double kPingTime = 10 * 1e-6;
+ // Priority that the ultrasonic round robin task runs.
+ static const uint32_t kPriority = 64;
+ // Max time (ms) between readings.
+ static constexpr double kMaxUltrasonicTime = 0.1;
static constexpr double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
static Task m_task; // task doing the round-robin automatic sensing
- static Ultrasonic *m_firstSensor; // head of the ultrasonic sensor list
+ static std::set<Ultrasonic*> m_sensors; // ultrasonic sensors
static std::atomic<bool> m_automaticEnabled; // automatic round robin mode
- static priority_mutex m_mutex; // synchronize access to the list of sensors
std::shared_ptr<DigitalOutput> m_pingChannel;
std::shared_ptr<DigitalInput> m_echoChannel;
bool m_enabled = false;
Counter m_counter;
- Ultrasonic *m_nextSensor;
DistanceUnit m_units;
std::shared_ptr<ITable> m_table;
diff --git a/wpilibc/Athena/include/Victor.h b/wpilibc/Athena/include/Victor.h
index 755955f..2aa2ecf 100644
--- a/wpilibc/Athena/include/Victor.h
+++ b/wpilibc/Athena/include/Victor.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 "SafePWM.h"
@@ -23,6 +24,7 @@
virtual void Set(float value, uint8_t syncGroup = 0) override;
virtual float Get() const override;
virtual void Disable() override;
+ virtual void StopMotor() override;
virtual void PIDWrite(float output) override;
diff --git a/wpilibc/Athena/include/VictorSP.h b/wpilibc/Athena/include/VictorSP.h
index 978cd0a..a53ccfd 100644
--- a/wpilibc/Athena/include/VictorSP.h
+++ b/wpilibc/Athena/include/VictorSP.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 "SafePWM.h"
@@ -20,6 +21,7 @@
virtual void Set(float value, uint8_t syncGroup = 0) override;
virtual float Get() const override;
virtual void Disable() override;
+ virtual void StopMotor() override;
virtual void PIDWrite(float output) override;
diff --git a/wpilibc/Athena/include/Vision/AxisCamera.h b/wpilibc/Athena/include/Vision/AxisCamera.h
index b315938..9252442 100644
--- a/wpilibc/Athena/include/Vision/AxisCamera.h
+++ b/wpilibc/Athena/include/Vision/AxisCamera.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/BaeUtilities.h b/wpilibc/Athena/include/Vision/BaeUtilities.h
index 8983ef4..5a1270b 100644
--- a/wpilibc/Athena/include/Vision/BaeUtilities.h
+++ b/wpilibc/Athena/include/Vision/BaeUtilities.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/BinaryImage.h b/wpilibc/Athena/include/Vision/BinaryImage.h
index 8b7b25c..d9a24c5 100644
--- a/wpilibc/Athena/include/Vision/BinaryImage.h
+++ b/wpilibc/Athena/include/Vision/BinaryImage.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/ColorImage.h b/wpilibc/Athena/include/Vision/ColorImage.h
index 0304ad8..493c541 100644
--- a/wpilibc/Athena/include/Vision/ColorImage.h
+++ b/wpilibc/Athena/include/Vision/ColorImage.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/FrcError.h b/wpilibc/Athena/include/Vision/FrcError.h
index 0897c84..bfe0748 100644
--- a/wpilibc/Athena/include/Vision/FrcError.h
+++ b/wpilibc/Athena/include/Vision/FrcError.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/HSLImage.h b/wpilibc/Athena/include/Vision/HSLImage.h
index 0407748..057a8c7 100644
--- a/wpilibc/Athena/include/Vision/HSLImage.h
+++ b/wpilibc/Athena/include/Vision/HSLImage.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/ImageBase.h b/wpilibc/Athena/include/Vision/ImageBase.h
index c20ecfe..5fc0470 100644
--- a/wpilibc/Athena/include/Vision/ImageBase.h
+++ b/wpilibc/Athena/include/Vision/ImageBase.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/MonoImage.h b/wpilibc/Athena/include/Vision/MonoImage.h
index d31f0a4..856d891 100644
--- a/wpilibc/Athena/include/Vision/MonoImage.h
+++ b/wpilibc/Athena/include/Vision/MonoImage.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/RGBImage.h b/wpilibc/Athena/include/Vision/RGBImage.h
index cb3b3e5..eeed545 100644
--- a/wpilibc/Athena/include/Vision/RGBImage.h
+++ b/wpilibc/Athena/include/Vision/RGBImage.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/Threshold.h b/wpilibc/Athena/include/Vision/Threshold.h
index 83493e8..78d82b4 100644
--- a/wpilibc/Athena/include/Vision/Threshold.h
+++ b/wpilibc/Athena/include/Vision/Threshold.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/Vision/VisionAPI.h b/wpilibc/Athena/include/Vision/VisionAPI.h
index 3153f1b..7c7b5c1 100644
--- a/wpilibc/Athena/include/Vision/VisionAPI.h
+++ b/wpilibc/Athena/include/Vision/VisionAPI.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/Athena/include/WPILib.h b/wpilibc/Athena/include/WPILib.h
index 0386b03..8780c85 100644
--- a/wpilibc/Athena/include/WPILib.h
+++ b/wpilibc/Athena/include/WPILib.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
#define REAL
@@ -50,6 +51,7 @@
#include "DriverStation.h"
#include "Encoder.h"
#include "ErrorBase.h"
+#include "Filters/LinearDigitalFilter.h"
#include "GearTooth.h"
#include "GenericHID.h"
#include "interfaces/Accelerometer.h"
@@ -71,6 +73,7 @@
#include "Resource.h"
#include "RobotBase.h"
#include "RobotDrive.h"
+#include "SD540.h"
#include "SensorBase.h"
#include "SerialPort.h"
#include "Servo.h"
@@ -78,6 +81,7 @@
#include "SmartDashboard/SendableChooser.h"
#include "SmartDashboard/SmartDashboard.h"
#include "Solenoid.h"
+#include "Spark.h"
#include "SpeedController.h"
#include "SPI.h"
#include "Talon.h"
diff --git a/wpilibc/Athena/src/ADXL345_I2C.cpp b/wpilibc/Athena/src/ADXL345_I2C.cpp
index 000d4b8..ec87376 100644
--- a/wpilibc/Athena/src/ADXL345_I2C.cpp
+++ b/wpilibc/Athena/src/ADXL345_I2C.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 "ADXL345_I2C.h"
@@ -17,12 +17,13 @@
constexpr double ADXL345_I2C::kGsPerLSB;
/**
- * Constructor.
+ * Constructs the ADXL345 Accelerometer over I2C.
*
* @param port The I2C port the accelerometer is attached to
* @param range The range (+ or -) that the accelerometer will measure.
+ * @param deviceAddress the I2C address of the accelerometer (0x1D or 0x53)
*/
-ADXL345_I2C::ADXL345_I2C(Port port, Range range) : I2C(port, kAddress) {
+ADXL345_I2C::ADXL345_I2C(Port port, Range range, int deviceAddress) : I2C(port, deviceAddress) {
// Turn on the measurements
Write(kPowerCtlRegister, kPowerCtl_Measure);
// Specify the data format to read
diff --git a/wpilibc/Athena/src/ADXL345_SPI.cpp b/wpilibc/Athena/src/ADXL345_SPI.cpp
index 7fb3b71..9341db1 100644
--- a/wpilibc/Athena/src/ADXL345_SPI.cpp
+++ b/wpilibc/Athena/src/ADXL345_SPI.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 "ADXL345_SPI.h"
diff --git a/wpilibc/Athena/src/ADXL362.cpp b/wpilibc/Athena/src/ADXL362.cpp
index 3c70b9b..889e326 100644
--- a/wpilibc/Athena/src/ADXL362.cpp
+++ b/wpilibc/Athena/src/ADXL362.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 "ADXL362.h"
diff --git a/wpilibc/Athena/src/ADXRS450_Gyro.cpp b/wpilibc/Athena/src/ADXRS450_Gyro.cpp
index 6b111cf..a1ec4e3 100644
--- a/wpilibc/Athena/src/ADXRS450_Gyro.cpp
+++ b/wpilibc/Athena/src/ADXRS450_Gyro.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2015. All Rights Reserved. */
+/* Copyright (c) FIRST 2015-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. */
@@ -151,7 +151,3 @@
double ADXRS450_Gyro::GetRate() const {
return (double)m_spi.GetAccumulatorLastValue() * kDegreePerSecondPerLSB;
}
-
-std::string ADXRS450_Gyro::GetSmartDashboardType() const {
- return "ADXRS450_Gyro";
-}
diff --git a/wpilibc/Athena/src/AnalogAccelerometer.cpp b/wpilibc/Athena/src/AnalogAccelerometer.cpp
index 5cf41d9..824e751 100644
--- a/wpilibc/Athena/src/AnalogAccelerometer.cpp
+++ b/wpilibc/Athena/src/AnalogAccelerometer.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 "AnalogAccelerometer.h"
diff --git a/wpilibc/Athena/src/AnalogGyro.cpp b/wpilibc/Athena/src/AnalogGyro.cpp
index 54093ec..2574792 100644
--- a/wpilibc/Athena/src/AnalogGyro.cpp
+++ b/wpilibc/Athena/src/AnalogGyro.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 "AnalogGyro.h"
@@ -132,7 +132,7 @@
SetPIDSourceType(PIDSourceType::kDisplacement);
HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
- LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetChannel(), this);
+ LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(), this);
}
/**
@@ -251,5 +251,3 @@
(1 << m_analog->GetOversampleBits());
m_analog->SetAccumulatorDeadband(deadband);
}
-
-std::string AnalogGyro::GetSmartDashboardType() const { return "AnalogGyro"; }
diff --git a/wpilibc/Athena/src/AnalogInput.cpp b/wpilibc/Athena/src/AnalogInput.cpp
index dc8e872..7852aea 100644
--- a/wpilibc/Athena/src/AnalogInput.cpp
+++ b/wpilibc/Athena/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/Athena/src/AnalogOutput.cpp b/wpilibc/Athena/src/AnalogOutput.cpp
index 74a1f9c..2092936 100644
--- a/wpilibc/Athena/src/AnalogOutput.cpp
+++ b/wpilibc/Athena/src/AnalogOutput.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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. */
diff --git a/wpilibc/Athena/src/AnalogPotentiometer.cpp b/wpilibc/Athena/src/AnalogPotentiometer.cpp
index 1372d84..8c8aaac 100644
--- a/wpilibc/Athena/src/AnalogPotentiometer.cpp
+++ b/wpilibc/Athena/src/AnalogPotentiometer.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
#include "AnalogPotentiometer.h"
#include "ControllerPower.h"
diff --git a/wpilibc/Athena/src/AnalogTrigger.cpp b/wpilibc/Athena/src/AnalogTrigger.cpp
index b49d6a0..36a1459 100644
--- a/wpilibc/Athena/src/AnalogTrigger.cpp
+++ b/wpilibc/Athena/src/AnalogTrigger.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 "AnalogTrigger.h"
diff --git a/wpilibc/Athena/src/AnalogTriggerOutput.cpp b/wpilibc/Athena/src/AnalogTriggerOutput.cpp
index 8254275..eb67d36 100644
--- a/wpilibc/Athena/src/AnalogTriggerOutput.cpp
+++ b/wpilibc/Athena/src/AnalogTriggerOutput.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 "AnalogTriggerOutput.h"
diff --git a/wpilibc/Athena/src/BuiltInAccelerometer.cpp b/wpilibc/Athena/src/BuiltInAccelerometer.cpp
index a504524..fb4ae04 100644
--- a/wpilibc/Athena/src/BuiltInAccelerometer.cpp
+++ b/wpilibc/Athena/src/BuiltInAccelerometer.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "BuiltInAccelerometer.h"
diff --git a/wpilibc/Athena/src/CANJaguar.cpp b/wpilibc/Athena/src/CANJaguar.cpp
index fc71e84..7dcca19 100644
--- a/wpilibc/Athena/src/CANJaguar.cpp
+++ b/wpilibc/Athena/src/CANJaguar.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2009. All Rights Reserved. */
+/* Copyright (c) FIRST 2009-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 "CANJaguar.h"
@@ -245,8 +246,11 @@
uint8_t dataBuffer[8];
uint8_t dataSize;
- if (m_safetyHelper && !m_safetyHelper->IsAlive() && m_controlEnabled) {
+ if (m_safetyHelper) m_safetyHelper->Feed();
+
+ if (m_stopped) {
EnableControl();
+ m_stopped = false;
}
if (m_controlEnabled) {
@@ -289,8 +293,6 @@
}
sendMessage(messageID, dataBuffer, dataSize, kSendMessagePeriod);
-
- if (m_safetyHelper) m_safetyHelper->Feed();
}
m_value = outputValue;
@@ -1010,6 +1012,7 @@
case kPercentVbus:
case kVoltage:
case kFollower:
+ case kMotionProfile:
wpi_setWPIErrorWithContext(
IncompatibleMode,
"PID constants only apply in Speed, Position, and Current mode");
@@ -1045,6 +1048,7 @@
case kPercentVbus:
case kVoltage:
case kFollower:
+ case kMotionProfile:
wpi_setWPIErrorWithContext(
IncompatibleMode,
"PID constants only apply in Speed, Position, and Current mode");
@@ -1080,6 +1084,7 @@
case kPercentVbus:
case kVoltage:
case kFollower:
+ case kMotionProfile:
wpi_setWPIErrorWithContext(
IncompatibleMode,
"PID constants only apply in Speed, Position, and Current mode");
@@ -1516,7 +1521,7 @@
// Disable the previous mode
DisableControl();
- if (controlMode == kFollower)
+ if ((controlMode == kFollower) || (controlMode == kMotionProfile))
wpi_setWPIErrorWithContext(IncompatibleMode,
"The Jaguar only supports Current, Voltage, "
"Position, Speed, and Percent (Throttle) "
@@ -1930,12 +1935,14 @@
uint8_t CANJaguar::GetDeviceID() const { return m_deviceNumber; }
/**
- * Common interface for stopping the motor
+ * Common interface for stopping the motor until the next Set() command
* Part of the MotorSafety interface
*
* @deprecated Call DisableControl instead.
*/
-void CANJaguar::StopMotor() { DisableControl(); }
+void CANJaguar::StopMotor() {
+ m_stopped = true;
+}
/**
* Common interface for inverting direction of a speed controller.
@@ -1978,7 +1985,7 @@
if (m_table != nullptr) {
m_table->PutString("~TYPE~", "CANSpeedController");
m_table->PutString("Type", "CANJaguar");
- m_table->PutString("Mode", GetModeName(m_controlMode));
+ m_table->PutNumber("Mode", m_controlMode);
if (IsModePID(m_controlMode)) {
m_table->PutNumber("p", GetP());
m_table->PutNumber("i", GetI());
diff --git a/wpilibc/Athena/src/CANTalon.cpp b/wpilibc/Athena/src/CANTalon.cpp
index f937212..9adc66c 100644
--- a/wpilibc/Athena/src/CANTalon.cpp
+++ b/wpilibc/Athena/src/CANTalon.cpp
@@ -1,13 +1,16 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "CANTalon.h"
#include "WPIErrors.h"
#include <unistd.h> // usleep
#include <sstream>
+#include "LiveWindow/LiveWindow.h"
+
/**
* Number of adc engineering units per 0 to 3.3V sweep.
* This is necessary for scaling Analog Position in rotations/RPM.
@@ -35,6 +38,7 @@
ApplyControlMode(m_controlMode);
m_impl->SetProfileSlotSelect(m_profile);
m_isInverted = false;
+ LiveWindow::GetInstance()->AddActuator("CANTalon", m_deviceNumber, this);
}
/**
* Constructor for the CANTalon device.
@@ -48,6 +52,7 @@
m_safetyHelper(new MotorSafetyHelper(this)) {
ApplyControlMode(m_controlMode);
m_impl->SetProfileSlotSelect(m_profile);
+ LiveWindow::GetInstance()->AddActuator("CANTalon", m_deviceNumber, this);
}
CANTalon::~CANTalon() {
@@ -129,6 +134,12 @@
void CANTalon::Set(float value, uint8_t syncGroup) {
/* feed safety helper since caller just updated our output */
m_safetyHelper->Feed();
+
+ if (m_stopped) {
+ EnableControl();
+ m_stopped = false;
+ }
+
if (m_controlEnabled) {
m_setPoint = value; /* cache set point for GetSetpoint() */
CTR_Code status = CTR_OKAY;
@@ -156,6 +167,9 @@
double milliamperes = (m_isInverted ? -value : value) * 1000.0; /* mA*/
status = m_impl->SetDemand(milliamperes);
} break;
+ case CANSpeedController::kMotionProfile: {
+ status = m_impl->SetDemand((int)value);
+ } break;
default:
wpi_setWPIErrorWithContext(
IncompatibleMode,
@@ -1198,7 +1212,43 @@
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * Overrides the forward and reverse limit switch enables.
+ * Unlike ConfigLimitMode, this function allows individual control of forward and
+ * reverse limit switch enables.
+ * Unlike ConfigLimitMode, this function does not affect the soft-limit features of Talon SRX.
+ * @see ConfigLimitMode()
+ */
+void CANTalon::ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, bool bReverseLimitSwitchEn) {
+ CTR_Code status = CTR_OKAY;
+ int fwdRevEnable;
+ /* chose correct signal value based on caller's requests enables */
+ if(!bForwardLimitSwitchEn) {
+ /* caller wants Forward Limit Switch OFF */
+ if(!bReverseLimitSwitchEn) {
+ /* caller wants both OFF */
+ fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev;
+ } else {
+ /* caller Forward OFF and Reverse ON */
+ fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_DisableFwd_EnableRev;
+ }
+ } else {
+ /* caller wants Forward Limit Switch ON */
+ if(!bReverseLimitSwitchEn) {
+ /* caller wants Forward ON and Reverse OFF */
+ fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_EnableFwd_DisableRev;
+ } else {
+ /* caller wants both ON */
+ fwdRevEnable = CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev;
+ }
+ }
+ /* update signal and error check code */
+ status = m_impl->SetOverrideLimitSwitchEn(fwdRevEnable);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
* Configures the soft limit enable (wear leveled persistent memory).
* Also sets the limit switch overrides.
*/
@@ -1275,6 +1325,32 @@
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
}
+
+/**
+ * Set the Forward Soft Limit Enable.
+ * This is the same setting that is in the Web-Based Configuration.
+ * @param bForwardSoftLimitEn true to enable Soft limit, false to disable.
+ */
+void CANTalon::ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn) {
+ CTR_Code status = CTR_OKAY;
+ status = m_impl->SetForwardSoftEnable(bForwardSoftLimitEn);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Set the Reverse Soft Limit Enable.
+ * This is the same setting that is in the Web-Based Configuration.
+ * @param bReverseSoftLimitEn true to enable Soft limit, false to disable.
+ */
+void CANTalon::ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn) {
+ CTR_Code status = CTR_OKAY;
+ status = m_impl->SetReverseSoftEnable(bReverseSoftLimitEn);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
/**
* Change the fwd limit switch setting to normally open or closed.
* Talon will disable momentarilly if the Talon's current setting
@@ -1430,6 +1506,9 @@
case kFollower:
m_sendMode = kFollowerMode;
break;
+ case kMotionProfile:
+ m_sendMode = kMotionProfileMode;
+ break;
}
// Keep the talon disabled until Set() is called.
CTR_Code status = m_impl->SetModeSelect((int)kDisabled);
@@ -1668,6 +1747,136 @@
ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity,risingEdge ? 1 : 0);
}
}
+
+/**
+ * Calling application can opt to speed up the handshaking between the robot API and the Talon to increase the
+ * download rate of the Talon's Motion Profile. Ideally the period should be no more than half the period
+ * of a trajectory point.
+ */
+void CANTalon::ChangeMotionControlFramePeriod(int periodMs)
+{
+ m_impl->ChangeMotionControlFramePeriod(periodMs);
+}
+
+/**
+ * Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top).
+ * Be sure to check GetMotionProfileStatus() to know when the buffer is actually cleared.
+ */
+void CANTalon::ClearMotionProfileTrajectories()
+{
+ m_impl->ClearMotionProfileTrajectories();
+}
+
+/**
+ * Retrieve just the buffer count for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and ideal
+ * if caller needs to quickly poll the progress of trajectory points being emptied
+ * into Talon's RAM. Otherwise just use GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+int CANTalon::GetMotionProfileTopLevelBufferCount()
+{
+ return m_impl->GetMotionProfileTopLevelBufferCount();
+}
+
+/**
+ * Push another trajectory point into the top level buffer (which is emptied into
+ * the Talon's bottom buffer as room allows).
+ * @param trajPt the trajectory point to insert into buffer.
+ * @return true if trajectory point push ok. CTR_BufferFull if buffer is full
+ * due to kMotionProfileTopBufferCapacity.
+ */
+bool CANTalon::PushMotionProfileTrajectory(const TrajectoryPoint & trajPt)
+{
+ /* convert positiona and velocity to native units */
+ int32_t targPos = ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position);
+ int32_t targVel = ScaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity);
+ /* bounds check signals that require it */
+ uint32_t profileSlotSelect = (trajPt.profileSlotSelect) ? 1 : 0;
+ uint8_t timeDurMs = (trajPt.timeDurMs >= 255) ? 255 : trajPt.timeDurMs; /* cap time to 255ms */
+ /* send it to the top level buffer */
+ CTR_Code status = m_impl->PushMotionProfileTrajectory(targPos, targVel, profileSlotSelect, timeDurMs, trajPt.velocityOnly, trajPt.isLastPoint, trajPt.zeroPos);
+ return (status == CTR_OKAY) ? true : false;
+}
+/**
+ * @return true if api-level (top) buffer is full.
+ */
+bool CANTalon::IsMotionProfileTopLevelBufferFull()
+{
+ return m_impl->IsMotionProfileTopLevelBufferFull();
+}
+
+/**
+ * This must be called periodically to funnel the trajectory points from the API's top level buffer to
+ * the Talon's bottom level buffer. Recommendation is to call this twice as fast as the executation rate of the motion profile.
+ * So if MP is running with 20ms trajectory points, try calling this routine every 10ms. All motion profile functions are thread-safe
+ * through the use of a mutex, so there is no harm in having the caller utilize threading.
+ */
+void CANTalon::ProcessMotionProfileBuffer()
+{
+ m_impl->ProcessMotionProfileBuffer();
+}
+
+/**
+ * Retrieve all status information.
+ * Since this all comes from one CAN frame, its ideal to have one routine to retrieve the frame once and decode everything.
+ * @param [out] motionProfileStatus contains all progress information on the currently running MP.
+ */
+void CANTalon::GetMotionProfileStatus(MotionProfileStatus & motionProfileStatus)
+{
+ uint32_t flags;
+ uint32_t profileSlotSelect;
+ int32_t targPos, targVel;
+ uint32_t topBufferRem, topBufferCnt, btmBufferCnt;
+ uint32_t outputEnable;
+ /* retrieve all motion profile signals from status frame */
+ CTR_Code status = m_impl->GetMotionProfileStatus(flags, profileSlotSelect, targPos, targVel, topBufferRem, topBufferCnt, btmBufferCnt, outputEnable);
+ /* completely update the caller's structure */
+ motionProfileStatus.topBufferRem = topBufferRem;
+ motionProfileStatus.topBufferCnt = topBufferCnt;
+ motionProfileStatus.btmBufferCnt = btmBufferCnt;
+ motionProfileStatus.hasUnderrun = (flags & CanTalonSRX::kMotionProfileFlag_HasUnderrun) ? true :false;
+ motionProfileStatus.isUnderrun = (flags & CanTalonSRX::kMotionProfileFlag_IsUnderrun) ? true :false;
+ motionProfileStatus.activePointValid = (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsValid) ? true :false;
+ motionProfileStatus.activePoint.isLastPoint = (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_IsLast) ? true :false;
+ motionProfileStatus.activePoint.velocityOnly = (flags & CanTalonSRX::kMotionProfileFlag_ActTraj_VelOnly) ? true :false;
+ motionProfileStatus.activePoint.position = ScaleNativeUnitsToRotations(m_feedbackDevice, targPos);
+ motionProfileStatus.activePoint.velocity = ScaleNativeUnitsToRpm(m_feedbackDevice, targVel);
+ motionProfileStatus.activePoint.profileSlotSelect = profileSlotSelect;
+ switch(outputEnable){
+ case CanTalonSRX::kMotionProf_Disabled:
+ motionProfileStatus.outputEnable = SetValueMotionProfileDisable;
+ break;
+ case CanTalonSRX::kMotionProf_Enable:
+ motionProfileStatus.outputEnable = SetValueMotionProfileEnable;
+ break;
+ case CanTalonSRX::kMotionProf_Hold:
+ motionProfileStatus.outputEnable = SetValueMotionProfileHold;
+ break;
+ default:
+ motionProfileStatus.outputEnable = SetValueMotionProfileDisable;
+ break;
+ }
+ motionProfileStatus.activePoint.zeroPos = false; /* this signal is only used sending pts to Talon */
+ motionProfileStatus.activePoint.timeDurMs = 0; /* this signal is only used sending pts to Talon */
+
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another point,
+ * but the low level buffer is empty.
+ *
+ * Once the Motion Profile Executer sets the hasUnderrun flag, it stays set until
+ * Robot Application clears it with this routine, which ensures Robot Application
+ * gets a chance to instrument or react. Caller could also check the isUnderrun flag
+ * which automatically clears when fault condition is removed.
+ */
+void CANTalon::ClearMotionProfileHasUnderrun()
+{
+ ConfigSetParameter(CanTalonSRX::eMotionProfileHasUnderrunErr, 0);
+}
/**
* Common interface for inverting direction of a speed controller.
* Only works in PercentVbus, speed, and Voltage modes.
@@ -1684,12 +1893,15 @@
bool CANTalon::GetInverted() const { return m_isInverted; }
/**
- * Common interface for stopping the motor
+ * Common interface for stopping the motor until the next Set() call
* Part of the MotorSafety interface
*
* @deprecated Call Disable instead.
*/
-void CANTalon::StopMotor() { Disable(); }
+void CANTalon::StopMotor() {
+ Disable();
+ m_stopped = true;
+}
void CANTalon::ValueChanged(ITable* source, llvm::StringRef key,
std::shared_ptr<nt::Value> value, bool isNew) {
@@ -1716,7 +1928,7 @@
if (m_table != nullptr) {
m_table->PutString("~TYPE~", "CANSpeedController");
m_table->PutString("Type", "CANTalon");
- m_table->PutString("Mode", GetModeName(m_controlMode));
+ m_table->PutNumber("Mode", m_controlMode);
m_table->PutNumber("p", GetP());
m_table->PutNumber("i", GetI());
m_table->PutNumber("d", GetD());
diff --git a/wpilibc/Athena/src/CameraServer.cpp b/wpilibc/Athena/src/CameraServer.cpp
index af319a4..54b7e06 100644
--- a/wpilibc/Athena/src/CameraServer.cpp
+++ b/wpilibc/Athena/src/CameraServer.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
#include "CameraServer.h"
#include "WPIErrors.h"
#include "Utility.h"
diff --git a/wpilibc/Athena/src/ControllerPower.cpp b/wpilibc/Athena/src/ControllerPower.cpp
index ea508af..a62539f 100644
--- a/wpilibc/Athena/src/ControllerPower.cpp
+++ b/wpilibc/Athena/src/ControllerPower.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "ControllerPower.h"
diff --git a/wpilibc/Athena/src/Counter.cpp b/wpilibc/Athena/src/Counter.cpp
index cd70de5..a138867 100644
--- a/wpilibc/Athena/src/Counter.cpp
+++ b/wpilibc/Athena/src/Counter.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 "Counter.h"
diff --git a/wpilibc/Athena/src/DigitalGlitchFilter.cpp b/wpilibc/Athena/src/DigitalGlitchFilter.cpp
index 56147cb..4cde562 100644
--- a/wpilibc/Athena/src/DigitalGlitchFilter.cpp
+++ b/wpilibc/Athena/src/DigitalGlitchFilter.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2015. All Rights Reserved. */
+/* Copyright (c) FIRST 2015-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 <algorithm>
diff --git a/wpilibc/Athena/src/DigitalInput.cpp b/wpilibc/Athena/src/DigitalInput.cpp
index f6b96f5..19538a4 100644
--- a/wpilibc/Athena/src/DigitalInput.cpp
+++ b/wpilibc/Athena/src/DigitalInput.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 "DigitalInput.h"
diff --git a/wpilibc/Athena/src/DigitalOutput.cpp b/wpilibc/Athena/src/DigitalOutput.cpp
index d74f93b..99b16e8 100644
--- a/wpilibc/Athena/src/DigitalOutput.cpp
+++ b/wpilibc/Athena/src/DigitalOutput.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 "DigitalOutput.h"
diff --git a/wpilibc/Athena/src/DoubleSolenoid.cpp b/wpilibc/Athena/src/DoubleSolenoid.cpp
index 4fb2943..f8ed267 100644
--- a/wpilibc/Athena/src/DoubleSolenoid.cpp
+++ b/wpilibc/Athena/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/Athena/src/DriverStation.cpp b/wpilibc/Athena/src/DriverStation.cpp
index 455a987..3226223 100644
--- a/wpilibc/Athena/src/DriverStation.cpp
+++ b/wpilibc/Athena/src/DriverStation.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 "DriverStation.h"
@@ -88,8 +88,8 @@
* @return Pointer to the DS instance
*/
DriverStation &DriverStation::GetInstance() {
- static DriverStation instance;
- return instance;
+ static DriverStation *instance = new DriverStation();
+ return *instance;
}
/**
@@ -134,6 +134,18 @@
}
/**
+ * Reports errors related to unplugged joysticks
+ * Throttles the errors so that they don't overwhelm the DS
+ */
+void DriverStation::ReportJoystickUnpluggedWarning(std::string message) {
+ double currentTime = Timer::GetFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ ReportWarning(message);
+ m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+ }
+}
+
+/**
* Returns the number of axes on a given joystick port
*
* @param stick The joystick port number
@@ -159,7 +171,7 @@
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
}
- std::string retVal(m_joystickDescriptor[0].name);
+ std::string retVal(m_joystickDescriptor[stick].name);
return retVal;
}
@@ -255,9 +267,8 @@
if (axis >= kMaxJoystickAxes)
wpi_setWPIError(BadJoystickAxis);
else
- ReportJoystickUnpluggedError(
- "WARNING: Joystick Axis missing, check if all controllers are "
- "plugged in\n");
+ ReportJoystickUnpluggedWarning(
+ "Joystick Axis missing, check if all controllers are plugged in");
return 0.0f;
}
@@ -285,9 +296,8 @@
if (pov >= kMaxJoystickPOVs)
wpi_setWPIError(BadJoystickAxis);
else
- ReportJoystickUnpluggedError(
- "WARNING: Joystick POV missing, check if all controllers are plugged "
- "in\n");
+ ReportJoystickUnpluggedWarning(
+ "Joystick POV missing, check if all controllers are plugged in");
return -1;
}
@@ -323,14 +333,13 @@
}
if (button > m_joystickButtons[stick].count) {
- ReportJoystickUnpluggedError(
- "WARNING: Joystick Button missing, check if all controllers are "
- "plugged in\n");
+ ReportJoystickUnpluggedWarning(
+ "Joystick Button missing, check if all controllers are plugged in");
return false;
}
if (button == 0) {
ReportJoystickUnpluggedError(
- "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+ "Button indexes begin at 1 in WPILib for C++ and Java");
return false;
}
return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
@@ -527,11 +536,25 @@
* The error is also printed to the program console.
*/
void DriverStation::ReportError(std::string error) {
- std::cout << error << std::endl;
+ HALSendError(1, 1, 0, error.c_str(), "", "", 1);
+}
- 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) {
+ HALSendError(0, 1, 0, error.c_str(), "", "", 1);
+}
+
+/**
+ * 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) {
+ HALSendError(is_error, code, 0, error.c_str(), location.c_str(),
+ stack.c_str(), 1);
}
diff --git a/wpilibc/Athena/src/Encoder.cpp b/wpilibc/Athena/src/Encoder.cpp
index 0fa12df..7c5a499 100644
--- a/wpilibc/Athena/src/Encoder.cpp
+++ b/wpilibc/Athena/src/Encoder.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 "Encoder.h"
diff --git a/wpilibc/Athena/src/GearTooth.cpp b/wpilibc/Athena/src/GearTooth.cpp
index b659125..4eed6b6 100644
--- a/wpilibc/Athena/src/GearTooth.cpp
+++ b/wpilibc/Athena/src/GearTooth.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 "GearTooth.h"
diff --git a/wpilibc/Athena/src/I2C.cpp b/wpilibc/Athena/src/I2C.cpp
index 79767b3..d70a132 100644
--- a/wpilibc/Athena/src/I2C.cpp
+++ b/wpilibc/Athena/src/I2C.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 "I2C.h"
diff --git a/wpilibc/Athena/src/Internal/HardwareHLReporting.cpp b/wpilibc/Athena/src/Internal/HardwareHLReporting.cpp
index 2176c80..3b1a7b6 100644
--- a/wpilibc/Athena/src/Internal/HardwareHLReporting.cpp
+++ b/wpilibc/Athena/src/Internal/HardwareHLReporting.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 "Internal/HardwareHLReporting.h"
#include "HAL/HAL.hpp"
diff --git a/wpilibc/Athena/src/InterruptableSensorBase.cpp b/wpilibc/Athena/src/InterruptableSensorBase.cpp
index 6e66c34..7c9a80f 100644
--- a/wpilibc/Athena/src/InterruptableSensorBase.cpp
+++ b/wpilibc/Athena/src/InterruptableSensorBase.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 "InterruptableSensorBase.h"
diff --git a/wpilibc/Athena/src/IterativeRobot.cpp b/wpilibc/Athena/src/IterativeRobot.cpp
index fa1d2ba..3d6946a 100644
--- a/wpilibc/Athena/src/IterativeRobot.cpp
+++ b/wpilibc/Athena/src/IterativeRobot.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 "IterativeRobot.h"
diff --git a/wpilibc/Athena/src/Jaguar.cpp b/wpilibc/Athena/src/Jaguar.cpp
index ce61d36..b2bd57a 100644
--- a/wpilibc/Athena/src/Jaguar.cpp
+++ b/wpilibc/Athena/src/Jaguar.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 "Jaguar.h"
@@ -77,3 +77,8 @@
* @param output Write out the PWM value as was found in the PIDController
*/
void Jaguar::PIDWrite(float output) { Set(output); }
+
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void Jaguar::StopMotor() { this->SafePWM::StopMotor(); }
\ No newline at end of file
diff --git a/wpilibc/Athena/src/Joystick.cpp b/wpilibc/Athena/src/Joystick.cpp
index 388832c..0a5b623 100644
--- a/wpilibc/Athena/src/Joystick.cpp
+++ b/wpilibc/Athena/src/Joystick.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 "Joystick.h"
diff --git a/wpilibc/Athena/src/MotorSafetyHelper.cpp b/wpilibc/Athena/src/MotorSafetyHelper.cpp
index 60bba30..a78c348 100644
--- a/wpilibc/Athena/src/MotorSafetyHelper.cpp
+++ b/wpilibc/Athena/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/Athena/src/Notifier.cpp b/wpilibc/Athena/src/Notifier.cpp
index fc3059c..0023100 100644
--- a/wpilibc/Athena/src/Notifier.cpp
+++ b/wpilibc/Athena/src/Notifier.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 "Notifier.h"
@@ -11,207 +11,63 @@
#include "WPIErrors.h"
#include "HAL/HAL.hpp"
-Notifier *Notifier::timerQueueHead = nullptr;
-priority_recursive_mutex Notifier::queueMutex;
-priority_mutex Notifier::halMutex;
-void *Notifier::m_notifier = nullptr;
-std::atomic<int> Notifier::refcount{0};
-
/**
* Create a Notifier for timer event notification.
* @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;
- // do the first time intialization of static variables
- if (refcount.fetch_add(1) == 0) {
- int32_t status = 0;
- {
- std::lock_guard<priority_mutex> sync(halMutex);
- if (!m_notifier)
- m_notifier = initializeNotifier(ProcessQueue, nullptr, &status);
- }
- wpi_setErrorWithContext(status, getHALErrorMessage(status));
- }
+ int32_t status = 0;
+ m_notifier = initializeNotifier(&Notifier::Notify, this, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
* Free the resources for a timer event.
- * All resources will be freed and the timer event will be removed from the
- * queue if necessary.
*/
Notifier::~Notifier() {
- {
- std::lock_guard<priority_recursive_mutex> sync(queueMutex);
- DeleteFromQueue();
- }
+ int32_t status = 0;
+ cleanNotifier(m_notifier, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
- // Delete the static variables when the last one is going away
- if (refcount.fetch_sub(1) == 1) {
- int32_t status = 0;
- {
- std::lock_guard<priority_mutex> sync(halMutex);
- if (m_notifier) {
- cleanNotifier(m_notifier, &status);
- m_notifier = nullptr;
- }
- }
- wpi_setErrorWithContext(status, getHALErrorMessage(status));
- }
-
- // Acquire the mutex; this makes certain that the handler is
- // not being executed by the interrupt manager.
+ /* Acquire the mutex; this makes certain that the handler is not being
+ * executed by the interrupt manager.
+ */
std::lock_guard<priority_mutex> lock(m_handlerMutex);
}
/**
- * Update the alarm hardware to reflect the current first element in the queue.
- * Compute the time the next alarm should occur based on the current time and
- * the
- * period for the first element in the timer queue.
- * WARNING: this method does not do synchronization! It must be called from
- * somewhere
- * that is taking care of synchronizing access to the queue.
+ * Update the HAL alarm time.
*/
void Notifier::UpdateAlarm() {
- if (timerQueueHead != nullptr) {
- int32_t status = 0;
- // This locking is necessary in order to avoid two things:
- // 1) Race condition issues with calling cleanNotifer() and
- // updateNotifierAlarm() at the same time.
- // 2) Avoid deadlock by making it so that this won't block waiting
- // for the mutex to unlock.
- // Checking refcount as well is unnecessary, but will not hurt.
- if (halMutex.try_lock() && refcount != 0) {
- if (m_notifier)
- updateNotifierAlarm(m_notifier,
- (uint32_t)(timerQueueHead->m_expirationTime * 1e6),
- &status);
- halMutex.unlock();
- }
- wpi_setStaticErrorWithContext(timerQueueHead, status,
- getHALErrorMessage(status));
- }
+ int32_t status = 0;
+ updateNotifierAlarm(m_notifier, (uint64_t)(m_expirationTime * 1e6), &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
/**
- * ProcessQueue is called whenever there is a timer interrupt.
- * We need to wake up and process the current top item in the timer queue as
- * long
- * as its scheduled time is after the current time. Then the item is removed or
- * rescheduled (repetitive events) in the queue.
+ * Notify is called by the HAL layer. We simply need to pass it through to
+ * the user handler.
*/
-void Notifier::ProcessQueue(uint32_t currentTimeInt, void *params) {
- Notifier *current;
- while (true) // keep processing past events until no more
- {
- {
- std::lock_guard<priority_recursive_mutex> sync(queueMutex);
- double currentTime = currentTimeInt * 1.0e-6;
- current = timerQueueHead;
- if (current == nullptr || current->m_expirationTime > currentTime) {
- break; // no more timer events to process
- }
- // need to process this entry
- timerQueueHead = current->m_nextEvent;
- if (current->m_periodic) {
- // if periodic, requeue the event
- // compute when to put into queue
- current->InsertInQueue(true);
- } else {
- // not periodic; removed from queue
- current->m_queued = false;
- }
- // Take handler mutex while holding queue mutex to make sure
- // the handler will execute to completion in case we are being deleted.
- current->m_handlerMutex.lock();
- }
+void Notifier::Notify(uint64_t currentTimeInt, void *param) {
+ Notifier* notifier = static_cast<Notifier*>(param);
- current->m_handler(current->m_param); // call the event handler
- current->m_handlerMutex.unlock();
+ notifier->m_processMutex.lock();
+ if (notifier->m_periodic) {
+ notifier->m_expirationTime += notifier->m_period;
+ notifier->UpdateAlarm();
}
- // reschedule the first item in the queue
- std::lock_guard<priority_recursive_mutex> sync(queueMutex);
- UpdateAlarm();
-}
-/**
- * Insert this Notifier into the timer queue in right place.
- * WARNING: this method does not do synchronization! It must be called from
- * somewhere
- * that is taking care of synchronizing access to the queue.
- * @param reschedule If false, the scheduled alarm is based on the current time
- * and UpdateAlarm
- * method is called which will enable the alarm if necessary.
- * If true, update the time by adding the period (no drift) when rescheduled
- * periodic from ProcessQueue.
- * This ensures that the public methods only update the queue after finishing
- * inserting.
- */
-void Notifier::InsertInQueue(bool reschedule) {
- if (reschedule) {
- m_expirationTime += m_period;
- } else {
- m_expirationTime = GetClock() + m_period;
- }
- if (m_expirationTime > Timer::kRolloverTime) {
- m_expirationTime -= Timer::kRolloverTime;
- }
- if (timerQueueHead == nullptr ||
- timerQueueHead->m_expirationTime >= this->m_expirationTime) {
- // the queue is empty or greater than the new entry
- // the new entry becomes the first element
- this->m_nextEvent = timerQueueHead;
- timerQueueHead = this;
- if (!reschedule) {
- // since the first element changed, update alarm, unless we already plan
- // to
- UpdateAlarm();
- }
- } 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;
-}
+ auto handler = notifier->m_handler;
-/**
- * Delete this Notifier from the timer queue.
- * WARNING: this method does not do synchronization! It must be called from
- * somewhere
- * that is taking care of synchronizing access to the queue.
- * Remove this Notifier from the timer queue and adjust the next interrupt time
- * to reflect
- * the current top of the queue.
- */
-void Notifier::DeleteFromQueue() {
- if (m_queued) {
- m_queued = false;
- wpi_assert(timerQueueHead != nullptr);
- if (timerQueueHead == this) {
- // remove the first item in the list - update the alarm
- timerQueueHead = this->m_nextEvent;
- 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
- }
- }
- }
- }
+ notifier->m_handlerMutex.lock();
+ notifier->m_processMutex.unlock();
+
+ if (handler) handler();
+ notifier->m_handlerMutex.unlock();
}
/**
@@ -220,43 +76,41 @@
* @param delay Seconds to wait before the handler is called.
*/
void Notifier::StartSingle(double delay) {
- std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ std::lock_guard<priority_mutex> sync(m_processMutex);
m_periodic = false;
m_period = delay;
- DeleteFromQueue();
- InsertInQueue(false);
+ m_expirationTime = GetClock() + m_period;
+ UpdateAlarm();
}
/**
* Register for periodic event notification.
* A timer event is queued for periodic event notification. Each time the
- * interrupt
- * occurs, the event will be immediately requeued for the same time interval.
+ * interrupt occurs, the event will be immediately requeued for the same time
+ * interval.
* @param period Period in seconds to call the handler starting one period after
* the call to this method.
*/
void Notifier::StartPeriodic(double period) {
- std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ std::lock_guard<priority_mutex> sync(m_processMutex);
m_periodic = true;
m_period = period;
- DeleteFromQueue();
- InsertInQueue(false);
+ m_expirationTime = GetClock() + m_period;
+ UpdateAlarm();
}
/**
* Stop timer events from occuring.
* Stop any repeating timer events from occuring. This will also remove any
- * single
- * notification events from the queue.
+ * single notification events from the queue.
* If a timer-based call to the registered handler is in progress, this function
- * will
- * block until the handler call is complete.
+ * will block until the handler call is complete.
*/
void Notifier::Stop() {
- {
- std::lock_guard<priority_recursive_mutex> sync(queueMutex);
- DeleteFromQueue();
- }
+ int32_t status = 0;
+ stopNotifierAlarm(m_notifier, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
// Wait for a currently executing handler to complete before returning from
// Stop()
std::lock_guard<priority_mutex> sync(m_handlerMutex);
diff --git a/wpilibc/Athena/src/PIDController.cpp b/wpilibc/Athena/src/PIDController.cpp
index fdbbb5a..c8fb63f 100644
--- a/wpilibc/Athena/src/PIDController.cpp
+++ b/wpilibc/Athena/src/PIDController.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 "PIDController.h"
@@ -55,7 +55,7 @@
void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
PIDSource *source, PIDOutput *output,
float period) {
- m_controlLoop = std::make_unique<Notifier>(PIDController::CallCalculate, this);
+ m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_P = Kp;
m_I = Ki;
@@ -67,6 +67,7 @@
m_period = period;
m_controlLoop->StartPeriodic(m_period);
+ m_setpointTimer.Start();
static int32_t instances = 0;
instances++;
@@ -78,24 +79,8 @@
}
/**
- * 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
- */
-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.
+ * This should only be called by the Notifier.
*/
void PIDController::Calculate() {
bool enabled;
@@ -103,7 +88,7 @@
PIDOutput *pidOutput;
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
pidInput = m_pidInput;
pidOutput = m_pidOutput;
enabled = m_enabled;
@@ -113,7 +98,7 @@
if (pidOutput == nullptr) return;
if (enabled) {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
float input = pidInput->PIDGet();
float result;
PIDOutput *pidOutput;
@@ -142,7 +127,7 @@
}
}
- 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) {
@@ -158,9 +143,9 @@
}
m_result = m_P * m_error + m_I * m_totalError +
- m_D * (m_prevInput - input) + m_setpoint * m_F;
+ m_D * (m_error - m_prevError) + CalculateFeedForward();
}
- m_prevInput = input;
+ m_prevError = m_error;
if (m_result > m_maximumOutput)
m_result = m_maximumOutput;
@@ -184,6 +169,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
@@ -192,7 +204,7 @@
*/
void PIDController::SetPID(double p, double i, double d) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -215,7 +227,7 @@
*/
void PIDController::SetPID(double p, double i, double d, double f) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -235,7 +247,7 @@
* @return proportional coefficient
*/
double PIDController::GetP() const {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_P;
}
@@ -244,7 +256,7 @@
* @return integral coefficient
*/
double PIDController::GetI() const {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_I;
}
@@ -253,7 +265,7 @@
* @return differential coefficient
*/
double PIDController::GetD() const {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_D;
}
@@ -262,7 +274,7 @@
* @return Feed forward coefficient
*/
double PIDController::GetF() const {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_F;
}
@@ -272,7 +284,7 @@
* @return the latest calculated output
*/
float PIDController::Get() const {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_result;
}
@@ -284,7 +296,7 @@
* @param continuous Set to true turns on continuous, false turns off continuous
*/
void PIDController::SetContinuous(bool continuous) {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_continuous = continuous;
}
@@ -296,7 +308,7 @@
*/
void PIDController::SetInputRange(float minimumInput, float maximumInput) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
@@ -312,7 +324,7 @@
*/
void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
@@ -325,7 +337,8 @@
*/
void PIDController::SetSetpoint(float setpoint) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput)
m_setpoint = m_maximumInput;
@@ -351,18 +364,27 @@
* @return the current setpoint
*/
double PIDController::GetSetpoint() const {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(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();
+}
+
+/**
* Returns the current difference of the input from the setpoint
* @return the current error
*/
float PIDController::GetError() const {
double pidInput;
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
pidInput = m_pidInput->PIDGet();
}
return GetSetpoint() - pidInput;
@@ -391,7 +413,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();
}
@@ -405,7 +427,7 @@
*/
void PIDController::SetTolerance(float percent) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -418,7 +440,7 @@
*/
void PIDController::SetPercentTolerance(float percent) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -431,7 +453,7 @@
*/
void PIDController::SetAbsoluteTolerance(float absTolerance) {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
@@ -447,6 +469,7 @@
* @param bufLength Number of previous cycles to average. Defaults to 1.
*/
void PIDController::SetToleranceBuffer(unsigned bufLength) {
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_bufLength = bufLength;
// Cut the buffer down to size if needed.
@@ -464,11 +487,12 @@
* setpoint.
* Ideally it should be based on being within the tolerance for some period of
* time.
+ * This will return false until at least one input value has been computed.
*/
bool PIDController::OnTarget() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ if (m_buf.size() == 0) return false;
double error = GetAvgError();
-
- std::lock_guard<priority_mutex> sync(m_mutex);
switch (m_toleranceType) {
case kPercentTolerance:
return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
@@ -488,7 +512,7 @@
*/
void PIDController::Enable() {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_enabled = true;
}
@@ -502,7 +526,7 @@
*/
void PIDController::Disable() {
{
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_pidOutput->PIDWrite(0);
m_enabled = false;
}
@@ -516,7 +540,7 @@
* Return true if PIDController is enabled.
*/
bool PIDController::IsEnabled() const {
- std::lock_guard<priority_mutex> sync(m_mutex);
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_enabled;
}
@@ -526,8 +550,8 @@
void PIDController::Reset() {
Disable();
- std::lock_guard<priority_mutex> sync(m_mutex);
- m_prevInput = 0;
+ std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+ m_prevError = 0;
m_totalError = 0;
m_result = 0;
}
diff --git a/wpilibc/Athena/src/PWM.cpp b/wpilibc/Athena/src/PWM.cpp
index 6b3b4b7..76d10cc 100644
--- a/wpilibc/Athena/src/PWM.cpp
+++ b/wpilibc/Athena/src/PWM.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 "PWM.h"
diff --git a/wpilibc/Athena/src/PowerDistributionPanel.cpp b/wpilibc/Athena/src/PowerDistributionPanel.cpp
index fb428cf..8378e2f 100644
--- a/wpilibc/Athena/src/PowerDistributionPanel.cpp
+++ b/wpilibc/Athena/src/PowerDistributionPanel.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved.
- */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "PowerDistributionPanel.h"
diff --git a/wpilibc/Athena/src/Preferences.cpp b/wpilibc/Athena/src/Preferences.cpp
index 95f6a4a..60a5520 100644
--- a/wpilibc/Athena/src/Preferences.cpp
+++ b/wpilibc/Athena/src/Preferences.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved. */
+/* Copyright (c) FIRST 2011-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 "Preferences.h"
diff --git a/wpilibc/Athena/src/Relay.cpp b/wpilibc/Athena/src/Relay.cpp
index 17f0ef8..5290d8b 100644
--- a/wpilibc/Athena/src/Relay.cpp
+++ b/wpilibc/Athena/src/Relay.cpp
@@ -1,9 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* 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. */
+/* 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 "Relay.h"
diff --git a/wpilibc/Athena/src/RobotBase.cpp b/wpilibc/Athena/src/RobotBase.cpp
index 2bd1d13..a1087c0 100644
--- a/wpilibc/Athena/src/RobotBase.cpp
+++ b/wpilibc/Athena/src/RobotBase.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 "RobotBase.h"
@@ -27,6 +27,7 @@
RobotBase &RobotBase::getInstance() { return *m_instance; }
void RobotBase::robotSetup(RobotBase *robot) {
+ printf("\n********** Robot program starting **********\n");
robot->StartCompetition();
}
@@ -49,12 +50,13 @@
RobotBase::setInstance(this);
NetworkTable::SetNetworkIdentity("Robot");
+ NetworkTable::SetPersistentFilename("/home/lvuser/networktables.ini");
FILE *file = nullptr;
file = fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
if (file != nullptr) {
- fputs("2016 C++ Beta5.0", file);
+ fputs("2016 C++ Release 4", file);
fclose(file);
}
}
diff --git a/wpilibc/Athena/src/RobotDrive.cpp b/wpilibc/Athena/src/RobotDrive.cpp
index c1353e9..69184e3 100644
--- a/wpilibc/Athena/src/RobotDrive.cpp
+++ b/wpilibc/Athena/src/RobotDrive.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 "RobotDrive.h"
@@ -745,9 +745,9 @@
}
void RobotDrive::StopMotor() {
- if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable();
- if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable();
- if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable();
- if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable();
+ if (m_frontLeftMotor != nullptr) m_frontLeftMotor->StopMotor();
+ if (m_frontRightMotor != nullptr) m_frontRightMotor->StopMotor();
+ if (m_rearLeftMotor != nullptr) m_rearLeftMotor->StopMotor();
+ if (m_rearRightMotor != nullptr) m_rearRightMotor->StopMotor();
m_safetyHelper->Feed();
}
diff --git a/wpilibc/Athena/src/SD540.cpp b/wpilibc/Athena/src/SD540.cpp
new file mode 100644
index 0000000..9d2ad1e
--- /dev/null
+++ b/wpilibc/Athena/src/SD540.cpp
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "SD540.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Note that the SD540 uses the following bounds for PWM values. These values
+ * should work reasonably well for
+ * most controllers, but if users experience issues such as asymmetric behavior
+ * around
+ * the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended.
+ * The calibration procedure can be found in the SD540 User Manual available
+ * from Mindsensors.
+ *
+ * 2.05ms = full "forward"
+ * 1.55ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.44ms = the "low end" of the deadband range
+ * 0.94ms = full "reverse"
+ */
+
+/**
+ * Constructor for a SD540
+ * @param channel The PWM channel that the SD540 is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+SD540::SD540(uint32_t channel) : SafePWM(channel) {
+ SetBounds(2.05, 1.55, 1.50, 1.44, .94);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetRaw(m_centerPwm);
+ SetZeroLatch();
+
+ HALReport(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
+ LiveWindow::GetInstance()->AddActuator("SD540", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void SD540::Set(float speed, uint8_t syncGroup) {
+ SetSpeed(m_isInverted ? -speed : speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float SD540::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for inverting direction of a speed controller.
+ * @param isInverted The state of inversion, true is inverted.
+ */
+void SD540::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool SD540::GetInverted() const { return m_isInverted; }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void SD540::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void SD540::PIDWrite(float output) { Set(output); }
+
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void SD540::StopMotor() { this->SafePWM::StopMotor(); }
diff --git a/wpilibc/Athena/src/SPI.cpp b/wpilibc/Athena/src/SPI.cpp
index a470597..f0af9e7 100644
--- a/wpilibc/Athena/src/SPI.cpp
+++ b/wpilibc/Athena/src/SPI.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 "SPI.h"
diff --git a/wpilibc/Athena/src/SafePWM.cpp b/wpilibc/Athena/src/SafePWM.cpp
index 801e3ee..d400ce4 100644
--- a/wpilibc/Athena/src/SafePWM.cpp
+++ b/wpilibc/Athena/src/SafePWM.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 "SafePWM.h"
diff --git a/wpilibc/Athena/src/SampleRobot.cpp b/wpilibc/Athena/src/SampleRobot.cpp
index 37b134f..3cd3cad 100644
--- a/wpilibc/Athena/src/SampleRobot.cpp
+++ b/wpilibc/Athena/src/SampleRobot.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 "SampleRobot.h"
diff --git a/wpilibc/Athena/src/SensorBase.cpp b/wpilibc/Athena/src/SensorBase.cpp
index 05f9f4f..984ad57 100644
--- a/wpilibc/Athena/src/SensorBase.cpp
+++ b/wpilibc/Athena/src/SensorBase.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 "SensorBase.h"
diff --git a/wpilibc/Athena/src/SerialPort.cpp b/wpilibc/Athena/src/SerialPort.cpp
index c4e259a..1544a62 100644
--- a/wpilibc/Athena/src/SerialPort.cpp
+++ b/wpilibc/Athena/src/SerialPort.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 "SerialPort.h"
diff --git a/wpilibc/Athena/src/Servo.cpp b/wpilibc/Athena/src/Servo.cpp
index 4300cc4..6c8c3b0 100644
--- a/wpilibc/Athena/src/Servo.cpp
+++ b/wpilibc/Athena/src/Servo.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 "Servo.h"
diff --git a/wpilibc/Athena/src/Solenoid.cpp b/wpilibc/Athena/src/Solenoid.cpp
index 5174f98..1192d26 100644
--- a/wpilibc/Athena/src/Solenoid.cpp
+++ b/wpilibc/Athena/src/Solenoid.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 "Solenoid.h"
diff --git a/wpilibc/Athena/src/SolenoidBase.cpp b/wpilibc/Athena/src/SolenoidBase.cpp
index dfdf1eb..254d144 100644
--- a/wpilibc/Athena/src/SolenoidBase.cpp
+++ b/wpilibc/Athena/src/SolenoidBase.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 "SolenoidBase.h"
diff --git a/wpilibc/Athena/src/Spark.cpp b/wpilibc/Athena/src/Spark.cpp
new file mode 100644
index 0000000..3517122
--- /dev/null
+++ b/wpilibc/Athena/src/Spark.cpp
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Spark.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Note that the Spark uses the following bounds for PWM values. These values
+ * should work reasonably well for
+ * most controllers, but if users experience issues such as asymmetric behavior
+ * around
+ * the deadband or inability to saturate the controller in either direction,
+ * calibration is recommended.
+ * The calibration procedure can be found in the Spark User Manual available
+ * from REV Robotics.
+ *
+ * 2.003ms = full "forward"
+ * 1.55ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.46ms = the "low end" of the deadband range
+ * 0.999ms = full "reverse"
+ */
+
+/**
+ * Constructor for a Spark
+ * @param channel The PWM channel that the Spark is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+Spark::Spark(uint32_t channel) : SafePWM(channel) {
+ SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetRaw(m_centerPwm);
+ SetZeroLatch();
+
+ HALReport(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
+ LiveWindow::GetInstance()->AddActuator("Spark", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void Spark::Set(float speed, uint8_t syncGroup) {
+ SetSpeed(m_isInverted ? -speed : speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float Spark::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for inverting direction of a speed controller.
+ * @param isInverted The state of inversion, true is inverted.
+ */
+void Spark::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool Spark::GetInverted() const { return m_isInverted; }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Spark::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Spark::PIDWrite(float output) { Set(output); }
+
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void Spark::StopMotor() { this->SafePWM::StopMotor(); }
\ No newline at end of file
diff --git a/wpilibc/Athena/src/Talon.cpp b/wpilibc/Athena/src/Talon.cpp
index dc1cf3b..64f9db8 100644
--- a/wpilibc/Athena/src/Talon.cpp
+++ b/wpilibc/Athena/src/Talon.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 "Talon.h"
@@ -82,3 +82,8 @@
* @param output Write out the PWM value as was found in the PIDController
*/
void Talon::PIDWrite(float output) { Set(output); }
+
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void Talon::StopMotor() { this->SafePWM::StopMotor(); }
diff --git a/wpilibc/Athena/src/TalonSRX.cpp b/wpilibc/Athena/src/TalonSRX.cpp
index 07438c4..e06f8ca 100644
--- a/wpilibc/Athena/src/TalonSRX.cpp
+++ b/wpilibc/Athena/src/TalonSRX.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 "TalonSRX.h"
@@ -79,3 +79,8 @@
* @param output Write out the PWM value as was found in the PIDController
*/
void TalonSRX::PIDWrite(float output) { Set(output); }
+
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void TalonSRX::StopMotor() { this->SafePWM::StopMotor(); }
\ No newline at end of file
diff --git a/wpilibc/Athena/src/Task.cpp b/wpilibc/Athena/src/Task.cpp
index 9157dca..5aa795b 100644
--- a/wpilibc/Athena/src/Task.cpp
+++ b/wpilibc/Athena/src/Task.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 "Task.h"
diff --git a/wpilibc/Athena/src/Timer.cpp b/wpilibc/Athena/src/Timer.cpp
index 53631e7..850b6be 100644
--- a/wpilibc/Athena/src/Timer.cpp
+++ b/wpilibc/Athena/src/Timer.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 "Timer.h"
diff --git a/wpilibc/Athena/src/USBCamera.cpp b/wpilibc/Athena/src/USBCamera.cpp
index 0c57d85..3f682a2 100644
--- a/wpilibc/Athena/src/USBCamera.cpp
+++ b/wpilibc/Athena/src/USBCamera.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
#include "USBCamera.h"
#include "Utility.h"
diff --git a/wpilibc/Athena/src/Ultrasonic.cpp b/wpilibc/Athena/src/Ultrasonic.cpp
index ba25a04..183e1ce 100644
--- a/wpilibc/Athena/src/Ultrasonic.cpp
+++ b/wpilibc/Athena/src/Ultrasonic.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 "Ultrasonic.h"
@@ -15,18 +15,17 @@
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
-constexpr double
- Ultrasonic::kPingTime; ///< Time (sec) for the ping trigger pulse.
-const uint32_t Ultrasonic::kPriority; ///< Priority that the ultrasonic round
- ///robin task runs.
-constexpr double
- Ultrasonic::kMaxUltrasonicTime; ///< Max time (ms) between readings.
+// Time (sec) for the ping trigger pulse.
+constexpr double Ultrasonic::kPingTime;
+// Priority that the ultrasonic round robin task runs.
+const uint32_t Ultrasonic::kPriority;
+// Max time (ms) between readings.
+constexpr double Ultrasonic::kMaxUltrasonicTime;
constexpr double Ultrasonic::kSpeedOfSoundInchesPerSec;
-Ultrasonic *Ultrasonic::m_firstSensor =
- nullptr; // head of the ultrasonic sensor list
Task Ultrasonic::m_task;
-std::atomic<bool> Ultrasonic::m_automaticEnabled{false}; // automatic round robin mode
-priority_mutex Ultrasonic::m_mutex;
+// automatic round robin mode
+std::atomic<bool> Ultrasonic::m_automaticEnabled{false};
+std::set<Ultrasonic*> Ultrasonic::m_sensors;
/**
* Background task that goes through the list of ultrasonic sensors and pings
@@ -34,20 +33,21 @@
* is configured to read the timing of the returned echo pulse.
*
* DANGER WILL ROBINSON, DANGER WILL ROBINSON:
- * This code runs as a task and assumes that none of the ultrasonic sensors will
- * change while it's
- * running. If one does, then this will certainly break. Make sure to disable
- * automatic mode before changing
- * anything with the sensors!!
+ * This code runs as a task and assumes that none of the ultrasonic sensors
+ * will change while it's running. Make sure to disable automatic mode before
+ * touching the list.
*/
void Ultrasonic::UltrasonicChecker() {
- Ultrasonic *u = nullptr;
while (m_automaticEnabled) {
- if (u == nullptr) u = m_firstSensor;
- if (u == nullptr) return;
- if (u->IsEnabled()) u->m_pingChannel->Pulse(kPingTime); // do the ping
- u = u->m_nextSensor;
- Wait(0.1); // wait for ping to return
+ for (auto& sensor : m_sensors) {
+ if (!m_automaticEnabled) break;
+
+ if (sensor->IsEnabled()) {
+ sensor->m_pingChannel->Pulse(kPingTime); // do the ping
+ }
+
+ Wait(0.1); // wait for ping to return
+ }
}
}
@@ -65,11 +65,7 @@
bool originalMode = m_automaticEnabled;
SetAutomaticMode(false); // kill task when adding a new sensor
// link this instance on the list
- {
- std::lock_guard<priority_mutex> lock(m_mutex);
- m_nextSensor = m_firstSensor;
- m_firstSensor = this;
- }
+ m_sensors.insert(this);
m_counter.SetMaxPeriod(1.0);
m_counter.SetSemiPeriodMode(true);
@@ -122,7 +118,6 @@
m_counter(m_echoChannel) {
if (pingChannel == nullptr || echoChannel == nullptr) {
wpi_setWPIError(NullParameter);
- m_nextSensor = nullptr;
m_units = units;
return;
}
@@ -180,26 +175,13 @@
Ultrasonic::~Ultrasonic() {
bool wasAutomaticMode = m_automaticEnabled;
SetAutomaticMode(false);
- wpi_assert(m_firstSensor != nullptr);
- {
- std::lock_guard<priority_mutex> lock(m_mutex);
- if (this == m_firstSensor) {
- m_firstSensor = m_nextSensor;
- if (m_firstSensor == nullptr) {
- SetAutomaticMode(false);
- }
- } else {
- wpi_assert(m_firstSensor->m_nextSensor != nullptr);
- for (Ultrasonic *s = m_firstSensor; s != nullptr; s = s->m_nextSensor) {
- if (this == s->m_nextSensor) {
- s->m_nextSensor = s->m_nextSensor->m_nextSensor;
- break;
- }
- }
- }
+ // No synchronization needed because the background task is stopped.
+ m_sensors.erase(this);
+
+ if (!m_sensors.empty() && wasAutomaticMode) {
+ SetAutomaticMode(true);
}
- if (m_firstSensor != nullptr && wasAutomaticMode) SetAutomaticMode(true);
}
/**
@@ -218,15 +200,15 @@
if (enabling == m_automaticEnabled) return; // ignore the case of no change
m_automaticEnabled = enabling;
+
if (enabling) {
- // enabling automatic mode.
- // Clear all the counters so no data is valid
- for (Ultrasonic *u = m_firstSensor; u != nullptr; u = u->m_nextSensor) {
- u->m_counter.Reset();
+ /* Clear all the counters so no data is valid. No synchronization is needed
+ * because the background task is stopped.
+ */
+ for (auto& sensor : m_sensors) {
+ sensor->m_counter.Reset();
}
- // Start round robin task
- wpi_assert(m_task.Verify() ==
- false); // should be false since was previously disabled
+
m_task = Task("UltrasonicChecker", &Ultrasonic::UltrasonicChecker);
// TODO: Currently, lvuser does not have permissions to set task priorities.
@@ -234,27 +216,25 @@
// Ultrasonic::SetAutomicMode().
//m_task.SetPriority(kPriority);
} else {
- // disabling automatic mode. Wait for background task to stop running.
- while (m_task.Verify())
- Wait(0.15); // just a little longer than the ping time for round-robin to
- // stop
-
- // clear all the counters (data now invalid) since automatic mode is stopped
- for (Ultrasonic *u = m_firstSensor; u != nullptr; u = u->m_nextSensor) {
- u->m_counter.Reset();
- }
- m_automaticEnabled = false;
+ // Wait for background task to stop running
m_task.join();
+
+ /* Clear all the counters (data now invalid) since automatic mode is
+ * disabled. No synchronization is needed because the background task is
+ * stopped.
+ */
+ for (auto& sensor : m_sensors) {
+ sensor->m_counter.Reset();
+ }
}
}
/**
* Single ping to ultrasonic sensor.
* Send out a single ping to the ultrasonic sensor. This only works if automatic
- * (round robin)
- * mode is disabled. A single ping is sent out, and the counter should count the
- * semi-period
- * when it comes in. The counter is reset to make the current value invalid.
+ * (round robin) mode is disabled. A single ping is sent out, and the counter
+ * should count the semi-period when it comes in. The counter is reset to make
+ * the current value invalid.
*/
void Ultrasonic::Ping() {
wpi_assert(!m_automaticEnabled);
@@ -275,9 +255,8 @@
/**
* Get the range in inches from the ultrasonic sensor.
* @return double Range in inches of the target returned from the ultrasonic
- * sensor. If there is
- * no valid value yet, i.e. at least one measurement hasn't completed, then
- * return 0.
+ * sensor. If there is no valid value yet, i.e. at least one measurement hasn't
+ * completed, then return 0.
*/
double Ultrasonic::GetRangeInches() const {
if (IsRangeValid())
@@ -291,7 +270,7 @@
* @return double Range in millimeters of the target returned by the ultrasonic
* sensor.
* If there is no valid value yet, i.e. at least one measurement hasn't
- * complted, then return 0.
+ * completed, then return 0.
*/
double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
diff --git a/wpilibc/Athena/src/Utility.cpp b/wpilibc/Athena/src/Utility.cpp
index 5998f0f..d675e60 100644
--- a/wpilibc/Athena/src/Utility.cpp
+++ b/wpilibc/Athena/src/Utility.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 "Utility.h"
@@ -29,11 +29,13 @@
const char *message, const char *fileName,
uint32_t lineNumber, const char *funcName) {
if (!conditionValue) {
+ std::stringstream locStream;
+ locStream << funcName << " [";
+ locStream << basename(fileName) << ":" << lineNumber << "]";
+
std::stringstream errorStream;
errorStream << "Assertion \"" << conditionText << "\" ";
- errorStream << "on line " << lineNumber << " ";
- errorStream << "of " << basename(fileName) << " ";
if (message[0] != '\0') {
errorStream << "failed: " << message << std::endl;
@@ -41,13 +43,12 @@
errorStream << "failed." << std::endl;
}
- errorStream << GetStackTrace(2);
-
+ std::string stack = GetStackTrace(2);
+ std::string location = locStream.str();
std::string error = errorStream.str();
// Print the error and send it to the DriverStation
- std::cout << error << std::endl;
- HALSetErrorData(error.c_str(), error.size(), 100);
+ HALSendError(1, 1, 0, error.c_str(), location.c_str(), stack.c_str(), 1);
}
return conditionValue;
@@ -65,12 +66,14 @@
const char *fileName,
uint32_t lineNumber,
const char *funcName) {
+ std::stringstream locStream;
+ locStream << funcName << " [";
+ locStream << basename(fileName) << ":" << lineNumber << "]";
+
std::stringstream errorStream;
errorStream << "Assertion \"" << valueA << " " << equalityType << " "
<< valueB << "\" ";
- errorStream << "on line " << lineNumber << " ";
- errorStream << "of " << basename(fileName) << " ";
if (message[0] != '\0') {
errorStream << "failed: " << message << std::endl;
@@ -78,13 +81,12 @@
errorStream << "failed." << std::endl;
}
- errorStream << GetStackTrace(3);
-
+ std::string trace = GetStackTrace(3);
+ std::string location = locStream.str();
std::string error = errorStream.str();
// Print the error and send it to the DriverStation
- std::cout << error << std::endl;
- HALSetErrorData(error.c_str(), error.size(), 100);
+ HALSendError(1, 1, 0, error.c_str(), location.c_str(), trace.c_str(), 1);
}
/**
@@ -156,9 +158,9 @@
* @return The current time in microseconds according to the FPGA (since FPGA
* reset).
*/
-uint32_t GetFPGATime() {
+uint64_t GetFPGATime() {
int32_t status = 0;
- uint32_t time = getFPGATime(&status);
+ uint64_t time = getFPGATime(&status);
wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
return time;
}
diff --git a/wpilibc/Athena/src/Victor.cpp b/wpilibc/Athena/src/Victor.cpp
index f8a4970..81c863c 100644
--- a/wpilibc/Athena/src/Victor.cpp
+++ b/wpilibc/Athena/src/Victor.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 "Victor.h"
@@ -82,3 +82,8 @@
* @param output Write out the PWM value as was found in the PIDController
*/
void Victor::PIDWrite(float output) { Set(output); }
+
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void Victor::StopMotor() { this->SafePWM::StopMotor(); }
\ No newline at end of file
diff --git a/wpilibc/Athena/src/VictorSP.cpp b/wpilibc/Athena/src/VictorSP.cpp
index ccbc163..57dddea 100644
--- a/wpilibc/Athena/src/VictorSP.cpp
+++ b/wpilibc/Athena/src/VictorSP.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 "VictorSP.h"
@@ -86,3 +86,8 @@
* @param output Write out the PWM value as was found in the PIDController
*/
void VictorSP::PIDWrite(float output) { Set(output); }
+
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void VictorSP::StopMotor() { this->SafePWM::StopMotor(); }
\ No newline at end of file
diff --git a/wpilibc/Athena/src/Vision/AxisCamera.cpp b/wpilibc/Athena/src/Vision/AxisCamera.cpp
index 990c68a..39dcf5b 100644
--- a/wpilibc/Athena/src/Vision/AxisCamera.cpp
+++ b/wpilibc/Athena/src/Vision/AxisCamera.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/AxisCamera.h"
diff --git a/wpilibc/Athena/src/Vision/BaeUtilities.cpp b/wpilibc/Athena/src/Vision/BaeUtilities.cpp
index f09ec64..f1b13ee 100644
--- a/wpilibc/Athena/src/Vision/BaeUtilities.cpp
+++ b/wpilibc/Athena/src/Vision/BaeUtilities.cpp
@@ -1,9 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved.
- */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
+
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
diff --git a/wpilibc/Athena/src/Vision/BinaryImage.cpp b/wpilibc/Athena/src/Vision/BinaryImage.cpp
index ea14cbc..4a30a79 100644
--- a/wpilibc/Athena/src/Vision/BinaryImage.cpp
+++ b/wpilibc/Athena/src/Vision/BinaryImage.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved.
- */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/BinaryImage.h"
diff --git a/wpilibc/Athena/src/Vision/ColorImage.cpp b/wpilibc/Athena/src/Vision/ColorImage.cpp
index bbbc242..ab37c0f 100644
--- a/wpilibc/Athena/src/Vision/ColorImage.cpp
+++ b/wpilibc/Athena/src/Vision/ColorImage.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/ColorImage.h"
diff --git a/wpilibc/Athena/src/Vision/FrcError.cpp b/wpilibc/Athena/src/Vision/FrcError.cpp
index f0b4725..31b76be 100644
--- a/wpilibc/Athena/src/Vision/FrcError.cpp
+++ b/wpilibc/Athena/src/Vision/FrcError.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "nivision.h"
diff --git a/wpilibc/Athena/src/Vision/HSLImage.cpp b/wpilibc/Athena/src/Vision/HSLImage.cpp
index 5b114c4..c9cdd11 100644
--- a/wpilibc/Athena/src/Vision/HSLImage.cpp
+++ b/wpilibc/Athena/src/Vision/HSLImage.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/HSLImage.h"
diff --git a/wpilibc/Athena/src/Vision/ImageBase.cpp b/wpilibc/Athena/src/Vision/ImageBase.cpp
index f35234a..9ef0a61 100644
--- a/wpilibc/Athena/src/Vision/ImageBase.cpp
+++ b/wpilibc/Athena/src/Vision/ImageBase.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/ImageBase.h"
diff --git a/wpilibc/Athena/src/Vision/MonoImage.cpp b/wpilibc/Athena/src/Vision/MonoImage.cpp
index 90703c0..469182d 100644
--- a/wpilibc/Athena/src/Vision/MonoImage.cpp
+++ b/wpilibc/Athena/src/Vision/MonoImage.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/MonoImage.h"
diff --git a/wpilibc/Athena/src/Vision/RGBImage.cpp b/wpilibc/Athena/src/Vision/RGBImage.cpp
index 5469122..92a305d 100644
--- a/wpilibc/Athena/src/Vision/RGBImage.cpp
+++ b/wpilibc/Athena/src/Vision/RGBImage.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/RGBImage.h"
diff --git a/wpilibc/Athena/src/Vision/Threshold.cpp b/wpilibc/Athena/src/Vision/Threshold.cpp
index 2e17243..35887c1 100644
--- a/wpilibc/Athena/src/Vision/Threshold.cpp
+++ b/wpilibc/Athena/src/Vision/Threshold.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include "Vision/Threshold.h"
diff --git a/wpilibc/Athena/src/Vision/VisionAPI.cpp b/wpilibc/Athena/src/Vision/VisionAPI.cpp
index 163721d..4d9c7c1 100644
--- a/wpilibc/Athena/src/Vision/VisionAPI.cpp
+++ b/wpilibc/Athena/src/Vision/VisionAPI.cpp
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(WIND_BASE)/WPILib. */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
/*----------------------------------------------------------------------------*/
#include <stdlib.h>
diff --git a/wpilibc/athena.gradle b/wpilibc/athena.gradle
index da49c5d..26bccfc 100644
--- a/wpilibc/athena.gradle
+++ b/wpilibc/athena.gradle
@@ -1,5 +1,7 @@
defineNetworkTablesProperties()
+def ntSourceDir = "$buildDir/ntSources"
+
model {
components {
wpilib_nonshared(NativeLibrarySpec) {
@@ -93,7 +95,6 @@
}
}
-
// Add the hal static and shared libraries as a dependency
project(':hal').tasks.whenTaskAdded { task ->
if (task.name == 'hALAthenaStaticLibrary' || task.name == 'hALAthenaSharedLibrary') {
@@ -101,10 +102,72 @@
}
}
+if (checkDoxygen()) {
+
+ def ntSourcesDependency = project.dependencies.create('edu.wpi.first.wpilib.networktables.cpp:NetworkTables:3.0.0-SNAPSHOT:sources@zip')
+ def ntSourcesConfig = project.configurations.detachedConfiguration(ntSourcesDependency)
+ ntSourcesDependency.setTransitive(false)
+ def ntSources = ntSourcesConfig.singleFile
+
+ task unzipCppNtSources(type: Copy) {
+ description = 'Unzips the C++ networktables sources for doc creation'
+ group = 'WPILib'
+ from zipTree(ntSources)
+ exclude 'META-INF/*'
+ into ntSourceDir
+ }
+
+ doxygen {
+ def halLocation = '../hal'
+ source file("${project.shared}/src")
+ source file("${project.shared}/include")
+ source file("${project.athena}/src")
+ source file("${project.athena}/include")
+ source file("$ntSourceDir/src")
+ source file("$ntSourceDir/include")
+ source file("$halLocation/shared")
+ source file("$halLocation/Athena")
+ source file("$halLocation/include")
+ // template file('cpp.doxy')
+ exclude 'pcre.h'
+ exclude 'nivision.h'
+ project_name 'WPILibC++'
+ javadoc_autobrief true
+ recursive true
+ quiet true
+ warnings false
+ warn_if_doc_error false
+ warn_no_paramdoc false
+ warn_format false
+ warn_logfile false
+ warn_if_undocumented false
+ generate_latex false
+ html_timestamp true
+ generate_treeview true
+ outputDir file("$buildDir/docs")
+ }
+
+ doxygen.dependsOn unzipCppNtSources
+
+ task doxygenZip(type: Zip) {
+ description = 'Generates doxygen zip file for publishing'
+ group = 'WPILib'
+ dependsOn doxygen
+ from doxygen.outputDir
+ }
+}
+
publishing {
publications {
wpilibc(MavenPublication) {
artifact wpilibcZip
+
+ if (checkDoxygen()) {
+ artifact (doxygenZip) {
+ classifier = 'doxygen'
+ }
+ }
+
groupId 'edu.wpi.first.wpilib.cmake'
artifactId 'cpp-root'
version '1.0.0'
@@ -113,3 +176,7 @@
setupWpilibRepo(it)
}
+
+clean {
+ ntSourceDir
+}
diff --git a/wpilibc/build.gradle b/wpilibc/build.gradle
index b535cad..663e0ad 100644
--- a/wpilibc/build.gradle
+++ b/wpilibc/build.gradle
@@ -1,5 +1,8 @@
-apply plugin: 'cpp'
-apply plugin: 'maven-publish'
+plugins {
+ id 'org.ysb33r.doxygen' version '0.2'
+ id 'cpp'
+ id 'maven-publish'
+}
evaluationDependsOn(':hal')
@@ -9,7 +12,7 @@
// Attempts to execute the doxygen command. If there is no exception, doxygen exists, so return true. If there's
// an IOException, it doesn't exist, so return false
-boolean checkDoxygen() {
+ext.checkDoxygen = {
try {
'doxygen'.execute()
true
@@ -19,4 +22,8 @@
}
apply from: 'athena.gradle'
-apply from: 'simulation.gradle'
+
+if (hasProperty('makeSim')){
+ apply from: 'simulation.gradle'
+}
+
diff --git a/wpilibc/shared/include/Base.h b/wpilibc/shared/include/Base.h
index b1b806f..8bb8d4c 100644
--- a/wpilibc/shared/include/Base.h
+++ b/wpilibc/shared/include/Base.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
// MSVC 2013 doesn't allow "= default" on move constructors, but since we are
diff --git a/wpilibc/shared/include/Buttons/Button.h b/wpilibc/shared/include/Buttons/Button.h
index 8897569..3c77886 100644
--- a/wpilibc/shared/include/Buttons/Button.h
+++ b/wpilibc/shared/include/Buttons/Button.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __BUTTON_H__
diff --git a/wpilibc/shared/include/Buttons/ButtonScheduler.h b/wpilibc/shared/include/Buttons/ButtonScheduler.h
index 6222da7..f05274d 100644
--- a/wpilibc/shared/include/Buttons/ButtonScheduler.h
+++ b/wpilibc/shared/include/Buttons/ButtonScheduler.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __BUTTON_SCHEDULER_H__
diff --git a/wpilibc/shared/include/Buttons/CancelButtonScheduler.h b/wpilibc/shared/include/Buttons/CancelButtonScheduler.h
index ba3ba24..1c5d328 100644
--- a/wpilibc/shared/include/Buttons/CancelButtonScheduler.h
+++ b/wpilibc/shared/include/Buttons/CancelButtonScheduler.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __CANCEL_BUTTON_SCHEDULER_H__
diff --git a/wpilibc/shared/include/Buttons/HeldButtonScheduler.h b/wpilibc/shared/include/Buttons/HeldButtonScheduler.h
index 6dcd7a3..0a29105 100644
--- a/wpilibc/shared/include/Buttons/HeldButtonScheduler.h
+++ b/wpilibc/shared/include/Buttons/HeldButtonScheduler.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __HELD_BUTTON_SCHEDULER_H__
diff --git a/wpilibc/shared/include/Buttons/InternalButton.h b/wpilibc/shared/include/Buttons/InternalButton.h
index fdad6f9..6beea7b 100644
--- a/wpilibc/shared/include/Buttons/InternalButton.h
+++ b/wpilibc/shared/include/Buttons/InternalButton.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __INTERNAL_BUTTON_H__
diff --git a/wpilibc/shared/include/Buttons/JoystickButton.h b/wpilibc/shared/include/Buttons/JoystickButton.h
index 028efea..b1163f0 100644
--- a/wpilibc/shared/include/Buttons/JoystickButton.h
+++ b/wpilibc/shared/include/Buttons/JoystickButton.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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_BUTTON_H__
diff --git a/wpilibc/shared/include/Buttons/NetworkButton.h b/wpilibc/shared/include/Buttons/NetworkButton.h
index 9dcba58..b534e00 100644
--- a/wpilibc/shared/include/Buttons/NetworkButton.h
+++ b/wpilibc/shared/include/Buttons/NetworkButton.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __NETWORK_BUTTON_H__
diff --git a/wpilibc/shared/include/Buttons/PressedButtonScheduler.h b/wpilibc/shared/include/Buttons/PressedButtonScheduler.h
index 62ff7a8..7a2e477 100644
--- a/wpilibc/shared/include/Buttons/PressedButtonScheduler.h
+++ b/wpilibc/shared/include/Buttons/PressedButtonScheduler.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __PRESSED_BUTTON_SCHEDULER_H__
diff --git a/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h b/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h
index 2a29981..a9ee3c8 100644
--- a/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h
+++ b/wpilibc/shared/include/Buttons/ReleasedButtonScheduler.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __RELEASED_BUTTON_SCHEDULER_H__
diff --git a/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h b/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h
index d79b456..4c2b5eb 100644
--- a/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h
+++ b/wpilibc/shared/include/Buttons/ToggleButtonScheduler.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __TOGGLE_BUTTON_SCHEDULER_H__
diff --git a/wpilibc/shared/include/Buttons/Trigger.h b/wpilibc/shared/include/Buttons/Trigger.h
index 3a05995..719b072 100644
--- a/wpilibc/shared/include/Buttons/Trigger.h
+++ b/wpilibc/shared/include/Buttons/Trigger.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __TRIGGER_H__
diff --git a/wpilibc/shared/include/CircularBuffer.h b/wpilibc/shared/include/CircularBuffer.h
new file mode 100644
index 0000000..e4c7739
--- /dev/null
+++ b/wpilibc/shared/include/CircularBuffer.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015-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 <vector>
+#include <cstddef>
+
+/**
+ * This is a simple circular buffer so we don't need to "bucket brigade" copy
+ * old values.
+ */
+template <class T>
+class CircularBuffer {
+ public:
+ CircularBuffer(size_t size);
+
+ void PushFront(T value);
+ void PushBack(T value);
+ T PopFront();
+ T PopBack();
+ void Reset();
+
+ T& operator[](size_t index);
+ const T& operator[](size_t index) const;
+
+ private:
+ std::vector<T> m_data;
+
+ // Index of element at front of buffer
+ size_t m_front = 0;
+
+ // Number of elements used in buffer
+ size_t m_length = 0;
+
+ size_t ModuloInc(size_t index);
+ size_t ModuloDec(size_t index);
+};
+
+#include "CircularBuffer.inc"
diff --git a/wpilibc/shared/include/CircularBuffer.inc b/wpilibc/shared/include/CircularBuffer.inc
new file mode 100644
index 0000000..c42dea8
--- /dev/null
+++ b/wpilibc/shared/include/CircularBuffer.inc
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015-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 <algorithm>
+
+template <class T>
+CircularBuffer<T>::CircularBuffer(size_t size) : m_data(size, 0) {}
+
+/**
+ * Push new value onto front of the buffer. The value at the back is overwritten
+ * if the buffer is full.
+ */
+template <class T>
+void CircularBuffer<T>::PushFront(T value) {
+ if (m_data.size() == 0) {
+ return;
+ }
+
+ m_front = ModuloDec(m_front);
+
+ m_data[m_front] = value;
+
+ if (m_length < m_data.size()) {
+ m_length++;
+ }
+}
+
+/**
+ * Push new value onto back of the buffer. The value at the front is overwritten
+ * if the buffer is full.
+ */
+template <class T>
+void CircularBuffer<T>::PushBack(T value) {
+ if (m_data.size() == 0) {
+ return;
+ }
+
+ m_data[(m_front + m_length) % m_data.size()] = value;
+
+ if (m_length < m_data.size()) {
+ m_length++;
+ } else {
+ // Increment front if buffer is full to maintain size
+ m_front = ModuloInc(m_front);
+ }
+}
+
+/**
+ * Pop value at front of buffer.
+ */
+template <class T>
+T CircularBuffer<T>::PopFront() {
+ // If there are no elements in the buffer, do nothing
+ if (m_length == 0) {
+ return 0;
+ }
+
+ T& temp = m_data[m_front];
+ m_front = ModuloInc(m_front);
+ m_length--;
+ return temp;
+}
+
+/**
+ * Pop value at back of buffer.
+ */
+template <class T>
+T CircularBuffer<T>::PopBack() {
+ // If there are no elements in the buffer, do nothing
+ if (m_length == 0) {
+ return 0;
+ }
+
+ m_length--;
+ return m_data[(m_front + m_length) % m_data.size()];
+}
+
+template <class T>
+void CircularBuffer<T>::Reset() {
+ std::fill(m_data.begin(), m_data.end(), 0);
+ m_front = 0;
+ m_length = 0;
+}
+
+/**
+ * Returns element at index starting from front of buffer.
+ */
+template <class T>
+T& CircularBuffer<T>::operator[](size_t index) {
+ return m_data[(m_front + index) % m_data.size()];
+}
+
+/**
+ * Returns element at index starting from front of buffer.
+ */
+template <class T>
+const T& CircularBuffer<T>::operator[](size_t index) const {
+ return m_data[(m_front + index) % m_data.size()];
+}
+
+/**
+ * Increment an index modulo the length of the m_data buffer
+ */
+template <class T>
+size_t CircularBuffer<T>::ModuloInc(size_t index) {
+ return (index + 1) % m_data.size();
+}
+
+/**
+ * Decrement an index modulo the length of the m_data buffer
+ */
+template <class T>
+size_t CircularBuffer<T>::ModuloDec(size_t index) {
+ if (index == 0) {
+ return m_data.size() - 1;
+ } else {
+ return index - 1;
+ }
+}
diff --git a/wpilibc/shared/include/Commands/Command.h b/wpilibc/shared/include/Commands/Command.h
index 6ec5512..47fe7f1 100644
--- a/wpilibc/shared/include/Commands/Command.h
+++ b/wpilibc/shared/include/Commands/Command.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __COMMAND_H__
diff --git a/wpilibc/shared/include/Commands/CommandGroup.h b/wpilibc/shared/include/Commands/CommandGroup.h
index 7f289ca..309d591 100644
--- a/wpilibc/shared/include/Commands/CommandGroup.h
+++ b/wpilibc/shared/include/Commands/CommandGroup.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __COMMAND_GROUP_H__
diff --git a/wpilibc/shared/include/Commands/CommandGroupEntry.h b/wpilibc/shared/include/Commands/CommandGroupEntry.h
index 442a02c..fd9b387 100644
--- a/wpilibc/shared/include/Commands/CommandGroupEntry.h
+++ b/wpilibc/shared/include/Commands/CommandGroupEntry.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __COMMAND_GROUP_ENTRY_H__
diff --git a/wpilibc/shared/include/Commands/PIDCommand.h b/wpilibc/shared/include/Commands/PIDCommand.h
index d67c063..b9fb2ca 100644
--- a/wpilibc/shared/include/Commands/PIDCommand.h
+++ b/wpilibc/shared/include/Commands/PIDCommand.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __PID_COMMAND_H__
diff --git a/wpilibc/shared/include/Commands/PIDSubsystem.h b/wpilibc/shared/include/Commands/PIDSubsystem.h
index ff05514..fea1847 100644
--- a/wpilibc/shared/include/Commands/PIDSubsystem.h
+++ b/wpilibc/shared/include/Commands/PIDSubsystem.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __PID_SUBSYSTEM_H__
diff --git a/wpilibc/shared/include/Commands/PrintCommand.h b/wpilibc/shared/include/Commands/PrintCommand.h
index cbb3e5e..577252d 100644
--- a/wpilibc/shared/include/Commands/PrintCommand.h
+++ b/wpilibc/shared/include/Commands/PrintCommand.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __PRINT_COMMAND_H__
diff --git a/wpilibc/shared/include/Commands/Scheduler.h b/wpilibc/shared/include/Commands/Scheduler.h
index 065fbad..fc55256 100644
--- a/wpilibc/shared/include/Commands/Scheduler.h
+++ b/wpilibc/shared/include/Commands/Scheduler.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __SCHEDULER_H__
diff --git a/wpilibc/shared/include/Commands/StartCommand.h b/wpilibc/shared/include/Commands/StartCommand.h
index 8f53d14..4f97971 100644
--- a/wpilibc/shared/include/Commands/StartCommand.h
+++ b/wpilibc/shared/include/Commands/StartCommand.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __START_COMMAND_H__
diff --git a/wpilibc/shared/include/Commands/Subsystem.h b/wpilibc/shared/include/Commands/Subsystem.h
index 8762ec8..070af48 100644
--- a/wpilibc/shared/include/Commands/Subsystem.h
+++ b/wpilibc/shared/include/Commands/Subsystem.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __SUBSYSTEM_H__
diff --git a/wpilibc/shared/include/Commands/WaitCommand.h b/wpilibc/shared/include/Commands/WaitCommand.h
index 6db6dac..fa64b75 100644
--- a/wpilibc/shared/include/Commands/WaitCommand.h
+++ b/wpilibc/shared/include/Commands/WaitCommand.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __WAIT_COMMAND_H__
diff --git a/wpilibc/shared/include/Commands/WaitForChildren.h b/wpilibc/shared/include/Commands/WaitForChildren.h
index 858d243..5028cdb 100644
--- a/wpilibc/shared/include/Commands/WaitForChildren.h
+++ b/wpilibc/shared/include/Commands/WaitForChildren.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __WAIT_FOR_CHILDREN_H__
diff --git a/wpilibc/shared/include/Commands/WaitUntilCommand.h b/wpilibc/shared/include/Commands/WaitUntilCommand.h
index fd77f8e..2512a20 100644
--- a/wpilibc/shared/include/Commands/WaitUntilCommand.h
+++ b/wpilibc/shared/include/Commands/WaitUntilCommand.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __WAIT_UNTIL_COMMAND_H__
diff --git a/wpilibc/shared/include/Controller.h b/wpilibc/shared/include/Controller.h
index d852307..bad16d6 100644
--- a/wpilibc/shared/include/Controller.h
+++ b/wpilibc/shared/include/Controller.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/shared/include/Error.h b/wpilibc/shared/include/Error.h
index f9e301d..a001145 100644
--- a/wpilibc/shared/include/Error.h
+++ b/wpilibc/shared/include/Error.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 "Base.h"
diff --git a/wpilibc/shared/include/ErrorBase.h b/wpilibc/shared/include/ErrorBase.h
index 4730a29..756442d 100644
--- a/wpilibc/shared/include/ErrorBase.h
+++ b/wpilibc/shared/include/ErrorBase.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 "Base.h"
diff --git a/wpilibc/shared/include/Filters/Filter.h b/wpilibc/shared/include/Filters/Filter.h
new file mode 100644
index 0000000..1ab193b
--- /dev/null
+++ b/wpilibc/shared/include/Filters/Filter.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015-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 <memory>
+#include "PIDSource.h"
+
+/**
+ * Interface for filters
+ */
+class Filter : public PIDSource {
+ public:
+ Filter(std::shared_ptr<PIDSource> source);
+ virtual ~Filter() = default;
+
+ // PIDSource interface
+ virtual void SetPIDSourceType(PIDSourceType pidSource) override;
+ PIDSourceType GetPIDSourceType() const;
+ virtual double PIDGet() override = 0;
+
+ /**
+ * Returns the current filter estimate without also inserting new data as
+ * PIDGet() would do.
+ *
+ * @return The current filter estimate
+ */
+ virtual double Get() const = 0;
+
+ /**
+ * Reset the filter state
+ */
+ virtual void Reset() = 0;
+
+ protected:
+ /**
+ * Calls PIDGet() of source
+ *
+ * @return Current value of source
+ */
+ double PIDGetSource();
+
+ private:
+ std::shared_ptr<PIDSource> m_source;
+};
diff --git a/wpilibc/shared/include/Filters/LinearDigitalFilter.h b/wpilibc/shared/include/Filters/LinearDigitalFilter.h
new file mode 100644
index 0000000..b6dbce7
--- /dev/null
+++ b/wpilibc/shared/include/Filters/LinearDigitalFilter.h
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015-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 <initializer_list>
+#include <memory>
+#include <vector>
+#include "Filter.h"
+#include "CircularBuffer.h"
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR
+ * filters are supported. Static factory methods are provided to create commonly
+ * used types of filters.
+ *
+ * Filters are of the form:
+ * y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P) - (a0*y[n-1] + a2*y[n-2] + ... + aQ*y[n-Q])
+ *
+ * Where:
+ * y[n] is the output at time "n"
+ * x[n] is the input at time "n"
+ * y[n-1] is the output from the LAST time step ("n-1")
+ * x[n-1] is the input from the LAST time step ("n-1")
+ * b0...bP are the "feedforward" (FIR) gains
+ * a0...aQ are the "feedback" (IIR) gains
+ * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
+ * convention in signal processing.
+ *
+ * What can linear filters do? Basically, they can filter, or diminish, the
+ * effects of undesirable input frequencies. High frequencies, or rapid changes,
+ * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
+ * filter smooths out the signal, reducing the impact of these high frequency
+ * components. Likewise, a "high pass" filter gets rid of slow-moving signal
+ * components, letting you detect large changes more easily.
+ *
+ * Example FRC applications of filters:
+ * - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
+ * can do this faster in hardware)
+ * - Smoothing out joystick input to prevent the wheels from slipping or the
+ * robot from tipping
+ * - Smoothing motor commands so that unnecessary strain isn't put on
+ * electrical or mechanical components
+ * - If you use clever gains, you can make a PID controller out of this class!
+ *
+ * For more on filters, I highly recommend the following articles:
+ * http://en.wikipedia.org/wiki/Linear_filter
+ * http://en.wikipedia.org/wiki/Iir_filter
+ * http://en.wikipedia.org/wiki/Fir_filter
+ *
+ * Note 1: PIDGet() should be called by the user on a known, regular period.
+ * You can set up a Notifier to do this (look at the WPILib PIDController
+ * class), or do it "inline" with code in a periodic function.
+ *
+ * Note 2: For ALL filters, gains are necessarily a function of frequency. If
+ * you make a filter that works well for you at, say, 100Hz, you will most
+ * definitely need to adjust the gains if you then want to run it at 200Hz!
+ * Combining this with Note 1 - the impetus is on YOU as a developer to make
+ * sure PIDGet() gets called at the desired, constant frequency!
+ */
+class LinearDigitalFilter : public Filter {
+ public:
+ LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains);
+ LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ std::initializer_list<double> ffGains,
+ const std::vector<double>& fbGains);
+ LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ const std::vector<double>& ffGains,
+ std::initializer_list<double> fbGains);
+ LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ const std::vector<double>& ffGains,
+ const std::vector<double>& fbGains);
+
+ // Static methods to create commonly used filters
+ static LinearDigitalFilter SinglePoleIIR(std::shared_ptr<PIDSource> source,
+ double timeConstant, double period);
+ static LinearDigitalFilter HighPass(std::shared_ptr<PIDSource> source,
+ double timeConstant, double period);
+ static LinearDigitalFilter MovingAverage(std::shared_ptr<PIDSource> source,
+ unsigned int taps);
+
+ // Filter interface
+ double Get() const override;
+ void Reset() override;
+
+ // PIDSource interface
+ double PIDGet() override;
+
+ private:
+ CircularBuffer<double> m_inputs;
+ CircularBuffer<double> m_outputs;
+ std::vector<double> m_inputGains;
+ std::vector<double> m_outputGains;
+};
diff --git a/wpilibc/shared/include/GenericHID.h b/wpilibc/shared/include/GenericHID.h
index bf1fe8e..17a959f 100644
--- a/wpilibc/shared/include/GenericHID.h
+++ b/wpilibc/shared/include/GenericHID.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 <stdint.h>
diff --git a/wpilibc/Athena/include/GyroBase.h b/wpilibc/shared/include/GyroBase.h
similarity index 80%
rename from wpilibc/Athena/include/GyroBase.h
rename to wpilibc/shared/include/GyroBase.h
index 6ed44aa..5d93fc4 100644
--- a/wpilibc/Athena/include/GyroBase.h
+++ b/wpilibc/shared/include/GyroBase.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 "SensorBase.h"
diff --git a/wpilibc/shared/include/HLUsageReporting.h b/wpilibc/shared/include/HLUsageReporting.h
index 5d8d34f..0da2b5c 100644
--- a/wpilibc/shared/include/HLUsageReporting.h
+++ b/wpilibc/shared/include/HLUsageReporting.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
class HLUsageReportingInterface {
diff --git a/wpilibc/shared/include/LiveWindow/LiveWindow.h b/wpilibc/shared/include/LiveWindow/LiveWindow.h
index 7b40f67..17817a9 100644
--- a/wpilibc/shared/include/LiveWindow/LiveWindow.h
+++ b/wpilibc/shared/include/LiveWindow/LiveWindow.h
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2012-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 _LIVE_WINDOW_H
#define _LIVE_WINDOW_H
diff --git a/wpilibc/shared/include/LiveWindow/LiveWindowSendable.h b/wpilibc/shared/include/LiveWindow/LiveWindowSendable.h
index 4f2023a..16debcc 100644
--- a/wpilibc/shared/include/LiveWindow/LiveWindowSendable.h
+++ b/wpilibc/shared/include/LiveWindow/LiveWindowSendable.h
@@ -1,7 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) Patrick Plenefisch 2012. All Rights Reserved. */
+/* Copyright (c) FIRST 2012-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 LIVEWINDOWSENDABLE_H_
diff --git a/wpilibc/shared/include/LiveWindow/LiveWindowStatusListener.h b/wpilibc/shared/include/LiveWindow/LiveWindowStatusListener.h
index 4fa3eb7..88b373b 100644
--- a/wpilibc/shared/include/LiveWindow/LiveWindowStatusListener.h
+++ b/wpilibc/shared/include/LiveWindow/LiveWindowStatusListener.h
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2012-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 _LIVE_WINDOW_STATUS_LISTENER_H
#define _LIVE_WINDOW_STATUS_LISTENER_H
diff --git a/wpilibc/shared/include/Notifier.h b/wpilibc/shared/include/Notifier.h
deleted file mode 100644
index 056ed0a..0000000
--- a/wpilibc/shared/include/Notifier.h
+++ /dev/null
@@ -1,57 +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 "ErrorBase.h"
-#include "HAL/cpp/priority_mutex.h"
-#include <thread>
-#include <atomic>
-
-typedef void (*TimerEventHandler)(void *param);
-
-class Notifier : public ErrorBase {
- public:
- Notifier(TimerEventHandler handler, void *param = nullptr);
- virtual ~Notifier();
-
- Notifier(const Notifier&) = delete;
- Notifier& operator=(const Notifier&) = delete;
-
- void StartSingle(double delay);
- void StartPeriodic(double period);
- void Stop();
-
- private:
- static Notifier *timerQueueHead;
- static priority_recursive_mutex queueMutex;
- static priority_mutex halMutex;
- static void *m_notifier;
- static std::atomic<int> refcount;
-
- static void ProcessQueue(
- uint32_t mask, void *params); // process the timer queue on a timer event
- static void
- UpdateAlarm(); // update the FPGA alarm since the queue has changed
- void InsertInQueue(
- bool reschedule); // insert this Notifier in the timer queue
- void DeleteFromQueue(); // delete this Notifier from the timer queue
- TimerEventHandler m_handler; // address of the handler
- void *m_param; // a parameter to pass to the handler
- double m_period = 0; // the relative time (either periodic or single)
- double m_expirationTime = 0; // absolute expiration time for the current event
- Notifier *m_nextEvent = nullptr; // next Nofifier event
- bool m_periodic = false; // true if this is a periodic event
- bool m_queued = false; // indicates if this entry is queued
- priority_mutex m_handlerMutex; // held by interrupt manager task while
- // handler call is in progress
-
-#ifdef FRC_SIMULATOR
- static std::thread m_task;
- static std::atomic<bool> m_stopped;
-#endif
- static void Run();
-};
diff --git a/wpilibc/shared/include/PIDController.h b/wpilibc/shared/include/PIDController.h
index 301356c..5e74389 100644
--- a/wpilibc/shared/include/PIDController.h
+++ b/wpilibc/shared/include/PIDController.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 "Base.h"
@@ -13,6 +14,7 @@
#include "PIDSource.h"
#include "Notifier.h"
#include "HAL/cpp/priority_mutex.h"
+#include "Timer.h"
#include <memory>
@@ -54,6 +56,7 @@
virtual void SetSetpoint(float setpoint) override;
virtual double GetSetpoint() const override;
+ double GetDeltaSetpoint() const;
virtual float GetError() const;
virtual float GetAvgError() const;
@@ -81,6 +84,7 @@
std::shared_ptr<ITable> m_table;
virtual void Calculate();
+ virtual double CalculateFeedForward();
private:
float m_P; // factor for "proportional" control
@@ -93,7 +97,7 @@
float m_minimumInput = 0; // minimum input - limit setpoint to this
bool m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder
bool m_enabled = false; // is the pid controller enabled
- float m_prevInput = 0; // the prior sensor input (used to compute velocity)
+ float m_prevError = 0; // the prior error (used to compute velocity)
double m_totalError = 0; // the sum of the errors for use in the integral calc
enum {
kAbsoluteTolerance,
@@ -104,6 +108,7 @@
// the percetage or absolute error that is considered on target.
float m_tolerance = 0.05;
float m_setpoint = 0;
+ float m_prevSetpoint = 0;
float m_error = 0;
float m_result = 0;
float m_period;
@@ -113,13 +118,13 @@
std::queue<double> m_buf;
double m_bufTotal = 0;
- mutable priority_mutex m_mutex;
+ mutable priority_recursive_mutex m_mutex;
std::unique_ptr<Notifier> m_controlLoop;
+ Timer m_setpointTimer;
void Initialize(float p, float i, float d, float f, PIDSource *source,
PIDOutput *output, float period = 0.05);
- static void CallCalculate(void *controller);
virtual std::shared_ptr<ITable> GetTable() const override;
virtual std::string GetSmartDashboardType() const override;
diff --git a/wpilibc/shared/include/PIDInterface.h b/wpilibc/shared/include/PIDInterface.h
index 5d50199..efcc184 100644
--- a/wpilibc/shared/include/PIDInterface.h
+++ b/wpilibc/shared/include/PIDInterface.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 "Base.h"
diff --git a/wpilibc/shared/include/PIDOutput.h b/wpilibc/shared/include/PIDOutput.h
index 40421d0..ad720dc 100644
--- a/wpilibc/shared/include/PIDOutput.h
+++ b/wpilibc/shared/include/PIDOutput.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 "Base.h"
diff --git a/wpilibc/shared/include/PIDSource.h b/wpilibc/shared/include/PIDSource.h
index 8f46ea4..1a2be9c 100644
--- a/wpilibc/shared/include/PIDSource.h
+++ b/wpilibc/shared/include/PIDSource.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
enum class PIDSourceType { kDisplacement, kRate };
diff --git a/wpilibc/shared/include/Resource.h b/wpilibc/shared/include/Resource.h
index f7061c8..8d26442 100644
--- a/wpilibc/shared/include/Resource.h
+++ b/wpilibc/shared/include/Resource.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/shared/include/RobotState.h b/wpilibc/shared/include/RobotState.h
index 53bf32a..ce48ca0 100644
--- a/wpilibc/shared/include/RobotState.h
+++ b/wpilibc/shared/include/RobotState.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 <memory>
diff --git a/wpilibc/shared/include/SensorBase.h b/wpilibc/shared/include/SensorBase.h
index 8638b5e..51fd94c 100644
--- a/wpilibc/shared/include/SensorBase.h
+++ b/wpilibc/shared/include/SensorBase.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/shared/include/SmartDashboard/Sendable.h b/wpilibc/shared/include/SmartDashboard/Sendable.h
index 613c7f8..78206d0 100644
--- a/wpilibc/shared/include/SmartDashboard/Sendable.h
+++ b/wpilibc/shared/include/SmartDashboard/Sendable.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __SMART_DASHBOARD_DATA__
diff --git a/wpilibc/shared/include/SmartDashboard/SendableChooser.h b/wpilibc/shared/include/SmartDashboard/SendableChooser.h
index bfdf877..9560746 100644
--- a/wpilibc/shared/include/SmartDashboard/SendableChooser.h
+++ b/wpilibc/shared/include/SmartDashboard/SendableChooser.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __SENDABLE_CHOOSER_H__
diff --git a/wpilibc/shared/include/SmartDashboard/SmartDashboard.h b/wpilibc/shared/include/SmartDashboard/SmartDashboard.h
index a1cfb22..51e51d1 100644
--- a/wpilibc/shared/include/SmartDashboard/SmartDashboard.h
+++ b/wpilibc/shared/include/SmartDashboard/SmartDashboard.h
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 __SMART_DASHBOARD_H__
diff --git a/wpilibc/shared/include/Task.h b/wpilibc/shared/include/Task.h
index 7752d0d..2735f48 100644
--- a/wpilibc/shared/include/Task.h
+++ b/wpilibc/shared/include/Task.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/shared/include/Task.inc b/wpilibc/shared/include/Task.inc
index 3514b92..3c90aba 100644
--- a/wpilibc/shared/include/Task.inc
+++ b/wpilibc/shared/include/Task.inc
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
#include "HAL/HAL.hpp"
#include <atomic>
diff --git a/wpilibc/shared/include/Timer.h b/wpilibc/shared/include/Timer.h
index 60b9604..b3fe77d 100644
--- a/wpilibc/shared/include/Timer.h
+++ b/wpilibc/shared/include/Timer.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 "Base.h"
diff --git a/wpilibc/shared/include/Utility.h b/wpilibc/shared/include/Utility.h
index 69edb63..89b9729 100644
--- a/wpilibc/shared/include/Utility.h
+++ b/wpilibc/shared/include/Utility.h
@@ -1,9 +1,10 @@
-/*---------------------------------------------------------------------------*/
-/* 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. */
-/*---------------------------------------------------------------------------*/
+/*----------------------------------------------------------------------------*/
+/* 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
/** @file
@@ -46,6 +47,6 @@
uint16_t GetFPGAVersion();
uint32_t GetFPGARevision();
-uint32_t GetFPGATime();
+uint64_t GetFPGATime();
bool GetUserButton();
std::string GetStackTrace(uint32_t offset);
diff --git a/wpilibc/shared/include/WPIErrors.h b/wpilibc/shared/include/WPIErrors.h
index 8346fdf..ccf6dae 100644
--- a/wpilibc/shared/include/WPIErrors.h
+++ b/wpilibc/shared/include/WPIErrors.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 "stdint.h"
diff --git a/wpilibc/shared/include/interfaces/Accelerometer.h b/wpilibc/shared/include/interfaces/Accelerometer.h
index 528d120..12296ca 100644
--- a/wpilibc/shared/include/interfaces/Accelerometer.h
+++ b/wpilibc/shared/include/interfaces/Accelerometer.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/shared/include/interfaces/Gyro.h b/wpilibc/shared/include/interfaces/Gyro.h
index 8317ca5..6766444 100644
--- a/wpilibc/shared/include/interfaces/Gyro.h
+++ b/wpilibc/shared/include/interfaces/Gyro.h
@@ -1,8 +1,10 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* 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 $(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/shared/include/interfaces/Potentiometer.h b/wpilibc/shared/include/interfaces/Potentiometer.h
index d52892d..1c2e99a 100644
--- a/wpilibc/shared/include/interfaces/Potentiometer.h
+++ b/wpilibc/shared/include/interfaces/Potentiometer.h
@@ -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. */
/*----------------------------------------------------------------------------*/
#ifndef INTERFACES_POTENTIOMETER_H
diff --git a/wpilibc/shared/src/Buttons/Button.cpp b/wpilibc/shared/src/Buttons/Button.cpp
index b57fd89..c4bd5ec 100644
--- a/wpilibc/shared/src/Buttons/Button.cpp
+++ b/wpilibc/shared/src/Buttons/Button.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/Button.h"
diff --git a/wpilibc/shared/src/Buttons/ButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ButtonScheduler.cpp
index ddc9025..318fd5f 100644
--- a/wpilibc/shared/src/Buttons/ButtonScheduler.cpp
+++ b/wpilibc/shared/src/Buttons/ButtonScheduler.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/ButtonScheduler.h"
diff --git a/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp b/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp
index 1d2b6cc..52544e5 100644
--- a/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp
+++ b/wpilibc/shared/src/Buttons/CancelButtonScheduler.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/CancelButtonScheduler.h"
diff --git a/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp b/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp
index 04c8e3b..37de385 100644
--- a/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp
+++ b/wpilibc/shared/src/Buttons/HeldButtonScheduler.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/HeldButtonScheduler.h"
diff --git a/wpilibc/shared/src/Buttons/InternalButton.cpp b/wpilibc/shared/src/Buttons/InternalButton.cpp
index 8c9b22a..9e0e121 100644
--- a/wpilibc/shared/src/Buttons/InternalButton.cpp
+++ b/wpilibc/shared/src/Buttons/InternalButton.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/InternalButton.h"
diff --git a/wpilibc/shared/src/Buttons/JoystickButton.cpp b/wpilibc/shared/src/Buttons/JoystickButton.cpp
index c2bdf62..a7a31e6 100644
--- a/wpilibc/shared/src/Buttons/JoystickButton.cpp
+++ b/wpilibc/shared/src/Buttons/JoystickButton.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/JoystickButton.h"
diff --git a/wpilibc/shared/src/Buttons/NetworkButton.cpp b/wpilibc/shared/src/Buttons/NetworkButton.cpp
index 858703c..54604ed 100644
--- a/wpilibc/shared/src/Buttons/NetworkButton.cpp
+++ b/wpilibc/shared/src/Buttons/NetworkButton.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/NetworkButton.h"
diff --git a/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp b/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp
index 5a0c506..60792dc 100644
--- a/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp
+++ b/wpilibc/shared/src/Buttons/PressedButtonScheduler.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/PressedButtonScheduler.h"
diff --git a/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp
index e7131e6..dde271e 100644
--- a/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp
+++ b/wpilibc/shared/src/Buttons/ReleasedButtonScheduler.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/ReleasedButtonScheduler.h"
diff --git a/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp b/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp
index c4048d2..dc2f058 100644
--- a/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp
+++ b/wpilibc/shared/src/Buttons/ToggleButtonScheduler.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/ToggleButtonScheduler.h"
diff --git a/wpilibc/shared/src/Buttons/Trigger.cpp b/wpilibc/shared/src/Buttons/Trigger.cpp
index 54a1eba..3f15b7e 100644
--- a/wpilibc/shared/src/Buttons/Trigger.cpp
+++ b/wpilibc/shared/src/Buttons/Trigger.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Buttons/Button.h"
diff --git a/wpilibc/shared/src/Commands/Command.cpp b/wpilibc/shared/src/Commands/Command.cpp
index 37effbe..f965712 100644
--- a/wpilibc/shared/src/Commands/Command.cpp
+++ b/wpilibc/shared/src/Commands/Command.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/Command.h"
diff --git a/wpilibc/shared/src/Commands/CommandGroup.cpp b/wpilibc/shared/src/Commands/CommandGroup.cpp
index edb5ec9..91cf788 100644
--- a/wpilibc/shared/src/Commands/CommandGroup.cpp
+++ b/wpilibc/shared/src/Commands/CommandGroup.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/CommandGroup.h"
diff --git a/wpilibc/shared/src/Commands/CommandGroupEntry.cpp b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
index c9f3fa4..0534a71 100644
--- a/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
+++ b/wpilibc/shared/src/Commands/CommandGroupEntry.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/CommandGroupEntry.h"
diff --git a/wpilibc/shared/src/Commands/PIDCommand.cpp b/wpilibc/shared/src/Commands/PIDCommand.cpp
index c6ac919..b6f426f 100644
--- a/wpilibc/shared/src/Commands/PIDCommand.cpp
+++ b/wpilibc/shared/src/Commands/PIDCommand.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/PIDCommand.h"
diff --git a/wpilibc/shared/src/Commands/PIDSubsystem.cpp b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
index b3d327f..e478073 100644
--- a/wpilibc/shared/src/Commands/PIDSubsystem.cpp
+++ b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/PIDSubsystem.h"
diff --git a/wpilibc/shared/src/Commands/PrintCommand.cpp b/wpilibc/shared/src/Commands/PrintCommand.cpp
index b5e8dc7..1d94694 100644
--- a/wpilibc/shared/src/Commands/PrintCommand.cpp
+++ b/wpilibc/shared/src/Commands/PrintCommand.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/PrintCommand.h"
diff --git a/wpilibc/shared/src/Commands/Scheduler.cpp b/wpilibc/shared/src/Commands/Scheduler.cpp
index 0b796e3..955b337 100644
--- a/wpilibc/shared/src/Commands/Scheduler.cpp
+++ b/wpilibc/shared/src/Commands/Scheduler.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/Scheduler.h"
diff --git a/wpilibc/shared/src/Commands/StartCommand.cpp b/wpilibc/shared/src/Commands/StartCommand.cpp
index 60db054..ca5f67b 100644
--- a/wpilibc/shared/src/Commands/StartCommand.cpp
+++ b/wpilibc/shared/src/Commands/StartCommand.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/StartCommand.h"
diff --git a/wpilibc/shared/src/Commands/Subsystem.cpp b/wpilibc/shared/src/Commands/Subsystem.cpp
index 0b4b92f..7e3ef61 100644
--- a/wpilibc/shared/src/Commands/Subsystem.cpp
+++ b/wpilibc/shared/src/Commands/Subsystem.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/Subsystem.h"
diff --git a/wpilibc/shared/src/Commands/WaitCommand.cpp b/wpilibc/shared/src/Commands/WaitCommand.cpp
index f4e4c00..8c5be65 100644
--- a/wpilibc/shared/src/Commands/WaitCommand.cpp
+++ b/wpilibc/shared/src/Commands/WaitCommand.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/WaitCommand.h"
diff --git a/wpilibc/shared/src/Commands/WaitForChildren.cpp b/wpilibc/shared/src/Commands/WaitForChildren.cpp
index a3a129d..e08dfe0 100644
--- a/wpilibc/shared/src/Commands/WaitForChildren.cpp
+++ b/wpilibc/shared/src/Commands/WaitForChildren.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/WaitForChildren.h"
diff --git a/wpilibc/shared/src/Commands/WaitUntilCommand.cpp b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
index ea0e664..17d6f3e 100644
--- a/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
+++ b/wpilibc/shared/src/Commands/WaitUntilCommand.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "Commands/WaitUntilCommand.h"
diff --git a/wpilibc/shared/src/Error.cpp b/wpilibc/shared/src/Error.cpp
index 1206361..5237a92 100644
--- a/wpilibc/shared/src/Error.cpp
+++ b/wpilibc/shared/src/Error.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 "Error.h"
@@ -66,24 +66,21 @@
}
void Error::Report() {
- std::stringstream errorStream;
+ std::stringstream locStream;
+ locStream << m_function << " [";
- errorStream << "Error on line " << m_lineNumber << " ";
-#if defined(_UNIX)
- errorStream << "of " << basename(m_filename.c_str()) << ": ";
-#elif defined(_WIN32)
+#if defined(_WIN32)
const int MAX_DIR = 100;
char basename[MAX_DIR];
_splitpath_s(m_filename.c_str(), NULL, 0, basename, MAX_DIR, NULL, 0, NULL, 0);
- errorStream << "of " << basename << ": ";
+ locStream << basename;
+#else
+ locStream << basename(m_filename.c_str());
#endif
+ locStream << ":" << m_lineNumber << "]";
- errorStream << m_message << std::endl;
- errorStream << GetStackTrace(4);
-
- std::string error = errorStream.str();
-
- DriverStation::ReportError(error);
+ DriverStation::ReportError(true, m_code, m_message, locStream.str(),
+ GetStackTrace(4));
}
void Error::Clear() {
diff --git a/wpilibc/shared/src/ErrorBase.cpp b/wpilibc/shared/src/ErrorBase.cpp
index 329c983..c1e41d8 100644
--- a/wpilibc/shared/src/ErrorBase.cpp
+++ b/wpilibc/shared/src/ErrorBase.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 "ErrorBase.h"
diff --git a/wpilibc/shared/src/Filters/Filter.cpp b/wpilibc/shared/src/Filters/Filter.cpp
new file mode 100644
index 0000000..d6042d7
--- /dev/null
+++ b/wpilibc/shared/src/Filters/Filter.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015-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 "Filters/Filter.h"
+
+Filter::Filter(std::shared_ptr<PIDSource> source) {
+ m_source = source;
+}
+
+void Filter::SetPIDSourceType(PIDSourceType pidSource) {
+ m_source->SetPIDSourceType(pidSource);
+}
+
+PIDSourceType Filter::GetPIDSourceType() const {
+ return m_source->GetPIDSourceType();
+}
+
+double Filter::PIDGetSource() {
+ return m_source->PIDGet();
+}
diff --git a/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp b/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp
new file mode 100644
index 0000000..e4a5cff
--- /dev/null
+++ b/wpilibc/shared/src/Filters/LinearDigitalFilter.cpp
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015-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 "Filters/LinearDigitalFilter.h"
+#include <cassert>
+#include <cmath>
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains) :
+ Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+ m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ std::initializer_list<double> ffGains,
+ const std::vector<double>& fbGains) :
+ Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+ m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ const std::vector<double>& ffGains,
+ std::initializer_list<double> fbGains) :
+ Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+ m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Create a linear FIR or IIR filter
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ const std::vector<double>& ffGains,
+ const std::vector<double>& fbGains) :
+ Filter(source), m_inputs(ffGains.size()), m_outputs(fbGains.size()),
+ m_inputGains(ffGains), m_outputGains(fbGains) {}
+
+/**
+ * Creates a one-pole IIR low-pass filter of the form:
+ * y[n] = (1-gain)*x[n] + gain*y[n-1]
+ * where gain = e^(-dt / T), T is the time constant in seconds
+ *
+ * This filter is stable for time constants greater than zero
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(std::shared_ptr<PIDSource> source,
+ double timeConstant,
+ double period) {
+ double gain = std::exp(-period / timeConstant);
+ return LinearDigitalFilter(std::move(source), {1.0 - gain}, {-gain});
+}
+
+/**
+ * Creates a first-order high-pass filter of the form:
+ * y[n] = gain*x[n] + (-gain)*x[n-1] + gain*y[n-1]
+ * where gain = e^(-dt / T), T is the time constant in seconds
+ *
+ * This filter is stable for time constants greater than zero
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param timeConstant The discrete-time time constant in seconds
+ * @param period The period in seconds between samples taken by the user
+ */
+LinearDigitalFilter LinearDigitalFilter::HighPass(std::shared_ptr<PIDSource> source,
+ double timeConstant,
+ double period) {
+ double gain = std::exp(-period / timeConstant);
+ return LinearDigitalFilter(std::move(source), {gain, -gain}, {-gain});
+}
+
+/**
+ * Creates a K-tap FIR moving average filter of the form:
+ * y[n] = 1/k * (x[k] + x[k-1] + ... + x[0])
+ *
+ * This filter is always stable.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param taps The number of samples to average over. Higher = smoother but
+ * slower
+ */
+LinearDigitalFilter LinearDigitalFilter::MovingAverage(std::shared_ptr<PIDSource> source,
+ unsigned int taps) {
+ assert(taps > 0);
+
+ std::vector<double> gains(taps, 1.0 / taps);
+ return LinearDigitalFilter(std::move(source), gains, {});
+}
+
+/**
+ * {@inheritDoc}
+ */
+double LinearDigitalFilter::Get() const {
+ double retVal = 0.0;
+
+ // Calculate the new value
+ for (unsigned int i = 0; i < m_inputGains.size(); i++) {
+ retVal += m_inputs[i] * m_inputGains[i];
+ }
+ for (unsigned int i = 0; i < m_outputGains.size(); i++) {
+ retVal -= m_outputs[i] * m_outputGains[i];
+ }
+
+ return retVal;
+}
+
+/**
+ * {@inheritDoc}
+ */
+void LinearDigitalFilter::Reset() {
+ m_inputs.Reset();
+ m_outputs.Reset();
+}
+
+/**
+ * Calculates the next value of the filter
+ *
+ * @return The filtered value at this step
+ */
+double LinearDigitalFilter::PIDGet() {
+ double retVal = 0.0;
+
+ // Rotate the inputs
+ m_inputs.PushFront(PIDGetSource());
+
+ // Calculate the new value
+ for (unsigned int i = 0; i < m_inputGains.size(); i++) {
+ retVal += m_inputs[i] * m_inputGains[i];
+ }
+ for (unsigned int i = 0; i < m_outputGains.size(); i++) {
+ retVal -= m_outputs[i] * m_outputGains[i];
+ }
+
+ // Rotate the outputs
+ m_outputs.PushFront(retVal);
+
+ return retVal;
+}
diff --git a/wpilibc/Athena/src/GyroBase.cpp b/wpilibc/shared/src/GyroBase.cpp
similarity index 82%
rename from wpilibc/Athena/src/GyroBase.cpp
rename to wpilibc/shared/src/GyroBase.cpp
index 4cc9056..f1044b7 100644
--- a/wpilibc/Athena/src/GyroBase.cpp
+++ b/wpilibc/shared/src/GyroBase.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 "GyroBase.h"
diff --git a/wpilibc/shared/src/HLUsageReporting.cpp b/wpilibc/shared/src/HLUsageReporting.cpp
index bcbc6b6..07cf446 100644
--- a/wpilibc/shared/src/HLUsageReporting.cpp
+++ b/wpilibc/shared/src/HLUsageReporting.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 "HLUsageReporting.h"
diff --git a/wpilibc/shared/src/LiveWindow/LiveWindow.cpp b/wpilibc/shared/src/LiveWindow/LiveWindow.cpp
index 793ddcc..1dfd507 100644
--- a/wpilibc/shared/src/LiveWindow/LiveWindow.cpp
+++ b/wpilibc/shared/src/LiveWindow/LiveWindow.cpp
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2012-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 "LiveWindow/LiveWindow.h"
#include "networktables/NetworkTable.h"
#include <algorithm>
diff --git a/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp b/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp
index 021798d..31e5db6 100644
--- a/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp
+++ b/wpilibc/shared/src/LiveWindow/LiveWindowStatusListener.cpp
@@ -1,3 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2012-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 "LiveWindow/LiveWindowStatusListener.h"
#include "Commands/Scheduler.h"
diff --git a/wpilibc/shared/src/PIDSource.cpp b/wpilibc/shared/src/PIDSource.cpp
index b6b5200..4e44b69 100644
--- a/wpilibc/shared/src/PIDSource.cpp
+++ b/wpilibc/shared/src/PIDSource.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 "PIDSource.h"
diff --git a/wpilibc/shared/src/Resource.cpp b/wpilibc/shared/src/Resource.cpp
index 7f9e476..a68122e 100644
--- a/wpilibc/shared/src/Resource.cpp
+++ b/wpilibc/shared/src/Resource.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 "Resource.h"
diff --git a/wpilibc/shared/src/RobotState.cpp b/wpilibc/shared/src/RobotState.cpp
index cb28208..f3db1da 100644
--- a/wpilibc/shared/src/RobotState.cpp
+++ b/wpilibc/shared/src/RobotState.cpp
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
#include "RobotState.h"
#include "Base.h"
diff --git a/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp b/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp
index 255cc3a..cc6543f 100644
--- a/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp
+++ b/wpilibc/shared/src/SmartDashboard/SendableChooser.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "SmartDashboard/SendableChooser.h"
diff --git a/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp b/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp
index bee0c03..9fbf605 100644
--- a/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp
+++ b/wpilibc/shared/src/SmartDashboard/SmartDashboard.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2011-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 "SmartDashboard/SmartDashboard.h"
diff --git a/wpilibc/shared/src/interfaces/Potentiometer.cpp b/wpilibc/shared/src/interfaces/Potentiometer.cpp
index 3856090..9ac097b 100644
--- a/wpilibc/shared/src/interfaces/Potentiometer.cpp
+++ b/wpilibc/shared/src/interfaces/Potentiometer.cpp
@@ -1,8 +1,8 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2015. All Rights Reserved.
- */
+/* Copyright (c) FIRST 2015-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 <interfaces/Potentiometer.h>
diff --git a/wpilibc/simulation.gradle b/wpilibc/simulation.gradle
index c010a05..7da3216dc6 100644
--- a/wpilibc/simulation.gradle
+++ b/wpilibc/simulation.gradle
@@ -1,23 +1,96 @@
-publishing {
- publications {
- wpilibcSim(MavenPublication) {
- artifact wpilibcSimZip
- groupId 'edu.wpi.first.wpilibc.simulation'
- artifactId 'WPILibCSim'
- version '0.1.0'
- }
+import org.apache.tools.ant.taskdefs.condition.Os
+
+ //cmake wrapper tasks
+task setupCmake(type: Exec) {
+ description = 'create build directory for cmake to use'
+ group = 'WPILib Simulation'
+ workingDir '..'
+ commandLine 'mkdir', '-p', 'build'
+}
+
+task cmake(type: Exec, dependsOn: setupCmake) {
+ description = 'run cmake in the build directory to generate makefiles'
+ group = 'WPILib Simulation'
+ workingDir '../build'
+ if (Os.isFamily(Os.FAMILY_WINDOWS)) {
+ commandLine '../configure.bat',
+ "-DNTCORE_INCLUDE_DIR=$netTablesInclude",
+ "-DNTCORE_LIBDIR=$netLibDesktopLocation",
+ "-DSIMULATION_INSTALL_DIR=$simulationInstallDir"
+ }
+ else {
+ commandLine 'cmake', '..',
+ "-DNTCORE_INCLUDE_DIR=$netTablesInclude",
+ "-DNTCORE_LIBDIR=$netLibDesktopLocation",
+ "-DSIMULATION_INSTALL_DIR=$simulationInstallDir"
}
}
-task wpilibcSimZip(type: Zip) {
- description 'Creates the include zip file for wpilibc'
- group 'WPILib'
- baseName 'WPILibCSim'
- destinationDir = project.buildDir
- into 'sim/include'
- from "${simulation}/include"
- from "${shared}/include"
- from '../build/simulation/gz_msgs/generated'
- from netTablesInclude
- from '../hal/include'
+task frc_gazebo_plugins(type: Exec, dependsOn: cmake) {
+ description = 'build Gazebo plugins with cmake'
+ group = 'WPILib Simulation'
+ workingDir '../build'
+ if (Os.isFamily(Os.FAMILY_WINDOWS)) {
+ commandLine 'nmake', 'frc_gazebo_plugins'
+ }
+ else {
+ commandLine 'make', 'frc_gazebo_plugins'
+ }
}
+
+task gz_msgs(type: Exec, dependsOn: cmake) {
+ description = 'build gz_msgs library with cmake'
+ group = 'WPILib Simulation'
+ workingDir '../build'
+ if (Os.isFamily(Os.FAMILY_WINDOWS)) {
+ commandLine 'nmake', 'gz_msgs'
+ }
+ else {
+ commandLine 'make', 'gz_msgs'
+ }
+}
+
+task wpilibcSim(type: Exec, dependsOn: ['cmake', ':unzipNetworkTables']) {
+ description = 'build WPILib C++ for simulation with cmake'
+ group = 'WPILib Simulation'
+ workingDir '../build'
+ if (Os.isFamily(Os.FAMILY_WINDOWS)) {
+ commandLine 'nmake', 'wpilibcSim'
+ }
+ else {
+ commandLine 'make', 'wpilibcSim'
+ }
+}
+
+task allcsim(dependsOn: [wpilibcSim, gz_msgs, frc_gazebo_plugins]){
+ description = 'wrapper task to build all c++ simulation tasks'
+ group = 'WPILib Simulation'
+}
+
+task wpilibcSimCopy(type: Copy, dependsOn: allcsim) {
+ description = 'copy headers and ntcore library to make simulation zip'
+ group = 'WPILib Simulation'
+ into "$simulationInstallDir"
+
+ from ("$netTablesInclude"){
+ into "include"
+ }
+
+ from ("../hal/include"){
+ into "include"
+ }
+
+ from ("simulation/include"){
+ into "include"
+ }
+
+ from ("shared/include"){
+ into "include"
+ }
+
+ from ("$netLibDesktopLocation/libntcore.so") {
+ into "lib"
+ }
+}
+
+build.dependsOn allcsim
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"