diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index 8dbf32d66f..1ffa2024c6 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -119,4 +119,5 @@ private: bool m_controlEnabled; ControlMode m_controlMode; + TalonControlMode m_sendMode; }; diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index 6815b5edd9..d8624097a3 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -20,9 +20,7 @@ CANTalon::CANTalon(int deviceNumber) , m_controlEnabled(true) , m_controlMode(kPercentVbus) { - // The control mode may already have been set; GetControlMode will reset - // m_controlMode to match the Talon. - GetControlMode(); + SetControlMode(m_controlMode); m_impl->SetProfileSlotSelect(m_profile); } @@ -91,7 +89,7 @@ void CANTalon::Set(float value, uint8_t syncGroup) { if(m_controlEnabled) { CTR_Code status; - switch(GetControlMode()) { + switch(m_controlMode) { case CANSpeedController::kPercentVbus: { m_impl->Set(value); @@ -122,6 +120,13 @@ void CANTalon::Set(float value, uint8_t syncGroup) if (status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } + + status = m_impl->SetModeSelect(m_sendMode); + + if (status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + } } @@ -975,28 +980,28 @@ void CANTalon::ConfigFaultTime(float faultTime) void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) { m_controlMode = mode; - TalonControlMode sendMode; switch (mode) { case kPercentVbus: - sendMode = kThrottle; + m_sendMode = kThrottle; break; case kCurrent: - sendMode = kCurrentMode; + m_sendMode = kCurrentMode; break; case kSpeed: - sendMode = kSpeedMode; + m_sendMode = kSpeedMode; break; case kPosition: - sendMode = kPositionMode; + m_sendMode = kPositionMode; break; case kVoltage: - sendMode = kVoltageMode; + m_sendMode = kVoltageMode; break; case kFollower: - sendMode = kFollowerMode; + m_sendMode = kFollowerMode; break; } - CTR_Code status = m_impl->SetModeSelect((int)sendMode); + // Keep the talon disabled until Set() is called. + CTR_Code status = m_impl->SetModeSelect((int)kDisabled); if(status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } @@ -1007,35 +1012,6 @@ void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) */ CANSpeedController::ControlMode CANTalon::GetControlMode() { - // Take the opportunity to check that the control mode is what we think it is. - int temp; - CTR_Code status = m_impl->GetModeSelect(temp); - if(status != CTR_OKAY) { - wpi_setErrorWithContext(status, getHALErrorMessage(status)); - } - switch ((TalonControlMode)temp) { - case kThrottle: - m_controlMode = kPercentVbus; - break; - case kCurrentMode: - m_controlMode = kCurrent; - break; - case kSpeedMode: - m_controlMode = kSpeed; - break; - case kPositionMode: - m_controlMode = kPosition; - break; - case kVoltageMode: - m_controlMode = kVoltage; - break; - case kFollowerMode: - m_controlMode = kFollower; - break; - case kDisabled: - m_controlEnabled = false; - break; - } return m_controlMode; } diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java index a275d2ef8f..4c0c1d34f6 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java @@ -103,10 +103,8 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { * @param outputValue The setpoint value, as described above. */ public void set(double outputValue) { - //System.out.println("Enabled: " + m_controlEnabled + " Mode: " + m_controlMode); - m_controlMode = ControlMode.PercentVbus; if (m_controlEnabled) { - switch (getControlMode()) { + switch (m_controlMode) { case PercentVbus: m_impl.Set(outputValue); break; @@ -127,8 +125,8 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { default: break; } + m_impl.SetModeSelect(m_controlMode.value); } - //System.out.println("Enabled: " + m_controlEnabled + " Mode: " + m_controlMode); } /** @@ -316,21 +314,13 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { } public ControlMode getControlMode() { - long tempp = CanTalonJNI.new_intp(); - m_impl.GetModeSelect(new SWIGTYPE_p_int(tempp, true)); - ControlMode mode = ControlMode.valueOf(CanTalonJNI.intp_value(tempp)); - if (mode == ControlMode.Disabled) { - m_controlEnabled = false; - } - else { - m_controlMode = mode; - } - return mode; + return m_controlMode; } public void changeControlMode(ControlMode controlMode) { m_controlMode = controlMode; - m_impl.SetModeSelect(controlMode.value); + // Disable until set() is called. + m_impl.SetModeSelect(ControlMode.Disabled.value); } public void setFeedbackDevice(FeedbackDevice device) {