diff --git a/wpilibc/Athena/include/CANJaguar.h b/wpilibc/Athena/include/CANJaguar.h index 03637df147..c456e2aa69 100644 --- a/wpilibc/Athena/include/CANJaguar.h +++ b/wpilibc/Athena/include/CANJaguar.h @@ -230,6 +230,7 @@ class CANJaguar : public MotorSafety, mutable std::atomic m_receivedStatusMessage2{false}; bool m_controlEnabled = false; + bool m_stopped = false; void verify(); diff --git a/wpilibc/Athena/include/CANTalon.h b/wpilibc/Athena/include/CANTalon.h index b773c639be..2d866d197b 100644 --- a/wpilibc/Athena/include/CANTalon.h +++ b/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; diff --git a/wpilibc/Athena/src/CANJaguar.cpp b/wpilibc/Athena/src/CANJaguar.cpp index 2d63fc9a85..c267cd5aae 100644 --- a/wpilibc/Athena/src/CANJaguar.cpp +++ b/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. diff --git a/wpilibc/Athena/src/CANTalon.cpp b/wpilibc/Athena/src/CANTalon.cpp index d553fdd449..2198eda08d 100644 --- a/wpilibc/Athena/src/CANTalon.cpp +++ b/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 value, bool isNew) { diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java index c0ad46dad1..9b013c8456 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/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; } /* diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java index 73acefd88a..09bc46f0f1 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANTalon.java +++ b/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