Add an additional member variable for "stopped" which indicates the CAN motor controller has been explicitly stopped, but not disabled by the user (main use case is MotorSafety tripping). When Set() is called the next time the controller will be re-enabled automatically.

Change-Id: Ib1c0e5d0ddd55343d83039acbc46c0f9c4e451a1

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/CANTalon.h

@@ -437,6 +437,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/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;
@@ -1825,12 +1831,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) {

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

@@ -375,6 +375,15 @@ 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 +421,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 +1896,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 {
@@ -2247,6 +2254,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController {
   @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,11 @@ 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) {
@@ -1215,6 +1221,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, CANSpeedCont
   @Deprecated
   public void stopMotor() {
     disableControl();
+	m_stopped = true;
   }
 
   @Override