diff --git a/wpilibc/wpilibC++/include/CANJaguar.h b/wpilibc/wpilibC++/include/CANJaguar.h index 78e20503c7..38e0e4b253 100644 --- a/wpilibc/wpilibC++/include/CANJaguar.h +++ b/wpilibc/wpilibC++/include/CANJaguar.h @@ -227,6 +227,8 @@ protected: bool m_receivedStatusMessage1; bool m_receivedStatusMessage2; + bool m_controlEnabled; + void verify(); MotorSafetyHelper *m_safetyHelper; diff --git a/wpilibc/wpilibC++/lib/CANJaguar.cpp b/wpilibc/wpilibC++/lib/CANJaguar.cpp index fee23da553..a0377e3f7a 100644 --- a/wpilibc/wpilibC++/lib/CANJaguar.cpp +++ b/wpilibc/wpilibC++/lib/CANJaguar.cpp @@ -294,57 +294,60 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) uint8_t dataBuffer[8]; uint8_t dataSize; - if (m_safetyHelper && !m_safetyHelper->IsAlive()) + if(m_safetyHelper && !m_safetyHelper->IsAlive() && m_controlEnabled) { EnableControl(); } - switch(m_controlMode) + if(m_controlEnabled) { - case kPercentVbus: + switch(m_controlMode) { - messageID = LM_API_VOLT_T_SET; - if (outputValue > 1.0) outputValue = 1.0; - if (outputValue < -1.0) outputValue = -1.0; - dataSize = packPercentage(dataBuffer, outputValue); + case kPercentVbus: + { + messageID = LM_API_VOLT_T_SET; + if (outputValue > 1.0) outputValue = 1.0; + if (outputValue < -1.0) outputValue = -1.0; + dataSize = packPercentage(dataBuffer, outputValue); + } + break; + case kSpeed: + { + messageID = LM_API_SPD_T_SET; + dataSize = packFXP16_16(dataBuffer, outputValue); + } + break; + case kPosition: + { + messageID = LM_API_POS_T_SET; + dataSize = packFXP16_16(dataBuffer, outputValue); + } + break; + case kCurrent: + { + messageID = LM_API_ICTRL_T_SET; + dataSize = packFXP8_8(dataBuffer, outputValue); + } + break; + case kVoltage: + { + messageID = LM_API_VCOMP_T_SET; + dataSize = packFXP8_8(dataBuffer, outputValue); + } + break; + default: + return; } - break; - case kSpeed: + if (syncGroup != 0) { - messageID = LM_API_SPD_T_SET; - dataSize = packFXP16_16(dataBuffer, outputValue); + dataBuffer[dataSize] = syncGroup; + dataSize++; } - break; - case kPosition: - { - messageID = LM_API_POS_T_SET; - dataSize = packFXP16_16(dataBuffer, outputValue); - } - break; - case kCurrent: - { - messageID = LM_API_ICTRL_T_SET; - dataSize = packFXP8_8(dataBuffer, outputValue); - } - break; - case kVoltage: - { - messageID = LM_API_VCOMP_T_SET; - dataSize = packFXP8_8(dataBuffer, outputValue); - } - break; - default: - return; - } - if (syncGroup != 0) - { - dataBuffer[dataSize] = syncGroup; - dataSize++; - } - sendMessage(messageID, dataBuffer, dataSize, kSendMessagePeriod); + sendMessage(messageID, dataBuffer, dataSize, kSendMessagePeriod); - if (m_safetyHelper) m_safetyHelper->Feed(); + if (m_safetyHelper) m_safetyHelper->Feed(); + } m_value = outputValue; @@ -682,7 +685,7 @@ void CANJaguar::verify() } // Verify that any recently set parameters are correct - if(!m_controlModeVerified) + if(!m_controlModeVerified && m_controlEnabled) { if(getMessage(LM_API_STATUS_CMODE, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { @@ -1305,6 +1308,7 @@ void CANJaguar::EnableControl(double encoderInitialPosition) break; } + m_controlEnabled = true; m_controlModeVerified = false; } @@ -1332,10 +1336,12 @@ void CANJaguar::DisableControl() case kCurrent: sendMessage(LM_API_ICTRL_DIS, dataBuffer, dataSize); break; -case kVoltage: + case kVoltage: sendMessage(LM_API_VCOMP_DIS, dataBuffer, dataSize); break; } + + m_controlEnabled = false; } /** diff --git a/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp index a7bfcb9cb9..f59ec78a57 100644 --- a/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp +++ b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp @@ -134,6 +134,25 @@ TEST_F(CANJaguarTest, InitialStatus) { << "Jaguar has one or more fault set."; } +/** + * Ensure that the jaguar doesn't move when it's disabled + */ +TEST_F(CANJaguarTest, Disable) { + m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); + m_jaguar->EnableControl(); + m_jaguar->DisableControl(); + + double initialPosition = m_jaguar->GetPosition(); + + SetJaguar(kMotorTime, 1.0f); + m_jaguar->Set(0.0f); + + double finalPosition = m_jaguar->GetPosition(); + + EXPECT_NEAR(initialPosition, finalPosition, kEncoderPositionTolerance) + << "Jaguar moved while disabled"; +} + /** * Make sure the Jaguar keeps its state after a power cycle by setting a * control mode, turning the spike on and off, then checking if the Jaguar diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java index f4290ecd77..aac31d8b38 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java @@ -301,45 +301,47 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW byte[] data = new byte[8]; byte dataSize; - switch(m_controlMode) { - case PercentVbus: - messageID = CANJNI.LM_API_VOLT_T_SET; - dataSize = packPercentage(data, outputValue); - break; + if(m_controlEnabled) { + switch(m_controlMode) { + case PercentVbus: + messageID = CANJNI.LM_API_VOLT_T_SET; + dataSize = packPercentage(data, outputValue); + break; - case Speed: - messageID = CANJNI.LM_API_SPD_T_SET; - dataSize = packFXP16_16(data, outputValue); - break; + case Speed: + messageID = CANJNI.LM_API_SPD_T_SET; + dataSize = packFXP16_16(data, outputValue); + break; - case Position: - messageID = CANJNI.LM_API_POS_T_SET; - dataSize = packFXP16_16(data, outputValue); - break; + case Position: + messageID = CANJNI.LM_API_POS_T_SET; + dataSize = packFXP16_16(data, outputValue); + break; - case Current: - messageID = CANJNI.LM_API_ICTRL_T_SET; - dataSize = packFXP8_8(data, outputValue); - break; + case Current: + messageID = CANJNI.LM_API_ICTRL_T_SET; + dataSize = packFXP8_8(data, outputValue); + break; - case Voltage: - messageID = CANJNI.LM_API_VCOMP_T_SET; - dataSize = packFXP8_8(data, outputValue); - break; + case Voltage: + messageID = CANJNI.LM_API_VCOMP_T_SET; + dataSize = packFXP8_8(data, outputValue); + break; - default: - return; + default: + return; + } + + if(syncGroup != 0) { + data[dataSize++] = syncGroup; + } + + sendMessage(messageID, data, dataSize, kSendMessagePeriod); + + if(m_safetyHelper != null) m_safetyHelper.feed(); } - if(syncGroup != 0) { - data[dataSize++] = syncGroup; - } - - sendMessage(messageID, data, dataSize, kSendMessagePeriod); - - if(m_safetyHelper != null) m_safetyHelper.feed(); - m_value = outputValue; verify(); @@ -429,15 +431,13 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW getMessage(message, CANJNI.CAN_MSGID_FULL_M, data); } catch (CANMessageNotFoundException e) {} } - - enableControl(); } } catch(CANMessageNotFoundException e) { requestMessage(CANJNI.LM_API_STATUS_POWER); } // Verify that any recently set parameters are correct - if(!m_controlModeVerified) { + if(!m_controlModeVerified && m_controlEnabled) { try { getMessage(CANJNI.LM_API_STATUS_CMODE, CANJNI.CAN_MSGID_FULL_M, data); @@ -1012,6 +1012,8 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW sendMessage(CANJNI.LM_API_VCOMP_T_EN, new byte[0], 0); break; } + + m_controlEnabled = true; } /** @@ -1052,6 +1054,8 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW sendMessage(CANJNI.LM_API_VCOMP_DIS, new byte[0], 0); break; } + + m_controlEnabled = false; } /** @@ -1731,6 +1735,8 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW static final int kReceiveStatusAttempts = 50; + boolean m_controlEnabled = true; + static void sendMessageHelper(int messageID, byte[] data, int dataSize, int period) throws CANMessageNotFoundException { final int[] kTrustedMessages = { CANJNI.LM_API_VOLT_T_EN, CANJNI.LM_API_VOLT_T_SET, CANJNI.LM_API_SPD_T_EN, CANJNI.LM_API_SPD_T_SET,