diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index ab45adb3e9..85552f8e83 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -139,4 +139,11 @@ private: double m_setPoint; static const unsigned int kDelayForSolicitedSignalsUs = 4000; + /** + * Fixup the sendMode so Set() serializes the correct demand value. + * Also fills the modeSelecet in the control frame to disabled. + * @param mode Control mode to ultimately enter once user calls Set(). + * @see Set() + */ + void ApplyControlMode(CANSpeedController::ControlMode mode); }; diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index d023234d50..907587eba4 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -21,7 +21,7 @@ CANTalon::CANTalon(int deviceNumber) , m_controlMode(kPercentVbus) , m_setPoint(0) { - SetControlMode(m_controlMode); + ApplyControlMode(m_controlMode); m_impl->SetProfileSlotSelect(m_profile); } /** @@ -39,7 +39,7 @@ CANTalon::CANTalon(int deviceNumber,int controlPeriodMs) , m_controlMode(kPercentVbus) , m_setPoint(0) { - SetControlMode(m_controlMode); + ApplyControlMode(m_controlMode); m_impl->SetProfileSlotSelect(m_profile); } CANTalon::~CANTalon() { @@ -1100,9 +1100,12 @@ void CANTalon::ConfigFaultTime(float faultTime) } /** - * TODO documentation (see CANJaguar.cpp) + * Fixup the sendMode so Set() serializes the correct demand value. + * Also fills the modeSelecet in the control frame to disabled. + * @param mode Control mode to ultimately enter once user calls Set(). + * @see Set() */ -void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) +void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode) { m_controlMode = mode; switch (mode) { @@ -1131,6 +1134,17 @@ void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } +/** + * TODO documentation (see CANJaguar.cpp) + */ +void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) +{ + if(m_controlMode == mode){ + /* we already are in this mode, don't perform disable workaround */ + }else{ + ApplyControlMode(mode); + } +} /** * TODO documentation (see CANJaguar.cpp) 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 baf76b4050..5b74a8d85f 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 @@ -91,7 +91,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { m_profile = 0; m_setPoint = 0; setProfile(m_profile); - changeControlMode(ControlMode.PercentVbus); + applyControlMode(ControlMode.PercentVbus); } public CANTalon(int deviceNumber,int controlPeriodMs) { m_deviceNumber = deviceNumber; @@ -101,7 +101,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { m_profile = 0; m_setPoint = 0; setProfile(m_profile); - changeControlMode(ControlMode.PercentVbus); + applyControlMode(ControlMode.PercentVbus); } public CANTalon(int deviceNumber,int controlPeriodMs) { m_deviceNumber = deviceNumber; @@ -425,14 +425,26 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { public ControlMode getControlMode() { return m_controlMode; } - - public void changeControlMode(ControlMode controlMode) { + /** + * Fixup the m_controlMode so set() serializes the correct demand value. + * Also fills the modeSelecet in the control frame to disabled. + * @param controlMode Control mode to ultimately enter once user calls set(). + * @see #set + */ + public void applyControlMode(ControlMode controlMode) { m_controlMode = controlMode; if (controlMode == ControlMode.Disabled) m_controlEnabled = false; // Disable until set() is called. m_impl.SetModeSelect(ControlMode.Disabled.value); } + public void changeControlMode(ControlMode controlMode) { + if(m_controlMode == controlMode){ + /* we already are in this mode, don't perform disable workaround */ + }else{ + applyControlMode(controlMode); + } + } public void setFeedbackDevice(FeedbackDevice device) { m_impl.SetFeedbackDeviceSelect(device.value);