diff --git a/hal/lib/Athena/ctre/CanTalonSRX.h b/hal/lib/Athena/ctre/CanTalonSRX.h index 749d5055d3..da18f4fc92 100644 --- a/hal/lib/Athena/ctre/CanTalonSRX.h +++ b/hal/lib/Athena/ctre/CanTalonSRX.h @@ -211,6 +211,8 @@ public: eResetFlags=88, eFirmVers=89, eSettingsChanged=90, + eQuadFilterEn=91, + ePidIaccum=93, }param_t; /*---------------------setters and getters that use the solicated param request/response-------------*//** * Send a one shot frame to set an arbitrary signal. diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index 1ffa2024c6..0aacae8b82 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -100,6 +100,8 @@ public: void SetCloseLoopRampRate(double rampRate); void SelectProfileSlot(int slotIdx); double GetIzone(); + int GetIaccum(); + void ClearIaccum(); private: // Values for various modes as is sent in the CAN packets for the Talon. enum TalonControlMode { diff --git a/wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h b/wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h index 2ed5c61d00..dabbf04d00 100644 --- a/wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h +++ b/wpilibc/wpilibC++Devices/include/ctre/CanTalonSRX.h @@ -212,6 +212,8 @@ public: eResetFlags=88, eFirmVers=89, eSettingsChanged=90, + eQuadFilterEn=91, + ePidIaccum=93, }param_t; /*---------------------setters and getters that use the solicated param request/response-------------*//** * Send a one shot frame to set an arbitrary signal. diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index d8624097a3..4306f17ba6 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -815,9 +815,12 @@ void CANTalon::SetCloseLoopRampRate(double rampRate) uint32_t CANTalon::GetFirmwareVersion() { int firmwareVersion; - m_impl->RequestParam(CanTalonSRX::eFirmVers); + CTR_Code status = m_impl->RequestParam(CanTalonSRX::eFirmVers); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } usleep(1000); - CTR_Code status = m_impl->GetParamResponseInt32(CanTalonSRX::eFirmVers,firmwareVersion); + status = m_impl->GetParamResponseInt32(CanTalonSRX::eFirmVers,firmwareVersion); if(status != CTR_OKAY) { wpi_setErrorWithContext(status, getHALErrorMessage(status)); } @@ -830,6 +833,33 @@ uint32_t CANTalon::GetFirmwareVersion() return firmwareVersion; } +/** + * @return The accumulator for I gain. + */ +int CANTalon::GetIaccum() +{ + CTR_Code status = m_impl->RequestParam(CanTalonSRX::ePidIaccum); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + usleep(1000); /* small yield for getting response */ + int iaccum; + status = m_impl->GetParamResponseInt32(CanTalonSRX::ePidIaccum,iaccum); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } + return iaccum; +} +/** + * Clear the accumulator for I gain. + */ +void CANTalon::ClearIaccum() +{ + CTR_Code status = m_impl->SetParam(CanTalonSRX::ePidIaccum, 0); + if(status != CTR_OKAY) { + wpi_setErrorWithContext(status, getHALErrorMessage(status)); + } +} /** * 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 4c0c1d34f6..95f13c1460 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 @@ -438,7 +438,7 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { return CanTalonJNI.intp_value(fp); } - public double getRampRate() { + public double getCloseLoopRampRate() { // if(!(m_controlMode.equals(ControlMode.Position) || m_controlMode.equals(ControlMode.Speed))) { // throw new IllegalStateException("PID mode only applies in Position and Speed modes."); // } @@ -456,8 +456,41 @@ public class CANTalon implements MotorSafety, PIDOutput, SpeedController { m_impl.GetCloseLoopRampRate(m_profile, new SWIGTYPE_p_int(fp, true)); return CanTalonJNI.intp_value(fp); } + /** + * @return The version of the firmware running on the Talon + */ + public long GetFirmwareVersion() { + // Update the information that we have. + m_impl.RequestParam(CanTalonSRX.param_t.eFirmVers); + // Briefly wait for new values from the Talon. + Timer.delay(0.001); + + long fp = CanTalonJNI.new_intp(); + m_impl.GetParamResponseInt32(CanTalonSRX.param_t.eFirmVers, new SWIGTYPE_p_int(fp, true)); + return CanTalonJNI.intp_value(fp); + } + public long GetIaccum() { + + // Update the information that we have. + m_impl.RequestParam(CanTalonSRX.param_t.ePidIaccum); + + // Briefly wait for new values from the Talon. + Timer.delay(0.001); + + long fp = CanTalonJNI.new_intp(); + m_impl.GetParamResponseInt32(CanTalonSRX.param_t.ePidIaccum, new SWIGTYPE_p_int(fp, true)); + return CanTalonJNI.intp_value(fp); + } + + /** + * Clear the accumulator for I gain. + */ + public void ClearIaccum() + { + SWIGTYPE_p_CTR_Code status = m_impl.SetParam(CanTalonSRX.param_t.ePidIaccum, 0); + } /** * Set the proportional value of the currently selected profile. * diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java index 43d9e4f8ab..8b22403e65 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CanTalonSRX.java @@ -435,6 +435,8 @@ public class CanTalonSRX extends CtreCanNode { public final static CanTalonSRX.param_t eResetFlags = new CanTalonSRX.param_t("eResetFlags", CanTalonJNI.CanTalonSRX_eResetFlags_get()); public final static CanTalonSRX.param_t eFirmVers = new CanTalonSRX.param_t("eFirmVers", CanTalonJNI.CanTalonSRX_eFirmVers_get()); public final static CanTalonSRX.param_t eSettingsChanged = new CanTalonSRX.param_t("eSettingsChanged", CanTalonJNI.CanTalonSRX_eSettingsChanged_get()); + public final static CanTalonSRX.param_t eQuadFilterEn = new CanTalonSRX.param_t("eQuadFilterEn", CanTalonJNI.CanTalonSRX_eQuadFilterEn_get()); + public final static CanTalonSRX.param_t ePidIaccum = new CanTalonSRX.param_t("ePidIaccum", CanTalonJNI.CanTalonSRX_ePidIaccum_get()); public final int swigValue() { return swigValue; @@ -470,7 +472,7 @@ public class CanTalonSRX extends CtreCanNode { swigNext = this.swigValue+1; } - private static param_t[] swigValues = { eProfileParamSlot0_P, eProfileParamSlot0_I, eProfileParamSlot0_D, eProfileParamSlot0_F, eProfileParamSlot0_IZone, eProfileParamSlot0_CloseLoopRampRate, eProfileParamSlot1_P, eProfileParamSlot1_I, eProfileParamSlot1_D, eProfileParamSlot1_F, eProfileParamSlot1_IZone, eProfileParamSlot1_CloseLoopRampRate, eProfileParamSoftLimitForThreshold, eProfileParamSoftLimitRevThreshold, eProfileParamSoftLimitForEnable, eProfileParamSoftLimitRevEnable, eOnBoot_BrakeMode, eOnBoot_LimitSwitch_Forward_NormallyClosed, eOnBoot_LimitSwitch_Reverse_NormallyClosed, eOnBoot_LimitSwitch_Forward_Disable, eOnBoot_LimitSwitch_Reverse_Disable, eFault_OverTemp, eFault_UnderVoltage, eFault_ForLim, eFault_RevLim, eFault_HardwareFailure, eFault_ForSoftLim, eFault_RevSoftLim, eStckyFault_OverTemp, eStckyFault_UnderVoltage, eStckyFault_ForLim, eStckyFault_RevLim, eStckyFault_ForSoftLim, eStckyFault_RevSoftLim, eAppliedThrottle, eCloseLoopErr, eFeedbackDeviceSelect, eRevMotDuringCloseLoopEn, eModeSelect, eProfileSlotSelect, eRampThrottle, eRevFeedbackSensor, eLimitSwitchEn, eLimitSwitchClosedFor, eLimitSwitchClosedRev, eSensorPosition, eSensorVelocity, eCurrent, eBrakeIsEnabled, eEncPosition, eEncVel, eEncIndexRiseEvents, eQuadApin, eQuadBpin, eQuadIdxpin, eAnalogInWithOv, eAnalogInVel, eTemp, eBatteryV, eResetCount, eResetFlags, eFirmVers, eSettingsChanged }; + private static param_t[] swigValues = { eProfileParamSlot0_P, eProfileParamSlot0_I, eProfileParamSlot0_D, eProfileParamSlot0_F, eProfileParamSlot0_IZone, eProfileParamSlot0_CloseLoopRampRate, eProfileParamSlot1_P, eProfileParamSlot1_I, eProfileParamSlot1_D, eProfileParamSlot1_F, eProfileParamSlot1_IZone, eProfileParamSlot1_CloseLoopRampRate, eProfileParamSoftLimitForThreshold, eProfileParamSoftLimitRevThreshold, eProfileParamSoftLimitForEnable, eProfileParamSoftLimitRevEnable, eOnBoot_BrakeMode, eOnBoot_LimitSwitch_Forward_NormallyClosed, eOnBoot_LimitSwitch_Reverse_NormallyClosed, eOnBoot_LimitSwitch_Forward_Disable, eOnBoot_LimitSwitch_Reverse_Disable, eFault_OverTemp, eFault_UnderVoltage, eFault_ForLim, eFault_RevLim, eFault_HardwareFailure, eFault_ForSoftLim, eFault_RevSoftLim, eStckyFault_OverTemp, eStckyFault_UnderVoltage, eStckyFault_ForLim, eStckyFault_RevLim, eStckyFault_ForSoftLim, eStckyFault_RevSoftLim, eAppliedThrottle, eCloseLoopErr, eFeedbackDeviceSelect, eRevMotDuringCloseLoopEn, eModeSelect, eProfileSlotSelect, eRampThrottle, eRevFeedbackSensor, eLimitSwitchEn, eLimitSwitchClosedFor, eLimitSwitchClosedRev, eSensorPosition, eSensorVelocity, eCurrent, eBrakeIsEnabled, eEncPosition, eEncVel, eEncIndexRiseEvents, eQuadApin, eQuadBpin, eQuadIdxpin, eAnalogInWithOv, eAnalogInVel, eTemp, eBatteryV, eResetCount, eResetFlags, eFirmVers, eSettingsChanged, eQuadFilterEn, ePidIaccum }; private static int swigNext = 0; private final int swigValue; private final String swigName; diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java index 0267615689..bd55d12d3f 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/hal/CanTalonJNI.java @@ -143,6 +143,8 @@ public class CanTalonJNI { public final static native int CanTalonSRX_eResetFlags_get(); public final static native int CanTalonSRX_eFirmVers_get(); public final static native int CanTalonSRX_eSettingsChanged_get(); + public final static native int CanTalonSRX_eQuadFilterEn_get(); + public final static native int CanTalonSRX_ePidIaccum_get(); public final static native long CanTalonSRX_SetParam(long jarg1, CanTalonSRX jarg1_, int jarg2, double jarg3); public final static native long CanTalonSRX_RequestParam(long jarg1, CanTalonSRX jarg1_, int jarg2); public final static native long CanTalonSRX_GetParamResponse(long jarg1, CanTalonSRX jarg1_, int jarg2, long jarg3); diff --git a/wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp b/wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp index 539f2ff6e9..ac7810269e 100644 --- a/wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp +++ b/wpilibj/wpilibJavaJNI/lib/CanTalonSRXJNI.cpp @@ -2049,6 +2049,30 @@ SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1 } +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1eQuadFilterEn_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + CanTalonSRX::_param_t result; + + (void)jenv; + (void)jcls; + result = (CanTalonSRX::_param_t)CanTalonSRX::eQuadFilterEn; + jresult = (jint)result; + return jresult; +} + + +SWIGEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1ePidIaccum_1get(JNIEnv *jenv, jclass jcls) { + jint jresult = 0 ; + CanTalonSRX::_param_t result; + + (void)jenv; + (void)jcls; + result = (CanTalonSRX::_param_t)CanTalonSRX::ePidIaccum; + jresult = (jint)result; + return jresult; +} + + SWIGEXPORT jlong JNICALL Java_edu_wpi_first_wpilibj_hal_CanTalonJNI_CanTalonSRX_1SetParam(JNIEnv *jenv, jclass jcls, jlong jarg1, jobject jarg1_, jint jarg2, jdouble jarg3) { jlong jresult = 0 ; CanTalonSRX *arg1 = (CanTalonSRX *) 0 ; diff --git a/wpilibj/wpilibJavaJNI/swigTalon/README b/wpilibj/wpilibJavaJNI/swigTalon/README index ff44615791..52b1ea9d73 100644 --- a/wpilibj/wpilibJavaJNI/swigTalon/README +++ b/wpilibj/wpilibJavaJNI/swigTalon/README @@ -7,3 +7,4 @@ bindings are checked into git, so that it should work until someone goes and upd In order for this to work, I had to change the CanTalonSRX constructor to take a int deviceNumber instead of a uint8_t. Also, in all the SWIGTYPE* files, you must change protected methods to public functions. +Because the SWIGTYPE* files don't generally change, you can jsut do a git checkout -- SWIGTYPE* in wpilibJavaDevices/....../wpilibj/