diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index 0aacae8b82..467b758d29 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -102,6 +102,9 @@ public: double GetIzone(); int GetIaccum(); void ClearIaccum(); + + bool IsControlEnabled(); + double GetSetpoint(); private: // Values for various modes as is sent in the CAN packets for the Talon. enum TalonControlMode { @@ -122,4 +125,6 @@ private: bool m_controlEnabled; ControlMode m_controlMode; TalonControlMode m_sendMode; + + double m_setPoint; }; diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index 4306f17ba6..cf09f95e69 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -19,6 +19,7 @@ CANTalon::CANTalon(int deviceNumber) , m_profile(0) , m_controlEnabled(true) , m_controlMode(kPercentVbus) + , m_setPoint(0) { SetControlMode(m_controlMode); m_impl->SetProfileSlotSelect(m_profile); @@ -88,6 +89,7 @@ float CANTalon::Get() void CANTalon::Set(float value, uint8_t syncGroup) { if(m_controlEnabled) { + m_setPoint = value; CTR_Code status; switch(m_controlMode) { case CANSpeedController::kPercentVbus: @@ -148,6 +150,13 @@ void CANTalon::EnableControl() { m_controlEnabled = true; } +/** + * @return Whether the Talon is currently enabled. + */ +bool CANTalon::IsControlEnabled() { + return m_controlEnabled; +} + /** * @param p Proportional constant to use in PID loop. * @see SelectProfileSlot to choose between the two sets of gains. @@ -341,6 +350,13 @@ double CANTalon::GetIzone() return (double)iz; } +/** + * @return the current setpoint; ie, whatever was last passed to Set(). + */ +double CANTalon::GetSetpoint() { + return m_setPoint; +} + /** * Returns the voltage coming in from the battery. * 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 95f13c1460..276747dcb9 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 @@ -64,12 +64,15 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { boolean m_controlEnabled; int m_profile; + double m_setPoint; + public CANTalon(int deviceNumber) { m_deviceNumber = deviceNumber; m_impl = new CanTalonSRX(deviceNumber); m_safetyHelper = new MotorSafetyHelper(this); m_controlEnabled = true; m_profile = 0; + m_setPoint = 0; setProfile(m_profile); changeControlMode(ControlMode.PercentVbus); } @@ -104,6 +107,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { */ public void set(double outputValue) { if (m_controlEnabled) { + m_setPoint = outputValue; switch (m_controlMode) { case PercentVbus: m_impl.Set(outputValue); @@ -319,6 +323,8 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { public void changeControlMode(ControlMode controlMode) { m_controlMode = controlMode; + if (controlMode == ControlMode.Disabled) + m_controlEnabled = false; // Disable until set() is called. m_impl.SetModeSelect(ControlMode.Disabled.value); } @@ -337,6 +343,10 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { m_controlEnabled = false; } + public boolean isControlEnabled() { + return m_controlEnabled; + } + /** * Get the current proportional constant. * @@ -604,10 +614,18 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { setIZone(izone); setCloseLoopRampRate(ramprate); } + public void setPID(double p, double i, double d) { setPID(p, i, d, 0, 0, 0, m_profile); } + /** + * @return The latest value set using set(). + */ + public double getSetpoint() { + return m_setPoint; + } + /** * Select which closed loop profile to use, and uses whatever PIDF gains and * the such that are already there.