Updated release to version 5.

The request is not guaranteed to be in a single packet, so it may be necessary
to perform multiple reads to get the complete request.  The previous code
could unpredictably fail as it only performed a single read.

Fixes artf4817.

Change-Id: I7734a12e1a2542f5d7ca0889453c387f0bb30538

build.gradle

@@ -18,7 +18,6 @@ allprojects {
         maven {
             url publishUrl
         }
-        mavenLocal()
         maven {
             url repoBaseUrl
         }

hal/include/HAL/HAL.hpp

@@ -227,6 +227,9 @@ extern "C"
 	bool getFPGAButton(int32_t *status);
 
 	int HALSetErrorData(const char *errors, int errorsLength, int wait_ms);
+	int HALSendError(int isError, int32_t errorCode, int isLVCode,
+		const char *details, const char *location, const char *callStack,
+		int printMsg);
 
 	int HALGetControlWord(HALControlWord *data);
 	int HALGetAllianceStation(enum HALAllianceStationID *allianceStation);

hal/lib/Athena/HALAthena.cpp

@@ -32,6 +32,7 @@ static tGlobal *global = nullptr;
 static tSysWatchdog *watchdog = nullptr;
 
 static priority_mutex timeMutex;
+static priority_mutex msgMutex;
 static uint32_t timeEpoch = 0;
 static uint32_t prevFPGATime = 0;
 static void* rolloverNotifier = nullptr;
@@ -232,6 +233,54 @@ int HALSetErrorData(const char *errors, int errorsLength, int wait_ms)
 	return setErrorData(errors, errorsLength, wait_ms);
 }
 
