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.

Additional small formatting corrections to the previous commit starting
this were added.

Change-Id: Ie94565394927a910ce74bc628670ac3d658d8df9

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/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

@@ -247,10 +247,10 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) {
   uint8_t dataSize;
 
   if (m_safetyHelper) m_safetyHelper->Feed();
-  
+
   if (m_stopped) {
     EnableControl();
-	m_stopped = false;
+    m_stopped = false;
   }
 
   if (m_controlEnabled) {

wpilibc/Athena/src/CANTalon.cpp

@@ -134,12 +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;
+    m_stopped = false;
   }
-  
+
   if (m_controlEnabled) {
     m_setPoint = value;  /* cache set point for GetSetpoint() */
     CTR_Code status = CTR_OKAY;
@@ -1898,7 +1898,7 @@ bool CANTalon::GetInverted() const { return m_isInverted; }
  *
  * @deprecated Call Disable instead.
 */
-void CANTalon::StopMotor() { 
+void CANTalon::StopMotor() {
 	Disable();
 	m_stopped = true;
 }

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/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/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(); }

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

@@ -375,15 +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_safetyHelper != null)
+      m_safetyHelper.feed();
+
+    if (m_stopped) {
+      enableControl();
+      m_stopped = false;
+    }
+
     if (m_controlEnabled) {
       switch (m_controlMode) {
         case PercentVbus:
@@ -2251,10 +2250,9 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController {
    * @deprecated Use disableControl instead.
    */
   @Override
-  @Deprecated
   public void stopMotor() {
     disableControl();
-	m_stopped = true;
+    m_stopped = true;
   }
 
   /*

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

@@ -424,11 +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_stopped) {
+      enableControl();
+      m_stopped = false;
+    }
     if (m_controlEnabled) {
       m_setPoint = outputValue; /* cache set point for getSetpoint() */
       switch (m_controlMode) {
@@ -1218,10 +1217,9 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont
    * @deprecated Use disableControl instead.
    */
   @Override
-  @Deprecated
   public void stopMotor() {
     disableControl();
-	m_stopped = true;
+    m_stopped = true;
   }
 
   @Override

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/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/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();
+    }
+  }
+}