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/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/GyroBase.cpp b/wpilibc/Athena/src/GyroBase.cpp
deleted file mode 100644
index 4cc9056..0000000
--- a/wpilibc/Athena/src/GyroBase.cpp
+++ /dev/null
@@ -1,46 +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 "GyroBase.h"
-#include "WPIErrors.h"
-#include "LiveWindow/LiveWindow.h"
-
-/**
- * Get the PIDOutput for the PIDSource base object. Can be set to return
- * angle or rate using SetPIDSourceType(). Defaults to angle.
- *
- * @return The PIDOutput (angle or rate, defaults to angle)
- */
-double GyroBase::PIDGet() {
- switch (GetPIDSourceType()) {
- case PIDSourceType::kRate:
- return GetRate();
- case PIDSourceType::kDisplacement:
- return GetAngle();
- default:
- return 0;
- }
-}
-
-void GyroBase::UpdateTable() {
- if (m_table != nullptr) {
- m_table->PutNumber("Value", GetAngle());
- }
-}
-
-void GyroBase::StartLiveWindowMode() {}
-
-void GyroBase::StopLiveWindowMode() {}
-
-std::string GyroBase::GetSmartDashboardType() const { return "Gyro"; }
-
-void GyroBase::InitTable(std::shared_ptr<ITable> subTable) {
- m_table = subTable;
- UpdateTable();
-}
-
-std::shared_ptr<ITable> GyroBase::GetTable() const { return m_table; }
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>