Squashed 'third_party/allwpilib_2016/' changes from 7f61816..3ce6feb
3ce6feb Updated release number for the new release
e054bbc This adds StopMotor() to the SpeedController interface for C++ and Java. For Java, this is as simple as just adding it, as all motors already have an implementation from MotorSafety that is correctly resolved. For C++, I had to override StopMotor in the classes that descend from SafePWM and explicitly call the SafePWM version. RobotDrive now calls StopMotor on each of its SpeedControllers, instead of calling Disable or setting the motor to 0.0 as it was doing previously.
a15b9dc Merge "More updates to the Gyro test fixing potential null pointer exception"
21b7213 Added Config routine to allow enabling/disabling of limit switch and soft limits. This improves upon the ConfigLimitMode routine, which does not allow certain combinations of enable/disabled limit features. Also keeps parity with LV and Java.
1b45237 Merge "Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically."
1096b15 Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically.
7da21fa More updates to the Gyro test fixing potential null pointer exception
ede5862 Rate-limit duplicate error messages to avoid flooding console.
c22284d Merge "artf4818: Fix CAN Talon JNI references with underscores."
cac3741 Merge "Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now."
097aa8c Fixed the gyro deviation over time test
1d647b3 artf4818: Fix CAN Talon JNI references with underscores.
368cfc7 Merge "Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs."
833e459 Updated PDP port of Talon and disabled PDP tests for Victor and Jaguar since the Victor and Jaguar don't draw enough current for the PDP to read above 0. PDP tests for both java and cpp only test the Talon now.
6c096a3 Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs.
dd7eb0f Fixed robot drive for C++ Simulation
258a622 Merge "Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405"
b1386c6 Update version number for Release 3 Print distinctive message on robot program startup Change-Id: Ic91b81bd298ee6730503933cf0e733702e4b4405
a58de40 Merge "Removed publishing of java sim jar"
792d0d3 PDP Classes should support any PDP address
35df955 Merge "Remove maven local as a possible search location"
a0ce9ee Another improvement to HAL-joy getting to ensure it works in future RIO image updates.
0f02c31 Removed publishing of java sim jar
8435ac7 DriverStation::GetJoystickName(): Make work for stick>0.
b4cf4f4 Remove maven local as a possible search location
c3000c3 Merge "Fix HALGetJoystickDescriptor()."
4dec0b4 Merge "Fixed Simulation C++ API"
abc9c27 Fixed Simulation C++ API
b8ae9ec Fix HALGetJoystickDescriptor().
a60f874 Artf4800: Fixes HALGetJoystick*** Segfault
010b584 Merge "fix sim_ds launch script"
4429e16 Merge "Added build dir specification for sim javadoc to not overwrite athena javadoc"
ec9349b Initialized the m_sensors variable to fix artf4798.
9745af8 Added build dir specification for sim javadoc to not overwrite athena javadoc
4da8702 fix sim_ds launch script
05acf79 Fix C++ PIDController SetToleranceBuffer and OnTarget locking.
94a6b05 Merge "Fix onTarget() so that it returns false until there are any values retrieved"
d06053d Fix onTarget() so that it returns false until there are any values retrieved
74927cc Correctly set smart dashboard type for AnalogGyro and ADXRS450_Gyro.
070752f Merge "Fixed sim_ds script library path"
21a8bab Merge "PIDController feed forward term can now be calculated by the end user"
56bd6da Fixed sim_ds script library path
07710f1 Merge "Fixing install script... again"
e1cb61f Use absolute path for NT persistent storage.
09c7482 Fixing install script... again
a3b8bec PIDController feed forward term can now be calculated by the end user
790adb0 artf2612: Update license in source files.
042671c Merge "Removed gz_msgs from wpilibcSim"
c111690 Ultrasonic: replace linked list with std::set.
d71a8ed Removed gz_msgs from wpilibcSim
37259f7 Merge "Replaced linked list in Notifier with std::list"
cd17e7a Merge "Renamed Gyro to AnalogGyro to match athena API"
c5c8a87 Replaced linked list in Notifier with std::list
89405d8 Renamed Gyro to AnalogGyro to match athena API
4a19490 Merge "Adds CANTalon to LiveWindow"
c4a3567 Merge "Fixing the frcsim installer script"
295648f Adds CANTalon to LiveWindow
1b964a2 Merge "Fixes CAN devices in C++ library not showing in the livewindow"
7ba5cee Merge "HAL: Use extern "C" in implementation files."
d17d242 Fixes CAN devices in C++ library not showing in the livewindow
25a771a Added linear digital filters
7349c2c Fixing the frcsim installer script
c82122c Merge "Default bufLength for PIDController in Java should be 1"
58f3f97 Merge "Adds WaitResult to Java waitForInterrupt"
bc8ed12 HAL: Use extern "C" in implementation files.
4cac89e Default bufLength for PIDController in Java should be 1
64fcdcc Keep track of FPGA time rollovers with 64-bit time.
d30b283 Merge "Change C++ Notifier to allow std::function callback."
6ee3052 Merge "Rewrite C++ Notifier to use HAL multi-notifier support."
f5d09e2 Merge "Rewrite Java Notifier and update Interrupt JNI."
d0274aa Merge "Readded styleguide accidentally removed in the reorg"
68311ad Merge "Artf4179: Allow alternate I2C addresses for ADXL345_I2C"
dee12d4 Readded styleguide accidentally removed in the reorg
fa100df Fixed some typos in the comments of MotorEncoderFixture.java, a method name in CANMotorEncoderFixture.java, and the README files
3397b5c Adds WaitResult to Java waitForInterrupt
5f0dffd Artf4177: Use read byte count for ReadString
8564f33 Artf4179: Allow alternate I2C addresses for ADXL345_I2C
e52b52d Change C++ Notifier to allow std::function callback.
40b29e7 Rewrite C++ Notifier to use HAL multi-notifier support.
d126f45 Rewrite Java Notifier and update Interrupt JNI.
557805a Merge "finishing up FRCSim installer"
911b64b finishing up FRCSim installer
e24fe6f Merge "Artf4776 Fixes First DIO PWM usage errors"
f8f9284 Merge "Artf4774 Fixes HAL getHALErrorMessage missing error"
84428d5 Merge "Prevent double free in DigitalGlitchFilter"
a00a5ff Merge "Set correct error message"
9aeee98 Prevent double free in DigitalGlitchFilter
5d2186c working on install process for FRCSim 2016
7fdb616 Merge "This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places."
f67ebca Improved READMEs
8a7c019 Artf4776 Fixes First DIO PWM usage errors
1cd2f9a Added libnipalu to make vision programs link properly
375b92a This commit adds documentation generation, including grabbing ntcore sources, for both Java and C++. This will need changes made in the wpilib promotion tasks to copy the generatd documentation to the correct places.
8d0a990 Set correct error message
b65401f Artf4774 Fixes HAL getHALErrorMessage missing error
5f918be Condition java sim build on -PmakeSim flag
53bd180 Merge "Add SPARK and SD540 motor controllers"
66cbe69 Fixed double free of DriverStation.
431f345 Repaired simulation build on linux
611593c Add Cmake wrappers and unzip desktop ntcore builds
51a18cd Add SPARK and SD540 motor controllers
c05e883 Merge changes I55ce71c6,I803680c1
2b80029 Rewrite CANTalon JNI layer.
ef4c45b Last feature addition for CANTalon java/C++ user-facing API.
Change-Id: Ia3a124978a426991890b6f8abbe07d34d75ba38d
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 3ce6feb8acdeca46e93a55280fb6ace3a4d5bcd6
diff --git a/wpilibc/simulation/src/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;
}