From d567bd0bca88b7e3f376d4e2f460a42a6f6c4dd7 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 8 Feb 2016 18:13:57 -0500 Subject: [PATCH] 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 | 1 + wpilibc/Athena/include/CANTalon.h | 1 + wpilibc/Athena/src/CANJaguar.cpp | 13 ++++++++----- wpilibc/Athena/src/CANTalon.cpp | 13 +++++++++++-- .../java/edu/wpi/first/wpilibj/CANJaguar.java | 14 +++++++++++--- .../java/edu/wpi/first/wpilibj/CANTalon.java | 7 +++++++ 6 files changed, 39 insertions(+), 10 deletions(-) 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