+int HALSendError(int isError, int32_t errorCode, int isLVCode,
+	const char *details, const char *location, const char *callStack,
+	int printMsg)
+{
+	// Avoid flooding console by keeping track of previous 5 error
+	// messages and only printing again if they're longer than 1 second old.
+	static constexpr int KEEP_MSGS = 5;
+	std::lock_guard<priority_mutex> lock(msgMutex);
+	static std::string prev_msg[KEEP_MSGS];
+	static uint64_t prev_msg_time[KEEP_MSGS] = { 0, 0, 0 };
+
+	int32_t status = 0;
+	uint64_t curTime = getFPGATime(&status);
+	int i;
+	for (i=0; i<KEEP_MSGS; ++i) {
+		if (prev_msg[i] == details) break;
+	}
+	int retval = 0;
+	if (i == KEEP_MSGS || (curTime - prev_msg_time[i]) >= 1000000) {
+		retval = FRC_NetworkCommunication_sendError(isError, errorCode, isLVCode, details, location, callStack);
+		if (printMsg) {
+			if (location && location[0] != '\0') {
+				fprintf(stderr, "%s at %s: ",
+					isError ? "Error" : "Warning",
+					location);
+			}
+			fprintf(stderr, "%s\n", details);
+			if (callStack && callStack[0] != '\0') {
+				fprintf(stderr, "%s\n", callStack);
+			}
+		}
+		if (i == KEEP_MSGS) {
+			// replace the oldest one
+			i = 0;
+			uint64_t first = prev_msg_time[0];
+			for (int j=1; j<KEEP_MSGS; ++j) {
+				if (prev_msg_time[j] < first) {
+					first = prev_msg_time[j];
+					i = j;
+				}
+			}
+			prev_msg[i] = details;
+		}
+		prev_msg_time[i] = curTime;
+	}
+	return retval;
+}
+
 
 bool HALGetSystemActive(int32_t *status)
 {

hal/lib/Athena/ctre/PDP.cpp

@@ -11,10 +11,10 @@
 #define CONTROL_1		0x08041C00	/* PDP_Control_ClearStats */
 
 #define EXPECTED_RESPONSE_TIMEOUT_MS	(50)
-#define GET_STATUS1()		CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_STATUS2()		CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_STATUS3()		CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_STATUS_ENERGY()	CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS1()		CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS2()		CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS3()		CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS_ENERGY()	CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
 
 /* encoder/decoders */
 typedef struct _PdpStatus1_t{

hal/lib/Shared/HAL.cpp

@@ -34,17 +34,32 @@ int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons)
 {
 	return FRC_NetworkCommunication_getJoystickButtons(joystickNum, &buttons->buttons, &buttons->count);
 }
-
+/**
+ * Retrieve the Joystick Descriptor for particular slot
+ * @param desc [out] descriptor (data transfer object) to fill in.  desc is filled in regardless of success.
+ *                              In other words, if descriptor is not available, desc is filled in with default
+ *                              values matching the init-values in Java and C++ Driverstation for when caller
+ *                              requests a too-large joystick index.
+ *
+ * @return error code reported from Network Comm back-end.  Zero is good, nonzero is bad.
+ */
 int HALGetJoystickDescriptor(uint8_t joystickNum, HALJoystickDescriptor *desc)
 {
 	desc->isXbox = 0;
 	desc->type = -1;
 	desc->name[0] = '\0';
-	desc->axisCount = 0;
+	desc->axisCount = kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */
 	desc->buttonCount = 0;
 	desc->povCount = 0;
-	return FRC_NetworkCommunication_getJoystickDesc(joystickNum, &desc->isXbox, &desc->type, (char *)(&desc->name),
+	int retval = FRC_NetworkCommunication_getJoystickDesc(joystickNum, &desc->isXbox, &desc->type, (char *)(&desc->name),
 		&desc->axisCount, (uint8_t *)&desc->axisTypes, &desc->buttonCount, &desc->povCount);
+	/* check the return, if there is an error and the RIOimage predates FRC2017, then axisCount needs to be cleared */
+	if(retval != 0)
+	{
+		/* set count to zero so downstream code doesn't decode invalid axisTypes. */
+		desc->axisCount = 0;
+	}
+	return retval;
 }
 
 int HALGetJoystickIsXbox(uint8_t joystickNum)

wpilibc/Athena/include/CANJaguar.h

@@ -230,6 +230,7 @@ class CANJaguar : public MotorSafety,
   mutable std::atomic<bool> m_receivedStatusMessage2{false};
 
   bool m_controlEnabled = false;
+  bool m_stopped = false;
 
   void verify();
 

wpilibc/Athena/include/CANSpeedController.h

@@ -65,6 +65,7 @@ class CANSpeedController : public SpeedController {
 
   virtual float Get() const = 0;
   virtual void Set(float value, uint8_t syncGroup = 0) = 0;
+  virtual void StopMotor() = 0;
   virtual void Disable() = 0;
   virtual void SetP(double p) = 0;
   virtual void SetI(double i) = 0;

wpilibc/Athena/include/CANTalon.h

@@ -286,6 +286,9 @@ class CANTalon : public MotorSafety,
   virtual void ConfigLimitMode(LimitMode mode) override;
   virtual void ConfigForwardLimit(double forwardLimitPosition) override;
   virtual void ConfigReverseLimit(double reverseLimitPosition) override;
+  void ConfigLimitSwitchOverrides(bool bForwardLimitSwitchEn, bool bReverseLimitSwitchEn);
+  void ConfigForwardSoftLimitEnable(bool bForwardSoftLimitEn);
+  void ConfigReverseSoftLimitEnable(bool bReverseSoftLimitEn);
   /**
    * Change the fwd limit switch setting to normally open or closed.
    * Talon will disable momentarilly if the Talon's current setting
@@ -437,6 +440,7 @@ class CANTalon : public MotorSafety,
                   // actually test this.
 
   bool m_controlEnabled = true;
+  bool m_stopped = false;
   ControlMode m_controlMode = kPercentVbus;
   TalonControlMode m_sendMode;
 

wpilibc/Athena/include/DriverStation.h

@@ -31,6 +31,10 @@ class DriverStation : public SensorBase, public RobotStateInterface {
   virtual ~DriverStation();
   static DriverStation &GetInstance();
   static void ReportError(std::string error);
+  static void ReportWarning(std::string error);
+  static void ReportError(bool is_error, int32_t code, const std::string& error,
+                          const std::string& location,
+                          const std::string& stack);
 
   static const uint32_t kJoystickPorts = 6;
 
@@ -97,6 +101,7 @@ class DriverStation : public SensorBase, public RobotStateInterface {
  private:
   static DriverStation *m_instance;
   void ReportJoystickUnpluggedError(std::string message);
+  void ReportJoystickUnpluggedWarning(std::string message);
   void Run();
 
   HALJoystickAxes m_joystickAxes[kJoystickPorts];

wpilibc/Athena/include/Jaguar.h

@@ -21,6 +21,7 @@ class Jaguar : public SafePWM, public SpeedController {
   virtual void Set(float value, uint8_t syncGroup = 0) override;
   virtual float Get() const override;
   virtual void Disable() override;
+  virtual void StopMotor() override;
 
   virtual void PIDWrite(float output) override;
   virtual void SetInverted(bool isInverted) override;

wpilibc/Athena/include/SD540.h

@@ -21,6 +21,7 @@ class SD540 : public SafePWM, public SpeedController {
   virtual void Set(float value, uint8_t syncGroup = 0) override;
   virtual float Get() const override;
   virtual void Disable() override;
+  virtual void StopMotor() override;
 
   virtual void PIDWrite(float output) override;
 

wpilibc/Athena/include/Spark.h

@@ -21,6 +21,7 @@ class Spark : public SafePWM, public SpeedController {
   virtual void Set(float value, uint8_t syncGroup = 0) override;
   virtual float Get() const override;
   virtual void Disable() override;
+  virtual void StopMotor() override;
 
   virtual void PIDWrite(float output) override;
 

wpilibc/Athena/include/SpeedController.h

@@ -48,4 +48,9 @@ class SpeedController : public PIDOutput {
    * @return isInverted The state of inversion, true is inverted.
    */
   virtual bool GetInverted() const = 0;
+
+  /**
+   * Common interface to stop the motor until Set is called again.
+   */
+  virtual void StopMotor() = 0;
 };

wpilibc/Athena/include/Talon.h

@@ -25,6 +25,7 @@ class Talon : public SafePWM, public SpeedController {
   virtual void PIDWrite(float output) override;
   virtual void SetInverted(bool isInverted) override;
   virtual bool GetInverted() const override;
+  virtual void StopMotor() override;
 
  private:
   bool m_isInverted = false;

wpilibc/Athena/include/TalonSRX.h

@@ -22,6 +22,7 @@ class TalonSRX : public SafePWM, public SpeedController {
   virtual void Set(float value, uint8_t syncGroup = 0) override;
   virtual float Get() const override;
   virtual void Disable() override;
+  virtual void StopMotor() override;
 
   virtual void PIDWrite(float output) override;
   virtual void SetInverted(bool isInverted) override;

wpilibc/Athena/include/Victor.h

@@ -24,6 +24,7 @@ class Victor : public SafePWM, public SpeedController {
   virtual void Set(float value, uint8_t syncGroup = 0) override;
   virtual float Get() const override;
   virtual void Disable() override;
+  virtual void StopMotor() override;
 
   virtual void PIDWrite(float output) override;
 

wpilibc/Athena/include/VictorSP.h

@@ -21,6 +21,7 @@ class VictorSP : public SafePWM, public SpeedController {
   virtual void Set(float value, uint8_t syncGroup = 0) override;
   virtual float Get() const override;
   virtual void Disable() override;
+  virtual void StopMotor() override;
 
   virtual void PIDWrite(float output) override;
 

wpilibc/Athena/src/CANJaguar.cpp

@@ -246,8 +246,11 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) {
   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) {
@@ -290,8 +293,6 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) {
     }
 
     sendMessage(messageID, dataBuffer, dataSize, kSendMessagePeriod);
-
-    if (m_safetyHelper) m_safetyHelper->Feed();
   }
 
   m_value = outputValue;
@@ -1934,12 +1935,14 @@ void CANJaguar::GetDescription(std::ostringstream& desc) const {
 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.

wpilibc/Athena/src/CANTalon.cpp

@@ -134,6 +134,12 @@ float CANTalon::Get() const {
 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;
@@ -1206,7 +1212,43 @@ void CANTalon::DisableSoftPositionLimits() {
 }
 
 /**
- * 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.
  */
@@ -1283,6 +1325,32 @@ void CANTalon::ConfigForwardLimit(double forwardLimitPosition) {
     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
@@ -1825,12 +1893,15 @@ void CANTalon::SetInverted(bool isInverted) { m_isInverted = isInverted; }
 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) {

wpilibc/Athena/src/CameraServer.cpp

@@ -189,16 +189,26 @@ void CameraServer::Serve() {
     }
 
     Request req;
-    if (read(conn, &req, sizeof(req)) == -1) {
+    char reqBuf[sizeof(req)];
+    size_t reqPos = 0;
+
+    while (reqPos < sizeof(req)) {
+      ssize_t sizeRead = read(conn, &reqBuf[reqPos], sizeof(req) - reqPos);
+      if (sizeRead < 0) break;
+      reqPos += sizeRead;
+    }
+
+    if (reqPos < sizeof(req)) {
       wpi_setErrnoError();
       close(conn);
       continue;
-    } else {
-      req.fps = ntohl(req.fps);
-      req.compression = ntohl(req.compression);
-      req.size = ntohl(req.size);
     }
 
+    memcpy(&req, reqBuf, sizeof(req));
+    req.fps = ntohl(req.fps);
+    req.compression = ntohl(req.compression);
+    req.size = ntohl(req.size);
+
     // TODO: Support the SW Compression. The rest of the code below will work as
     // though this
     // check isn't here

wpilibc/Athena/src/DriverStation.cpp

@@ -133,6 +133,18 @@ void DriverStation::ReportJoystickUnpluggedError(std::string message) {
   }
 }
 
+/**
+ * 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
  *
@@ -255,9 +267,8 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) {
     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 @@ int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
     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 @@ bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) {
   }
 
   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 @@ double DriverStation::GetMatchTime() const {
  * The error is also printed to the program console.
  */
 void DriverStation::ReportError(std::string error) {
-  std::cout << error << std::endl;
-
-  HALControlWord controlWord;
-  HALGetControlWord(&controlWord);
-  if (controlWord.dsAttached) {
-    HALSetErrorData(error.c_str(), error.size(), 0);
-  }
+  HALSendError(1, 1, 0, error.c_str(), "", "", 1);
+}
+
+/**
+ * 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);
 }

wpilibc/Athena/src/Jaguar.cpp

@@ -77,3 +77,8 @@ bool Jaguar::GetInverted() const { return m_isInverted; }
  * @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(); }

wpilibc/Athena/src/RobotBase.cpp

@@ -27,6 +27,7 @@ void RobotBase::setInstance(RobotBase *robot) {
 RobotBase &RobotBase::getInstance() { return *m_instance; }
 
 void RobotBase::robotSetup(RobotBase *robot) {
+  printf("\n********** Robot program starting **********\n");
   robot->StartCompetition();
 }
 
@@ -55,7 +56,7 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
   file = fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
 
   if (file != nullptr) {
-    fputs("2016 C++ Beta5.0", file);
+    fputs("2016 C++ Release 5", file);
     fclose(file);
   }
 }

wpilibc/Athena/src/RobotDrive.cpp

@@ -745,9 +745,9 @@ void RobotDrive::GetDescription(std::ostringstream& desc) const {
 }
 
 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();
 }

wpilibc/Athena/src/SD540.cpp

@@ -86,3 +86,8 @@ void SD540::Disable() { SetRaw(kPwmDisabled); }
  * @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(); }

wpilibc/Athena/src/Spark.cpp

@@ -86,3 +86,8 @@ void Spark::Disable() { SetRaw(kPwmDisabled); }
  * @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(); }

wpilibc/Athena/src/Talon.cpp

@@ -82,3 +82,8 @@ bool Talon::GetInverted() const { return m_isInverted; }
  * @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(); }

wpilibc/Athena/src/TalonSRX.cpp

@@ -79,3 +79,8 @@ bool TalonSRX::GetInverted() const { return m_isInverted; }
  * @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(); }

wpilibc/Athena/src/Utility.cpp

@@ -29,11 +29,13 @@ bool wpi_assert_impl(bool conditionValue, const char *conditionText,
                      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 @@ bool wpi_assert_impl(bool conditionValue, const char *conditionText,
       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 @@ void wpi_assertEqual_common_impl(const char *valueA, const char *valueB,
                                  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 @@ void wpi_assertEqual_common_impl(const char *valueA, const char *valueB,
     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);
 }
 
 /**

wpilibc/Athena/src/Victor.cpp

@@ -82,3 +82,8 @@ bool Victor::GetInverted() const { return m_isInverted; }
  * @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(); }

wpilibc/Athena/src/VictorSP.cpp

@@ -86,3 +86,8 @@ void VictorSP::Disable() { SetRaw(kPwmDisabled); }
  * @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(); }

wpilibc/shared/src/Error.cpp

@@ -66,24 +66,21 @@ void Error::Set(Code code, llvm::StringRef contextMessage,
 }
 
 void Error::Report() {
-  std::stringstream errorStream;
+  std::stringstream locStream;
+  locStream << m_function << " [";
 
-  errorStream << "Error on line " << m_lineNumber << " ";
-#if defined(_UNIX)
-	errorStream << "of " << basename(m_filename.c_str()) << ": ";
-#elif defined(_WIN32)
+#if defined(_WIN32)
 	const int MAX_DIR = 100;
 	char basename[MAX_DIR];
 	_splitpath_s(m_filename.c_str(), NULL, 0, basename, MAX_DIR, NULL, 0, NULL, 0);
-	errorStream << "of " << basename << ": ";
+	locStream << basename;
+#else
+	locStream << basename(m_filename.c_str());
 #endif
+  locStream << ":" << m_lineNumber << "]";
 
-	errorStream << m_message << std::endl;
-	errorStream << GetStackTrace(4);
-
-  std::string error = errorStream.str();
-
-  DriverStation::ReportError(error);
+  DriverStation::ReportError(true, m_code, m_message, locStream.str(),
+                             GetStackTrace(4));
 }
 
 void Error::Clear() {

wpilibc/simulation/include/DriverStation.h

@@ -42,6 +42,8 @@ public:
 	virtual ~DriverStation() = default;
 	static DriverStation &GetInstance();
 	static void ReportError(std::string error);
+	static void ReportWarning(std::string error);
+	static void ReportError(bool is_error, int32_t code, const std::string& error, const std::string& location, const std::string& stack);
 
 	static const uint32_t kBatteryChannel = 7;
 	static const uint32_t kJoystickPorts = 4;

wpilibc/simulation/include/RobotDrive.h

@@ -94,7 +94,7 @@ protected:
 
 	static const int32_t kMaxNumberOfMotors = 4;
 
-	int32_t m_invertedMotors[kMaxNumberOfMotors];
+	int32_t m_invertedMotors[kMaxNumberOfMotors] = {1,1,1,1};
 	float m_sensitivity = 0.5;
 	double m_maxOutput = 1.0;
 	bool m_deleteSpeedControllers;

wpilibc/simulation/src/DriverStation.cpp

@@ -315,6 +315,31 @@ void DriverStation::ReportError(std::string error)
 	std::cout << error << std::endl;
 }
 
+/**
+ * Report a warning to the DriverStation messages window.
+ * The warning is also printed to the program console.
+ */
+void DriverStation::ReportWarning(std::string error)
+{
+	std::cout << error << std::endl;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(bool is_error, int32_t code,
+                                const std::string& error,
+                                const std::string& location,
+                                const std::string& stack)
+{
+	if (!location.empty())
+		std::cout << (is_error ? "Error" : "Warning") << " at " << location << ": ";
+	std::cout << error << std::endl;
+	if (!stack.empty())
+		std::cout << stack << std::endl;
+}
+
 /**
  * Return the team number that the Driver Station is configured for
  * @return The team number

wpilibc/simulation/src/RobotDrive.cpp

@@ -52,11 +52,6 @@ RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
 	InitRobotDrive();
 	m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
 	m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
-
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
-	}
 	SetLeftRightMotorOutputs(0.0, 0.0);
 	m_deleteSpeedControllers = true;
 }
@@ -79,10 +74,6 @@ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
 	m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
 	m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
 	m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
-	}
 	SetLeftRightMotorOutputs(0.0, 0.0);
 	m_deleteSpeedControllers = true;
 }

wpilibcIntegrationTests/include/TestBench.h

@@ -58,7 +58,7 @@ class TestBench {
   /* PDP channels */
   static const uint32_t kJaguarPDPChannel = 6;
   static const uint32_t kVictorPDPChannel = 8;
-  static const uint32_t kTalonPDPChannel = 11;
+  static const uint32_t kTalonPDPChannel = 10;
 
   /* PCM channels */
   static const int32_t kSolenoidChannel1 = 0;

wpilibcIntegrationTests/src/CANTalonTest.cpp

@@ -68,3 +68,9 @@ TEST(CANTalonTest, DISABLED_PositionModeWorks) {
   talon.Disable();
   EXPECT_NEAR(talon.Get(), 500, 1000);
 }
+
+TEST(CANTalonTest, GetFaults) {
+  CANTalon talon(deviceId);
+  EXPECT_EQ(talon.GetFaults(),0);
+  EXPECT_EQ(talon.GetStickyFaults(),0);
+}

wpilibcIntegrationTests/src/MotorEncoderTest.cpp

@@ -84,7 +84,7 @@ TEST_P(MotorEncoderTest, Increment) {
   Reset();
 
   /* Drive the speed controller briefly to move the encoder */
-  m_speedController->Set(1.0);
+  m_speedController->Set(0.2f);
   Wait(kMotorTime);
   m_speedController->Set(0.0);
 
@@ -100,7 +100,7 @@ TEST_P(MotorEncoderTest, Decrement) {
   Reset();
 
   /* Drive the speed controller briefly to move the encoder */
-  m_speedController->Set(-1.0f);
+  m_speedController->Set(-0.2f);
   Wait(kMotorTime);
   m_speedController->Set(0.0f);
 
@@ -131,12 +131,12 @@ TEST_P(MotorEncoderTest, ClampSpeed) {
  */
 TEST_P(MotorEncoderTest, PositionPIDController) {
   Reset();
-
+  double goal = 1000;
   m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement);
   PIDController pid(0.001f, 0.0005f, 0.0f, m_encoder, m_speedController);
-  pid.SetAbsoluteTolerance(20.0f);
-  pid.SetOutputRange(-0.3f, 0.3f);
-  pid.SetSetpoint(2500);
+  pid.SetAbsoluteTolerance(50.0f);
+  pid.SetOutputRange(-0.2f, 0.2f);
+  pid.SetSetpoint(goal);
 
   /* 10 seconds should be plenty time to get to the setpoint */
   pid.Enable();
@@ -145,7 +145,7 @@ TEST_P(MotorEncoderTest, PositionPIDController) {
 
   RecordProperty("PIDError", pid.GetError());
 
-  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds.";
+  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: "<<goal<<" Error was: "<<pid.GetError();
 }
 
 /**
@@ -156,20 +156,18 @@ TEST_P(MotorEncoderTest, VelocityPIDController) {
 
   m_encoder->SetPIDSourceType(PIDSourceType::kRate);
   PIDController pid(1e-5, 0.0f, 3e-5, 8e-5, m_encoder, m_speedController);
-  pid.SetAbsoluteTolerance(50.0f);
-  pid.SetToleranceBuffer(10);
+  pid.SetAbsoluteTolerance(200.0f);
+  pid.SetToleranceBuffer(50);
   pid.SetOutputRange(-0.3f, 0.3f);
-  pid.SetSetpoint(2000);
+  pid.SetSetpoint(600);
 
   /* 10 seconds should be plenty time to get to the setpoint */
   pid.Enable();
   Wait(10.0);
-
-  RecordProperty("PIDError", pid.GetAvgError());
-
-  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 2000 << " Error was: " << pid.GetError();
-
   pid.Disable();
+  RecordProperty("PIDError", pid.GetError());
+
+  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 600 << " Error was: " << pid.GetError();
 }
 
 /**

wpilibcIntegrationTests/src/MotorInvertingTest.cpp

@@ -14,7 +14,7 @@
 #include "TestBench.h"
 
 enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
-static const double motorSpeed = 0.25;
+static const double motorSpeed = 0.15;
 static const double delayTime = 0.5;
 std::ostream &operator<<(std::ostream &os, MotorInvertingTestType const &type) {
   switch (type) {

wpilibcIntegrationTests/src/PIDToleranceTest.cpp

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+#include "PIDSource.h"
+#include "PIDController.h"
+#include "PIDOutput.h"
+
+class PIDToleranceTest : public testing::Test{
+protected:
+	const double setpoint = 50.0;
+	const double range = 200;
+	const double tolerance = 10.0;
+	class fakeInput : public PIDSource{
+		public:
+		double val = 0;
+		void SetPIDSourceType(PIDSourceType pidSource){
+		  }
+		  PIDSourceType GetPIDSourceType(){
+			  return PIDSourceType::kDisplacement;
+		  }
+		  double PIDGet(){;
+		  	  return val;
+		  }
+	};
+	class fakeOutput : public PIDOutput{
+		void PIDWrite(float output){
+
+		}
+	};
+	fakeInput inp;
+	fakeOutput out;
+	PIDController *pid;
+	virtual void SetUp() override {
+		pid = new PIDController(0.5,0.0,0.0,&inp,&out);
+		pid->SetInputRange(-range/2,range/2);
+	}
+	virtual void TearDown() override {
+		delete pid;
+	}
+	virtual void reset(){
+		inp.val = 0;
+	}
+};
+
+TEST_F(PIDToleranceTest, Absolute){
+	reset();
+	pid->SetAbsoluteTolerance(tolerance);
+	pid->SetSetpoint(setpoint);
+	pid->Enable();
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+tolerance/2;
+	Wait(1.0);
+	EXPECT_TRUE(pid->OnTarget())<<"Error was not in tolerance when it should have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+10*tolerance;
+	Wait(1.0);
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+}
+
+TEST_F(PIDToleranceTest, Percent){
+	reset();
+	pid->SetPercentTolerance(tolerance);
+	pid->SetSetpoint(setpoint);
+	pid->Enable();
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+(tolerance)/200*range;//half of percent tolerance away from setpoint
+	Wait(1.0);
+	EXPECT_TRUE(pid->OnTarget())<<"Error was not in tolerance when it should have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+(tolerance)/50*range;//double percent tolerance away from setPoint
+	Wait(1.0);
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+
+}

wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp

@@ -56,40 +56,3 @@ TEST_F(PowerDistributionPanelTest, CheckCurrentTalon) {
       << "The Talon current was not positive";
 }
 
-/**
- * Test if the current changes when the motor is driven using a victor
- */
-TEST_F(PowerDistributionPanelTest, CheckCurrentVictor) {
-  Wait(kMotorTime);
-
-  /* The Current should be 0 */
-  EXPECT_FLOAT_EQ(0, m_pdp->GetCurrent(TestBench::kVictorPDPChannel))
-      << "The Victor current was non-zero";
-
-  /* Set the motor to full forward */
-  m_victor->Set(1.0);
-  Wait(kMotorTime);
-
-  /* The current should now be positive */
-  ASSERT_GT(m_pdp->GetCurrent(TestBench::kVictorPDPChannel), 0)
-      << "The Victor current was not positive";
-}
-
-/**
- * Test if the current changes when the motor is driven using a jaguar
- */
-TEST_F(PowerDistributionPanelTest, CheckCurrentJaguar) {
-  Wait(kMotorTime);
-
-  /* The Current should be 0 */
-  EXPECT_FLOAT_EQ(0, m_pdp->GetCurrent(TestBench::kJaguarPDPChannel))
-      << "The Jaguar current was non-zero";
-
-  /* Set the motor to full forward */
-  m_jaguar->Set(1.0);
-  Wait(kMotorTime);
-
-  /* The current should now be positive */
-  ASSERT_GT(m_pdp->GetCurrent(TestBench::kJaguarPDPChannel), 0)
-      << "The Jaguar current was not positive";
-}

wpilibj/simulation.gradle

@@ -53,29 +53,8 @@ task wpilibjSimJavadocJar(type: Jar, dependsOn: wpilibjSimJavadoc) {
     from wpilibjSimJavadoc.destinationDir
 }
 
-// Maven publishing configuration
-publishing {
-    publications {
-        wpilibjSim(MavenPublication) {
-            artifact wpilibjSimJar {
-                classifier = 'simulation'
-            }
-            artifact(wpilibjSimSources) {
-                classifier = 'sources-simulation'
-            }
-            artifact(wpilibjSimJavadocJar) {
-                classifier 'javadoc-simulation'
-            }
-
-            groupId 'edu.wpi.first.wpilibj'
-            artifactId 'wpilibJavaSim'
-            version '0.1.0-SNAPSHOT'
-        }
-    }
-}
-
 //we need to move the simulation jars to the install directory
-task copyJars(type: Copy) {
+task copyJars(type: Copy, dependsOn: wpilibjSimJar) {
     description = 'copy wpilibj simulation jar to make simulation zip'
     group = 'WPILib Simulation'
     from wpilibjSimJar.archivePath

wpilibj/src/athena/cpp/lib/CanTalonJNI.cpp

@@ -363,7 +363,7 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_ProcessMotionP
 {
   return ((CanTalonSRX*)handle)->ProcessMotionProfileBuffer();
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_OverTemp
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1OverTemp
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -371,7 +371,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_OverT
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_UnderVoltage
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1UnderVoltage
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -379,7 +379,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_Under
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_ForLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1ForLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -387,7 +387,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_ForLi
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_RevLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1RevLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -395,7 +395,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_RevLi
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_HardwareFailure
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1HardwareFailure
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -403,7 +403,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_Hardw
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_ForSoftLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1ForSoftLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -411,7 +411,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_ForSo
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_RevSoftLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_1RevSoftLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -419,7 +419,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetFault_RevSo
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_OverTemp
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1OverTemp
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -427,7 +427,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_UnderVoltage
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1UnderVoltage
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -435,7 +435,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_ForLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1ForLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -443,7 +443,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_RevLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1RevLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -451,7 +451,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_ForSoftLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1ForSoftLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -459,7 +459,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_RevSoftLim
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetStckyFault_1RevSoftLim
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -683,7 +683,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetPulseWidthR
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_IsValid
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1IsValid
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -691,7 +691,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_IsV
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_ProfileSlotSelect
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1ProfileSlotSelect
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -699,7 +699,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_Pro
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_VelOnly
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1VelOnly
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -707,7 +707,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_Vel
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_IsLast
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1IsLast
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -763,7 +763,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetCount
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_Velocity
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1Velocity
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;
@@ -771,7 +771,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_Vel
   CheckCTRStatus(env, status);
   return retval;
 }
-JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_Position
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_GetActTraj_1Position
   (JNIEnv * env, jclass, jlong handle)
 {
   int retval;

wpilibj/src/athena/cpp/lib/FRCNetworkCommunicationsLibrary.cpp

@@ -323,4 +323,26 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommun
   return returnValue;
 }
 
+/*
+ * Class:     edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary
+ * Method:    HALSendError
+ * Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
+ */
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALSendError
+  (JNIEnv * env, jclass, jboolean isError, jint errorCode, jboolean isLVCode, jstring details, jstring location, jstring callStack, jboolean printMsg)
+{
+  const char * detailsStr = env->GetStringUTFChars(details, NULL);
+  const char * locationStr = env->GetStringUTFChars(location, NULL);
+  const char * callStackStr = env->GetStringUTFChars(callStack, NULL);
+
+  NETCOMM_LOG(logDEBUG) << "Send Error: " << detailsStr;
+  NETCOMM_LOG(logDEBUG) << "Location: " << locationStr;
+  NETCOMM_LOG(logDEBUG) << "Call Stack: " << callStackStr;
+  jint returnValue = HALSendError(isError, errorCode, isLVCode, detailsStr, locationStr, callStackStr, printMsg);
+  env->ReleaseStringUTFChars(details,detailsStr);
+  env->ReleaseStringUTFChars(location,locationStr);
+  env->ReleaseStringUTFChars(callStack,callStackStr);
+  return returnValue;
+}
+
 }  // extern "C"

wpilibj/src/athena/cpp/lib/HALUtil.cpp

@@ -14,6 +14,7 @@
 #include "HAL/HAL.hpp"
 #include "errno.h"
 #include <string.h>
+#include <cstring>
 #include <string>
 
 // set the logging level
@@ -42,7 +43,7 @@ static jclass canMessageNotAllowedExCls = nullptr;
 static jclass canNotInitializedExCls = nullptr;
 static jclass uncleanStatusExCls = nullptr;
 
-static void GetStackTrace(JNIEnv *env, std::string& res) {
+static void GetStackTrace(JNIEnv *env, std::string& res, std::string& func) {
   // create a throwable
   static jmethodID constructorId = nullptr;
   if (!constructorId)
@@ -72,6 +73,7 @@ static void GetStackTrace(JNIEnv *env, std::string& res) {
     toStringId = env->GetMethodID(stackTraceElementCls, "toString",
                                   "()Ljava/lang/String;");
 
+  bool haveLoc = false;
   for (jsize i = 0; i < stackTraceLength; i++) {
     // add the result of toString method of each element in the result
     jobject curStackTraceElement = env->GetObjectArrayElement(stackTrace, i);
@@ -91,6 +93,16 @@ static void GetStackTrace(JNIEnv *env, std::string& res) {
     const char *tmp = env->GetStringUTFChars(stackElementString, nullptr);
     res += tmp;
     res += '\n';
+
+    // func is caller of immediate caller (if there was one)
+    // or, if we see it, the first user function
+    if (i == 1)
+      func = tmp;
+    else if (i > 1 && !haveLoc &&
+             std::strncmp(tmp, "edu.wpi.first.wpilibj", 21) != 0) {
+      func = tmp;
+      haveLoc = true;
+    }
     env->ReleaseStringUTFChars(stackElementString, tmp);
 
     env->DeleteLocalRef(curStackTraceElement);
@@ -110,14 +122,10 @@ void ReportError(JNIEnv *env, int32_t status, bool do_throw) {
     env->ThrowNew(runtimeExCls, buf);
     delete[] buf;
   } else {
-    std::string fullmsg = message;
-    fullmsg += " at ";
-    GetStackTrace(env, fullmsg);
-    fprintf(stderr, "%s\n", fullmsg.c_str());
-    HALControlWord controlWord;
-    HALGetControlWord(&controlWord);
-    if (controlWord.dsAttached)
-      HALSetErrorData(fullmsg.c_str(), fullmsg.size(), 0);
+    std::string stack = " at ";
+    std::string func;
+    GetStackTrace(env, stack, func);
+    HALSendError(1, status, 0, message, func.c_str(), stack.c_str(), 1);
   }
 }
 

wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java

@@ -375,6 +375,14 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController {
     byte[] data = new byte[8];
     byte dataSize;
 
+    if (m_safetyHelper != null)
+      m_safetyHelper.feed();
+
+    if (m_stopped) {
+      enableControl();
+      m_stopped = false;
+    }
+
     if (m_controlEnabled) {
       switch (m_controlMode) {
         case PercentVbus:
@@ -412,9 +420,6 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController {
       }
 
       sendMessage(messageID, data, dataSize, kSendMessagePeriod);
-
-      if (m_safetyHelper != null)
-        m_safetyHelper.feed();
     }
 
     m_value = outputValue;
@@ -1890,6 +1895,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController {
   static final int kReceiveStatusAttempts = 50;
 
   boolean m_controlEnabled = true;
+  boolean m_stopped = false;
 
   static void sendMessageHelper(int messageID, byte[] data, int dataSize, int period)
       throws CANMessageNotFoundException {
@@ -2244,9 +2250,9 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController {
    * @deprecated Use disableControl instead.
    */
   @Override
-  @Deprecated
   public void stopMotor() {
     disableControl();
+    m_stopped = true;
   }
 
   /*

wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java

@@ -284,6 +284,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont
 
   int m_deviceNumber;
   boolean m_controlEnabled;
+  boolean m_stopped = false;
   int m_profile;
 
   double m_setPoint;
@@ -423,6 +424,10 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont
   public void set(double outputValue) {
     /* feed safety helper since caller just updated our output */
     m_safetyHelper.feed();
+    if(m_stopped) {
+      enableControl();
+      m_stopped = false;
+    }
     if (m_controlEnabled) {
       m_setPoint = outputValue; /* cache set point for getSetpoint() */
       switch (m_controlMode) {
@@ -1212,9 +1217,9 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont
    * @deprecated Use disableControl instead.
    */
   @Override
-  @Deprecated
   public void stopMotor() {
     disableControl();
+    m_stopped = true;
   }
 
   @Override

wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -211,6 +211,18 @@ public class DriverStation implements RobotState.Interface {
     }
   }
 
+  /**
+   * Reports errors related to unplugged joysticks Throttles the errors so that
+   * they don't overwhelm the DS
+   */
+  private void reportJoystickUnpluggedWarning(String message) {
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      reportWarning(message, false);
+      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+    }
+  }
+
   /**
    * Get the value of the axis on a joystick. This depends on the mapping of the
    * joystick connected to the specified port.
@@ -229,8 +241,8 @@ public class DriverStation implements RobotState.Interface {
     }
 
     if (axis >= m_joystickAxes[stick].length) {
-      reportJoystickUnpluggedError("WARNING: Joystick axis " + axis + " on port " + stick
-          + " not available, check if controller is plugged in\n");
+      reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
+          + " not available, check if controller is plugged in");
       return 0.0;
     }
 
@@ -273,8 +285,8 @@ public class DriverStation implements RobotState.Interface {
     }
 
     if (pov >= m_joystickPOVs[stick].length) {
-      reportJoystickUnpluggedError("WARNING: Joystick POV " + pov + " on port " + stick
-          + " not available, check if controller is plugged in\n");
+      reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
+          + " not available, check if controller is plugged in");
       return -1;
     }
 
@@ -324,12 +336,12 @@ public class DriverStation implements RobotState.Interface {
 
 
     if (button > m_joystickButtons[stick].count) {
-      reportJoystickUnpluggedError("WARNING: Joystick Button " + button + " on port " + stick
-          + " not available, check if controller is plugged in\n");
+      reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
+          + " not available, check if controller is plugged in");
       return false;
     }
     if (button <= 0) {
-      reportJoystickUnpluggedError("ERROR: Button indexes begin at 1 in WPILib for C++ and Java\n");
+      reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java");
       return false;
     }
     return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
@@ -365,8 +377,8 @@ public class DriverStation implements RobotState.Interface {
     // TODO: Remove this when calling for descriptor on empty stick no longer
     // crashes
     if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].length) {
-      reportJoystickUnpluggedError("WARNING: Joystick on port " + stick
-          + " not available, check if controller is plugged in\n");
+      reportJoystickUnpluggedWarning("Joystick on port " + stick
+          + " not available, check if controller is plugged in");
       return false;
     }
     boolean retVal = false;
@@ -390,8 +402,8 @@ public class DriverStation implements RobotState.Interface {
     // TODO: Remove this when calling for descriptor on empty stick no longer
     // crashes
     if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].length) {
-      reportJoystickUnpluggedError("WARNING: Joystick on port " + stick
-          + " not available, check if controller is plugged in\n");
+      reportJoystickUnpluggedWarning("Joystick on port " + stick
+          + " not available, check if controller is plugged in");
       return -1;
     }
     return FRCNetworkCommunicationsLibrary.HALGetJoystickType((byte) stick);
@@ -411,8 +423,8 @@ public class DriverStation implements RobotState.Interface {
     // TODO: Remove this when calling for descriptor on empty stick no longer
     // crashes
     if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].length) {
-      reportJoystickUnpluggedError("WARNING: Joystick on port " + stick
-          + " not available, check if controller is plugged in\n");
+      reportJoystickUnpluggedWarning("Joystick on port " + stick
+          + " not available, check if controller is plugged in");
       return "";
     }
     return FRCNetworkCommunicationsLibrary.HALGetJoystickName((byte) stick);
@@ -600,19 +612,39 @@ public class DriverStation implements RobotState.Interface {
    * @param printTrace If true, append stack trace to error string
    */
   public static void reportError(String error, boolean printTrace) {
-    String errorString = error;
-    if (printTrace) {
-      errorString += " at ";
-      StackTraceElement[] traces = Thread.currentThread().getStackTrace();
-      for (int i = 2; i < traces.length; i++) {
-        errorString += traces[i].toString() + "\n";
+    reportErrorImpl(true, 1, error, printTrace);
+  }
+
+  /**
+   * Report warning to Driver Station. Also prints error to System.err Optionally
+   * appends Stack trace to warning message
+   *$
+   * @param printTrace If true, append stack trace to warning string
+   */
+  public static void reportWarning(String error, boolean printTrace) {
+    reportErrorImpl(false, 1, error, printTrace);
+  }
+
+  private static void reportErrorImpl(boolean is_error, int code, String error, boolean printTrace) {
+    StackTraceElement[] traces = Thread.currentThread().getStackTrace();
+    String locString;
+    if (traces.length > 3)
+      locString = traces[3].toString();
+    else
+      locString = new String();
+    boolean haveLoc = false;
+    String traceString = new String();
+    traceString = " at ";
+    for (int i = 3; i < traces.length; i++) {
+      String loc = traces[i].toString();
+      traceString += loc + "\n";
+      // get first user function
+      if (!haveLoc && !loc.startsWith("edu.wpi.first.wpilibj")) {
+        locString = loc;
+        haveLoc = true;
       }
     }
-    System.err.println(errorString);
-    HALControlWord controlWord = FRCNetworkCommunicationsLibrary.HALGetControlWord();
-    if (controlWord.getDSAttached()) {
-      FRCNetworkCommunicationsLibrary.HALSetErrorData(errorString);
-    }
+    FRCNetworkCommunicationsLibrary.HALSendError(is_error, code, false, error, locString, printTrace ? traceString : "", true);
   }
 
   /**

wpilibj/src/athena/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java

@@ -92,7 +92,7 @@ public class MotorSafetyHelper {
     if (!m_enabled || RobotState.isDisabled() || RobotState.isTest())
       return;
     if (m_stopTime < Timer.getFPGATimestamp()) {
-      System.err.println(m_safeObject.getDescription() + "... Output not updated often enough.");
+      DriverStation.reportError(m_safeObject.getDescription() + "... Output not updated often enough.", false);
 
       m_safeObject.stopMotor();
     }

wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -223,7 +223,7 @@ public abstract class RobotBase {
 
       output = new FileOutputStream(file);
 
-      output.write("2016 Java Beta5.0".getBytes());
+      output.write("2016 Java Release 5".getBytes());
 
     } catch (IOException ex) {
       ex.printStackTrace();
@@ -238,6 +238,7 @@ public abstract class RobotBase {
 
     boolean errorOnExit = false;
     try {
+      System.out.println("********** Robot program starting **********");
       robot.startCompetition();
     } catch (Throwable t) {
       DriverStation.reportError(

wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotDrive.java

@@ -794,16 +794,16 @@ public class RobotDrive implements MotorSafety {
 
   public void stopMotor() {
     if (m_frontLeftMotor != null) {
-      m_frontLeftMotor.set(0.0);
+      m_frontLeftMotor.stopMotor();
     }
     if (m_frontRightMotor != null) {
-      m_frontRightMotor.set(0.0);
+      m_frontRightMotor.stopMotor();
     }
     if (m_rearLeftMotor != null) {
-      m_rearLeftMotor.set(0.0);
+      m_rearLeftMotor.stopMotor();
     }
     if (m_rearRightMotor != null) {
-      m_rearRightMotor.set(0.0);
+      m_rearRightMotor.stopMotor();
     }
     if (m_safetyHelper != null)
       m_safetyHelper.feed();

wpilibj/src/athena/java/edu/wpi/first/wpilibj/SpeedController.java

@@ -51,10 +51,14 @@ public interface SpeedController extends PIDOutput {
    */
   boolean getInverted();
 
-
-
   /**
    * Disable the speed controller
    */
   void disable();
+
+  /**
+   * Stops motor movement. Motor can be moved again by calling set without having
+   * to re-enable the motor.
+   */
+  void stopMotor();
 }

wpilibj/src/athena/java/edu/wpi/first/wpilibj/communication/FRCNetworkCommunicationsLibrary.java

@@ -233,4 +233,6 @@ public class FRCNetworkCommunicationsLibrary extends JNIWrapper {
   public static native boolean HALGetBrownedOut();
 
   public static native int HALSetErrorData(String error);
+
+  public static native int HALSendError(boolean isError, int errorCode, boolean isLVCode, String details, String location, String callStack, boolean printMsg);
 }

wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java

@@ -6,7 +6,8 @@
 package edu.wpi.first.wpilibj;
 
 import java.util.TimerTask;
-import java.util.LinkedList;
+import java.util.ArrayDeque;
+import java.util.Queue;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
@@ -42,7 +43,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   private Tolerance m_tolerance; // the tolerance object used to check if on
                                  // target
   private int m_bufLength = 1;
-  private LinkedList<Double> m_buf;
+  private Queue<Double> m_buf;
   private double m_bufTotal = 0.0;
   private double m_setpoint = 0.0;
   private double m_prevSetpoint = 0.0;
@@ -162,7 +163,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     HLUsageReporting.reportPIDController(instances);
     m_tolerance = new NullTolerance();
 
-    m_buf = new LinkedList<Double>();
+    m_buf = new ArrayDeque<Double>(m_bufLength+1);
   }
 
   /**
@@ -311,11 +312,11 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         result = m_result;
 
         // Update the buffer.
-        m_buf.push(m_error);
+        m_buf.add(m_error);
         m_bufTotal += m_error;
         // Remove old elements when the buffer is full.
         if (m_buf.size() > m_bufLength) {
-          m_bufTotal -= m_buf.pop();
+          m_bufTotal -= m_buf.remove();
         }
       }
 
@@ -646,7 +647,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     // Cut the existing buffer down to size if needed.
     while (m_buf.size() > bufLength) {
-      m_bufTotal -= m_buf.pop();
+      m_bufTotal -= m_buf.remove();
     }
   }
 

wpilibj/src/sim/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java

@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * The MotorSafetyHelper object is constructed for every object that wants to
+ * implement the Motor Safety protocol. The helper object has the code to
+ * actually do the timing and call the motors Stop() method when the timeout
+ * expires. The motor object is expected to call the Feed() method whenever the
+ * motors value is updated.
+ *
+ * @author brad
+ */
+public class MotorSafetyHelper {
+
+  double m_expiration;
+  boolean m_enabled;
+  double m_stopTime;
+  MotorSafety m_safeObject;
+  MotorSafetyHelper m_nextHelper;
+  static MotorSafetyHelper m_headHelper = null;
+
+  /**
+   * The constructor for a MotorSafetyHelper object. The helper object is
+   * constructed for every object that wants to implement the Motor Safety
+   * protocol. The helper object has the code to actually do the timing and call
+   * the motors Stop() method when the timeout expires. The motor object is
+   * expected to call the Feed() method whenever the motors value is updated.
+   *
+   * @param safeObject a pointer to the motor object implementing MotorSafety.
+   *        This is used to call the Stop() method on the motor.
+   */
+  public MotorSafetyHelper(MotorSafety safeObject) {
+    m_safeObject = safeObject;
+    m_enabled = false;
+    m_expiration = MotorSafety.DEFAULT_SAFETY_EXPIRATION;
+    m_stopTime = Timer.getFPGATimestamp();
+    m_nextHelper = m_headHelper;
+    m_headHelper = this;
+  }
+
+  /**
+   * Feed the motor safety object. Resets the timer on this object that is used
+   * to do the timeouts.
+   */
+  public void feed() {
+    m_stopTime = Timer.getFPGATimestamp() + m_expiration;
+  }
+
+  /**
+   * Set the expiration time for the corresponding motor safety object.
+   *$
+   * @param expirationTime The timeout value in seconds.
+   */
+  public void setExpiration(double expirationTime) {
+    m_expiration = expirationTime;
+  }
+
+  /**
+   * Retrieve the timeout value for the corresponding motor safety object.
+   *$
+   * @return the timeout value in seconds.
+   */
+  public double getExpiration() {
+    return m_expiration;
+  }
+
+  /**
+   * Determine of the motor is still operating or has timed out.
+   *$
+   * @return a true value if the motor is still operating normally and hasn't
+   *         timed out.
+   */
+  public boolean isAlive() {
+    return !m_enabled || m_stopTime > Timer.getFPGATimestamp();
+  }
+
+  /**
+   * Check if this motor has exceeded its timeout. This method is called
+   * periodically to determine if this motor has exceeded its timeout value. If
+   * it has, the stop method is called, and the motor is shut down until its
+   * value is updated again.
+   */
+  public void check() {
+    if (!m_enabled || RobotState.isDisabled() || RobotState.isTest())
+      return;
+    if (m_stopTime < Timer.getFPGATimestamp()) {
+      DriverStation.reportError(m_safeObject.getDescription() + "... Output not updated often enough.", false);
+
+      m_safeObject.stopMotor();
+    }
+  }
+
+  /**
+   * Enable/disable motor safety for this device Turn on and off the motor
+   * safety option for this PWM object.
+   *$
+   * @param enabled True if motor safety is enforced for this object
+   */
+  public void setSafetyEnabled(boolean enabled) {
+    m_enabled = enabled;
+  }
+
+  /**
+   * Return the state of the motor safety enabled flag Return if the motor
+   * safety is currently enabled for this devicce.
+   *$
+   * @return True if motor safety is enforced for this device
+   */
+  public boolean isSafetyEnabled() {
+    return m_enabled;
+  }
+
+  /**
+   * Check the motors to see if any have timed out. This static method is called
+   * periodically to poll all the motors and stop any that have timed out.
+   */
+  // TODO: these should be synchronized with the setting methods in case it's
+  // called from a different thread
+  public static void checkMotors() {
+    for (MotorSafetyHelper msh = m_headHelper; msh != null; msh = msh.m_nextHelper) {
+      msh.check();
+    }
+  }
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CANTalonTest.java

@@ -131,4 +131,26 @@ public class CANTalonTest extends AbstractComsSetup {
     }
   }
 
+  //Test Get Fault methods (artf4814)
+  @Test
+  public void testGetFaults() {
+    CANTalon talon = new CANTalon(0);
+    talon.clearStickyFaults();
+
+    assertTrue(talon.getFaultOverTemp()==0);
+    assertTrue(talon.getFaultUnderVoltage()==0);
+    assertTrue(talon.getFaultForLim()==0);
+    assertTrue(talon.getFaultRevLim()==0);
+    assertTrue(talon.getFaultHardwareFailure()==0);
+    assertTrue(talon.getFaultForSoftLim()==0);
+    assertTrue(talon.getFaultRevSoftLim()==0);
+
+    assertTrue(talon.getStickyFaultOverTemp()==0);
+//    assertTrue(talon.getStickyFaultUnderVoltage()==0);
+    assertTrue(talon.getStickyFaultForLim()==0);
+    assertTrue(talon.getStickyFaultRevLim()==0);
+    assertTrue(talon.getStickyFaultForSoftLim()==0);
+    assertTrue(talon.getStickyFaultRevSoftLim()==0);
+  }
+
 }

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java

@@ -46,14 +46,14 @@ public class GyroTest extends AbstractComsSetup {
 
   @Test
   public void testAllGyroTests() {
-    testInitial();
-    testGyroAngle();
-    testDeviationOverTime();
+    testInitial(tpcam.getGyro());
+    testDeviationOverTime(tpcam.getGyro());
+    testGyroAngle(tpcam.getGyro());
     testGyroAngleCalibratedParameters();
   }
 
-  public void testInitial() {
-    double angle = tpcam.getGyro().getAngle();
+  public void testInitial(AnalogGyro gyro) {
+    double angle = gyro.getAngle();
     assertEquals(errorMessage(angle, 0), 0, angle, .5);
   }
 
@@ -63,7 +63,7 @@ public class GyroTest extends AbstractComsSetup {
    * for so setAngle is significantly off. This has been calibrated for the
    * servo on the rig.
    */
-  public void testGyroAngle() {
+  public void testGyroAngle(AnalogGyro gyro) {
     // Set angle
     for (int i = 0; i < 5; i++) {
       tpcam.getPan().set(0);
@@ -72,7 +72,7 @@ public class GyroTest extends AbstractComsSetup {
 
     Timer.delay(0.5);
     // Reset for setup
-    tpcam.getGyro().reset();
+    gyro.reset();
     Timer.delay(0.5);
 
     // Perform test
@@ -82,7 +82,7 @@ public class GyroTest extends AbstractComsSetup {
     }
     Timer.delay(1.2);
 
-    double angle = tpcam.getGyro().getAngle();
+    double angle = gyro.getAngle();
 
     double difference = TEST_ANGLE - angle;
 
@@ -91,16 +91,16 @@ public class GyroTest extends AbstractComsSetup {
     assertEquals(errorMessage(diff, TEST_ANGLE), TEST_ANGLE, angle, 10);
   }
 
-  public void testDeviationOverTime() {
+  public void testDeviationOverTime(AnalogGyro gyro) {
     // Make sure that the test isn't influenced by any previous motions.
     Timer.delay(0.5);
-    tpcam.getGyro().reset();
+    gyro.reset();
     Timer.delay(0.25);
-    double angle = tpcam.getGyro().getAngle();
+    double angle = gyro.getAngle();
     assertEquals(errorMessage(angle, 0), 0, angle, .5);
     Timer.delay(5);
-    angle = tpcam.getGyro().getAngle();
-    assertEquals("After 5 seconds " + errorMessage(angle, 0), 0, angle, 1);
+    angle = gyro.getAngle();
+    assertEquals("After 5 seconds " + errorMessage(angle, 0), 0, angle, 2.0);
   }
 
   /**
@@ -113,34 +113,11 @@ public class GyroTest extends AbstractComsSetup {
     int cCenter = tpcam.getGyro().getCenter();
     tpcam.freeGyro();
     tpcam.setupGyroParam(cCenter, cOffset);
-
+    Timer.delay(TiltPanCameraFixture.RESET_TIME);
     // Repeat tests
-    // Set angle
-    for (int i = 0; i < 5; i++) {
-      tpcam.getPan().set(0);
-      Timer.delay(.1);
-    }
-
-    Timer.delay(0.5);
-
-    // Reset for setup
-    tpcam.getGyroParam().reset();
-    Timer.delay(0.5);
-
-    // Perform test
-    for (int i = 0; i < 53; i++) {
-      tpcam.getPan().set(i / 100.0);
-      Timer.delay(0.05);
-    }
-    Timer.delay(1.2);
-
-    double angle = tpcam.getGyroParam().getAngle();
-
-    double difference = TEST_ANGLE - angle;
-
-    double diff = Math.abs(difference);
-
-    assertEquals(errorMessage(diff, TEST_ANGLE), TEST_ANGLE, angle, 10);
+    testInitial(tpcam.getGyroParam());
+    testDeviationOverTime(tpcam.getGyroParam());
+    testGyroAngle(tpcam.getGyroParam());
   }
 
   private String errorMessage(double difference, double target) {

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java

@@ -60,7 +60,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
   @Before
   public void setUp() {
     double initialSpeed = me.getMotor().get();
-    assertTrue(me.getType() + " Did not start with an initial speeed of 0 instead got: "
+    assertTrue(me.getType() + " Did not start with an initial speed of 0 instead got: "
         + initialSpeed, Math.abs(initialSpeed) < 0.001);
     me.setup();
 
@@ -101,7 +101,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
   public void testIncrement() {
     int startValue = me.getEncoder().get();
 
-    me.getMotor().set(.75);
+    me.getMotor().set(.2);
     Timer.delay(MOTOR_RUNTIME);
     int currentValue = me.getEncoder().get();
     assertTrue(me.getType() + " Encoder not incremented: start: " + startValue + "; current: "
@@ -117,7 +117,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
   public void testDecrement() {
     int startValue = me.getEncoder().get();
 
-    me.getMotor().set(-.75);
+    me.getMotor().set(-.2);
     Timer.delay(MOTOR_RUNTIME);
     int currentValue = me.getEncoder().get();
     assertTrue(me.getType() + " Encoder not decremented: start: " + startValue + "; current: "
@@ -170,18 +170,18 @@ public class MotorEncoderTest extends AbstractComsSetup {
   @Test
   public void testPositionPIDController() {
     me.getEncoder().setPIDSourceType(PIDSourceType.kDisplacement);
-    PIDController pid = new PIDController(0.003, 0.001, 0, me.getEncoder(), me.getMotor());
-    pid.setAbsoluteTolerance(50);
+    PIDController pid = new PIDController(0.001, 0.0005, 0, me.getEncoder(), me.getMotor());
+    pid.setAbsoluteTolerance(50.0);
     pid.setOutputRange(-0.2, 0.2);
-    pid.setSetpoint(2500);
+    pid.setSetpoint(1000);
 
     pid.enable();
     Timer.delay(10.0);
     pid.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(),
-        pid.onTarget());
+        "PID loop did not reach setpoint within 10 seconds. The average error was: " + pid.getAvgError() + 
+        "The current error was" + pid.getError(), pid.onTarget());
 
     pid.free();
   }
@@ -194,7 +194,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
     pid.setAbsoluteTolerance(200);
     pid.setToleranceBuffer(50);
     pid.setOutputRange(-0.3, 0.3);
-    pid.setSetpoint(2000);
+    pid.setSetpoint(600);
 
     pid.enable();
     Timer.delay(10.0);

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java

@@ -30,7 +30,7 @@ import java.util.logging.Logger;
 @RunWith(Parameterized.class)
 public class MotorInvertingTest extends AbstractComsSetup {
   static MotorEncoderFixture<?> fixture = null;
-  private static final double motorspeed = 0.35;
+  private static final double motorspeed = 0.2;
   private static final double delaytime = 0.3;
 
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java

@@ -65,9 +65,7 @@ public class PDPTest extends AbstractComsSetup {
   public static Collection<Object[]> generateData() {
     // logger.fine("Loading the MotorList");
     return Arrays.asList(new Object[][] {
-        {TestBench.getInstance().getTalonPair(), new Double(0.0)},
-        {TestBench.getInstance().getVictorPair(), new Double(0.0)},
-        {TestBench.getInstance().getJaguarPair(), new Double(0.0)}});
+        {TestBench.getInstance().getTalonPair(), new Double(0.0)}});
   }
 
   @After
@@ -97,7 +95,7 @@ public class PDPTest extends AbstractComsSetup {
 
     /* Set the motor to full forward */
     me.getMotor().set(1.0);
-    Timer.delay(0.25);
+    Timer.delay(2);
 
     /* The current should now be greater than the low current */
     assertThat("The driven current is not greater than the resting current.",

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java

@@ -46,7 +46,7 @@ public class PIDTest extends AbstractComsSetup {
   private NetworkTable table;
 
   private static final double absoluteTolerance = 50;
-  private static final double outputRange = 0.3;
+  private static final double outputRange = 0.25;
 
   private PIDController controller = null;
   private static MotorEncoderFixture me = null;
@@ -181,7 +181,7 @@ public class PIDTest extends AbstractComsSetup {
   public void testRotateToTarget() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 2500.0;
+    double setpoint = 1000.0;
     assertEquals(pidData() + "did not start at 0", 0, controller.get(), 0);
     controller.setSetpoint(setpoint);
     assertEquals(pidData() + "did not have an error of " + setpoint, setpoint,

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDToleranceTest.java

@@ -0,0 +1,88 @@
+package edu.wpi.first.wpilibj;
+
+import java.util.logging.Logger;
+
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+
+public class PIDToleranceTest extends AbstractComsSetup {
+	private static final Logger logger = Logger.getLogger(PIDToleranceTest.class.getName());
+	private PIDController pid;
+	private final double setPoint = 50.0;
+	private final double tolerance = 10.0;
+	private final double range = 200;
+	private class fakeInput implements PIDSource{
+		public double val;
+		public fakeInput(){
+			val = 0;
+		}
+		@Override
+		public PIDSourceType getPIDSourceType() {
+			return PIDSourceType.kDisplacement;
+		}
+
+		@Override
+		public double pidGet() {
+			return val;
+		}
+
+		@Override
+		public void setPIDSourceType(PIDSourceType arg0) {}
+	};
+	private fakeInput inp;
+	private PIDOutput out = new PIDOutput(){
+		@Override
+		public void pidWrite(double out) {
+		}
+		
+	};
+	@Override
+	protected Logger getClassLogger() {
+		return logger;
+	}
+	@Before
+	public void setUp() throws Exception{
+		inp = new fakeInput();
+		pid = new PIDController(0.05,0.0,0.0,inp,out);
+		pid.setInputRange(-range/2, range/2);
+	}
+	
+	@After
+	public void tearDown() throws Exception{
+		pid.free();
+		pid = null;
+	}
+	
+	@Test
+	public void testAbsoluteTolerance(){
+		pid.setAbsoluteTolerance(tolerance);
+		pid.setSetpoint(setPoint);
+		pid.enable();
+		Timer.delay(1);
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint+tolerance/2;
+		Timer.delay(1.0);
+		assertTrue("Error was not in tolerance when it should have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint + 10*tolerance;
+		Timer.delay(1.0);
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+	}
+	@Test
+	public void testPercentTolerance(){
+		pid.setPercentTolerance(tolerance);
+		pid.setSetpoint(setPoint);
+		pid.enable();
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint+(tolerance)/200*range;//half of percent tolerance away from setPoint
+		Timer.delay(1.0);
+		assertTrue("Error was not in tolerance when it should have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint + (tolerance)/50*range;//double percent tolerance away from setPoint
+		Timer.delay(1.0);
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+	}
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java

@@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite;
     DIOCrossConnectTest.class, EncoderTest.class, FilterNoiseTest.class,
     FilterOutputTest.class, GyroTest.class, MotorEncoderTest.class,
     MotorInvertingTest.class, PCMTest.class, PDPTest.class, PIDTest.class,
-    PreferencesTest.class, RelayCrossConnectTest.class, SampleTest.class,
-    TimerTest.class})
+    PIDToleranceTest.class, PreferencesTest.class, RelayCrossConnectTest.class, 
+    SampleTest.class, TimerTest.class})
 public class WpiLibJTestSuite extends AbstractTestSuite {
 }

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java

@@ -18,9 +18,9 @@ import java.util.logging.Logger;
  */
 public class CANJaguarInversionTest extends AbstractCANTest {
   private static final Logger logger = Logger.getLogger(CANJaguarInversionTest.class.getName());
-  private static final double motorVoltage = 5.0;
-  private static final double motorPercent = 0.5;
-  private static final double motorSpeed = 100;
+  private static final double motorVoltage = 2.0;
+  private static final double motorPercent = 0.1;
+  private static final double motorSpeed = 10;
   private static final double delayTime = 0.75;
   private static final double speedModeDelayTime = 2.0;
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java

@@ -37,7 +37,7 @@ public class CANPercentQuadEncoderModeTest extends AbstractCANTest {
   private static final Logger logger = Logger.getLogger(CANPercentQuadEncoderModeTest.class
       .getName());
   private static final double kStoppedValue = 0;
-  private static final double kRunningValue = 1;
+  private static final double kRunningValue = 0.3;
 
   /*
    * (non-Javadoc)

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java

@@ -31,7 +31,7 @@ public class CANSpeedQuadEncoderModeTest extends AbstractCANTest {
   /** The stopped value in rev/min */
   private static final double kStoppedValue = 0;
   /** The running value in rev/min */
-  private static final double kRunningValue = 200;
+  private static final double kRunningValue = 50;
 
   @Override
   protected Logger getClassLogger() {

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java

@@ -65,6 +65,7 @@ public abstract class TiltPanCameraFixture implements ITestFixture {
 
   @Override
   public boolean reset() {
+	if(gyro != null)
     gyro.reset();
     return true;
   }
@@ -101,8 +102,14 @@ public abstract class TiltPanCameraFixture implements ITestFixture {
     tilt = null;
     pan.free();
     pan = null;
+    if(gyro !=null){//in case not freed during gyro tests
+    	gyro.free();
+    	gyro = null;
+    }
+    if(gyroParam != null){//in case gyro tests failed before getting to this point
     gyroParam.free();
     gyroParam = null;
+    }
     return true;
   }
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java

@@ -68,7 +68,7 @@ public final class TestBench {
   /* PowerDistributionPanel channels */
   public static final int kJaguarPDPChannel = 6;
   public static final int kVictorPDPChannel = 8;
-  public static final int kTalonPDPChannel = 11;
+  public static final int kTalonPDPChannel = 10;
   public static final int kCANJaguarPDPChannel = 5;
 
   /* CAN ASSOICATED CHANNELS */