diff --git a/hal/lib/Athena/ctre/CanTalonSRX.cpp b/hal/lib/Athena/ctre/CanTalonSRX.cpp index 3c91eeff17..2f2266275c 100644 --- a/hal/lib/Athena/ctre/CanTalonSRX.cpp +++ b/hal/lib/Athena/ctre/CanTalonSRX.cpp @@ -571,6 +571,11 @@ CTR_Code CanTalonSRX::GetReverseSoftEnable(int & enable) CTR_Code CanTalonSRX::SetStatusFrameRate(unsigned frameEnum, unsigned periodMs) { int32_t status = 0; + /* bounds check the period */ + if(periodMs < 1) + periodMs = 1; + else if (periodMs > 255) + periodMs = 255; uint8_t period = (uint8_t)periodMs; /* tweak just the status messsage rate the caller cares about */ switch(frameEnum){ diff --git a/wpilibc/wpilibC++Devices/include/CANSpeedController.h b/wpilibc/wpilibC++Devices/include/CANSpeedController.h index 36c03928e8..16d3088d10 100644 --- a/wpilibc/wpilibC++Devices/include/CANSpeedController.h +++ b/wpilibc/wpilibC++Devices/include/CANSpeedController.h @@ -54,7 +54,10 @@ public: /** Only use switches for limits */ kLimitMode_SwitchInputsOnly = 0, /** Use both switches and soft limits */ - kLimitMode_SoftPositionLimits = 1 + kLimitMode_SoftPositionLimits = 1, + /* SRX extensions */ + /** Disable switches and disable soft limits */ + kLimitMode_SrxDisableSwitchInputs = 2, }; virtual float Get() = 0; diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index b1322eadcb..ab45adb3e9 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -26,6 +26,12 @@ public: AnalogEncoder=3, EncRising=4, EncFalling=5 + }; + enum StatusFrameRate { + StatusFrameRateGeneral=0, + StatusFrameRateFeedback=1, + StatusFrameRateQuadEncoder=2, + StatusFrameRateAnalogTempVbat=3, }; explicit CANTalon(int deviceNumber); explicit CANTalon(int deviceNumber,int controlPeriodMs); @@ -68,6 +74,7 @@ public: virtual double GetSpeed() override; virtual int GetClosedLoopError(); virtual int GetAnalogIn(); + virtual int GetAnalogInRaw(); virtual int GetAnalogInVel(); virtual int GetEncPosition(); virtual int GetEncVel(); @@ -97,6 +104,7 @@ public: virtual void ConfigFaultTime(float faultTime) override; virtual void SetControlMode(ControlMode mode); void SetFeedbackDevice(FeedbackDevice device); + void SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs); virtual ControlMode GetControlMode(); void SetSensorDirection(bool reverseSensor); void SetCloseLoopRampRate(double rampRate); @@ -104,6 +112,7 @@ public: int GetIzone(); int GetIaccum(); void ClearIaccum(); + int GetBrakeEnableDuringNeutral(); bool IsControlEnabled(); double GetSetpoint(); diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index c49df38369..d023234d50 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -273,6 +273,16 @@ void CANTalon::SetFeedbackDevice(FeedbackDevice device) wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } +/** + * Select the feedback device to use in closed-loop + */ +void CANTalon::SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs) +{ + CTR_Code status = m_impl->SetStatusFrameRate((int)stateFrame,periodMs); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } +} /** * TODO documentation (see CANJaguar.cpp) @@ -529,7 +539,9 @@ double CANTalon::GetSpeed() * Get the position of whatever is in the analog pin of the Talon, regardless of * whether it is actually being used for feedback. * - * @returns The value (0 - 1023) on the analog pin of the Talon. + * @returns The 24bit analog value. The bottom ten bits is the ADC (0 - 1023) on + * the analog pin of the Talon. The upper 14 bits + * tracks the overflows and underflows (continuous sensor). */ int CANTalon::GetAnalogIn() { @@ -540,7 +552,16 @@ int CANTalon::GetAnalogIn() } return position; } - +/** + * Get the position of whatever is in the analog pin of the Talon, regardless of + * whether it is actually being used for feedback. + * + * @returns The ADC (0 - 1023) on analog pin of the Talon. + */ +int CANTalon::GetAnalogInRaw() +{ + return GetAnalogIn() & 0x3FF; +} /** * Get the position of whatever is in the analog pin of the Talon, regardless of * whether it is actually being used for feedback. @@ -929,7 +950,18 @@ void CANTalon::ConfigNeutralMode(NeutralMode mode) wpi_setErrorWithContext(status, getHALErrorMessage(status)); } } - +/** + * @return nonzero if brake is enabled during neutral. Zero if coast is enabled during neutral. + */ +int CANTalon::GetBrakeEnableDuringNeutral() +{ + int brakeEn = 0; + CTR_Code status = m_impl->GetBrakeIsEnabled(brakeEn); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + return brakeEn; +} /** * TODO documentation (see CANJaguar.cpp) */ @@ -1005,6 +1037,23 @@ void CANTalon::ConfigLimitMode(LimitMode mode) wpi_setErrorWithContext(status, getHALErrorMessage(status)); } break; + + case kLimitMode_SrxDisableSwitchInputs: /** disable both limit switches and soft limits */ + /* turn on both limits. SRX has individual enables and polarity for each limit switch.*/ + status = m_impl->SetForwardSoftEnable(false); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + status = m_impl->SetReverseSoftEnable(false); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + /* override enable the limit switches, this circumvents the webdash */ + status = m_impl->SetOverrideLimitSwitchEn(CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + break; } } 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 67651b94cb..baf76b4050 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 @@ -55,6 +55,23 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { this.value = value; } } + /** enumerated types for frame rate ms */ + public enum StatusFrameRate { + General(0), Feedback(1), QuadEncoder(2), AnalogTempVbat(3); + public int value; + public static StatusFrameRate valueOf(int value) { + for(StatusFrameRate mode : values()) { + if(mode.value == value) { + return mode; + } + } + return null; + } + private StatusFrameRate(int value) { + this.value = value; + } + } + private CanTalonSRX m_impl; ControlMode m_controlMode; @@ -243,11 +260,52 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { return CanTalonJNI.intp_value(valuep); } + /** + * Get the number of of rising edges seen on the index pin. + * + * @return number of rising edges on idx pin. + */ + public int getNumberOfQuadIdxRises() { + long valuep = CanTalonJNI.new_intp(); + SWIGTYPE_p_int swigp = new SWIGTYPE_p_int(valuep, true); + m_impl.GetEncIndexRiseEvents(swigp); + return CanTalonJNI.intp_value(valuep); + } + /** + * @return IO level of QUADA pin. + */ + public int getPinStateQuadA(){ + long valuep = CanTalonJNI.new_intp(); + SWIGTYPE_p_int swigp = new SWIGTYPE_p_int(valuep, true); + m_impl.GetQuadApin(swigp); + return CanTalonJNI.intp_value(valuep); + } + /** + * @return IO level of QUADB pin. + */ + public int getPinStateQuadB() { + long valuep = CanTalonJNI.new_intp(); + SWIGTYPE_p_int swigp = new SWIGTYPE_p_int(valuep, true); + m_impl.GetQuadBpin(swigp); + return CanTalonJNI.intp_value(valuep); + } + /** + * @return IO level of QUAD Index pin. + */ + public int getPinStateQuadIdx(){ + long valuep = CanTalonJNI.new_intp(); + SWIGTYPE_p_int swigp = new SWIGTYPE_p_int(valuep, true); + m_impl.GetQuadIdxpin(swigp); + return CanTalonJNI.intp_value(valuep); + } + /** * Get the current analog in position, regardless of whether it is the current * feedback device. * - * @return The current value from the analog in (0 - 1023). + * @return The 24bit analog position. The bottom ten bits is the ADC (0 - 1023) on + * the analog pin of the Talon. The upper 14 bits + * tracks the overflows and underflows (continuous sensor). */ public int getAnalogInPosition() { long valuep = CanTalonJNI.new_intp(); @@ -255,7 +313,14 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { m_impl.GetAnalogInWithOv(swigp); return CanTalonJNI.intp_value(valuep); } - + /** + * Get the current analog in position, regardless of whether it is the current + * feedback device. + * @return The ADC (0 - 1023) on analog pin of the Talon. + */ + public int getAnalogInRaw() { + return getAnalogInPosition() & 0x3FF; + } /** * Get the current encoder velocity, regardless of whether it is the current * feedback device. @@ -280,7 +345,27 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { m_impl.GetCloseLoopErr(swigp); return CanTalonJNI.intp_value(valuep); } - + // Returns true if limit switch is closed. false if open. + public boolean isFwdLimitSwitchClosed() { + long valuep = CanTalonJNI.new_intp(); + SWIGTYPE_p_int swigp = new SWIGTYPE_p_int(valuep, true); + m_impl.GetLimitSwitchClosedFor(swigp); + return (CanTalonJNI.intp_value(valuep)==0) ? true : false; + } + // Returns true if limit switch is closed. false if open. + public boolean isRevLimitSwitchClosed() { + long valuep = CanTalonJNI.new_intp(); + SWIGTYPE_p_int swigp = new SWIGTYPE_p_int(valuep, true); + m_impl.GetLimitSwitchClosedRev(swigp); + return (CanTalonJNI.intp_value(valuep)==0) ? true : false; + } + // Returns true if break is enabled during neutral. false if coast. + public boolean getBrakeEnableDuringNeutral() { + long valuep = CanTalonJNI.new_intp(); + SWIGTYPE_p_int swigp = new SWIGTYPE_p_int(valuep, true); + m_impl.GetBrakeIsEnabled(swigp); + return (CanTalonJNI.intp_value(valuep)==0) ? false : true; + } // Returns temperature of Talon, in degrees Celsius. public double getTemp() { long tempp = CanTalonJNI.new_doublep(); // Create a new swig pointer. @@ -352,7 +437,9 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { public void setFeedbackDevice(FeedbackDevice device) { m_impl.SetFeedbackDeviceSelect(device.value); } - + public void setStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs){ + m_impl.SetStatusFrameRate(stateFrame.value,periodMs); + } public void enableControl() { changeControlMode(m_controlMode); m_controlEnabled = true; @@ -522,7 +609,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { m_impl.GetParamResponseInt32(CanTalonSRX.param_t.ePidIaccum, new SWIGTYPE_p_int(fp, true)); return CanTalonJNI.intp_value(fp); } - + /** * Clear the accumulator for I gain. */ @@ -690,7 +777,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { public void setForwardSoftLimit(int forwardLimit) { m_impl.SetForwardSoftLimit(forwardLimit); } - + public void enableForwardSoftLimit(boolean enable) { m_impl.SetForwardSoftEnable(enable ? 1 : 0); } @@ -698,7 +785,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { public void setReverseSoftLimit(int reverseLimit) { m_impl.SetReverseSoftLimit(reverseLimit); } - + public void enableReverseSoftLimit(boolean enable) { m_impl.SetReverseSoftEnable(enable ? 1 : 0); }