From 05715842ed0d1bc5766ea5e22c6eb339d1649bee Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Mon, 8 Feb 2016 02:35:14 -0500 Subject: [PATCH] Update CANTalon for 2016 - Add unit tests for CANTalon - Some backwards-incompatible hal_data changes, but increases consistency and should be easier to use now - Fixes #175 --- hal-base/hal/constants.py | 54 +++ hal-sim/hal_impl/functions.py | 389 +++++++++++---- wpilib/tests/test_cantalon.py | 110 +++++ wpilib/wpilib/cantalon.py | 886 +++++++++++++++++++++++++++++++--- 4 files changed, 1255 insertions(+), 184 deletions(-) create mode 100644 wpilib/tests/test_cantalon.py diff --git a/hal-base/hal/constants.py b/hal-base/hal/constants.py index 2ed00c6fe..e865662a7 100644 --- a/hal-base/hal/constants.py +++ b/hal-base/hal/constants.py @@ -163,6 +163,7 @@ class TalonSRXConst: kMode_CurrentCloseLoop = 3 kMode_VoltCompen = 4 kMode_SlaveFollower = 5 + kMode_MotionProfile = 6 kMode_NoDrive = 15 # limit switch enumerations @@ -183,7 +184,14 @@ class TalonSRXConst: kFeedbackDev_AnalogEncoder = 3 kFeedbackDev_CountEveryRisingEdge = 4 kFeedbackDev_CountEveryFallingEdge = 5 + kFeedbackDev_CtreMagEncoder_Relative = 6 + kFeedbackDev_CtreMagEncoder_Absolute = 7 kFeedbackDev_PosIsPulseWidth = 8 + + # feedback device status enumerations + kFeedbackDevStatus_Unknown = 0 + kFeedbackDevStatus_Present = 1 + kFeedbackDevStatus_NotPresent = 2 # ProfileSlotSelect enumerations kProfileSlotSelect_Slot0 = 0 @@ -194,6 +202,24 @@ class TalonSRXConst: kStatusFrame_Feedback = 1 kStatusFrame_Encoder = 2 kStatusFrame_AnalogTempVbat = 3 + kStatusFrame_PulseWidth = 4 + kStatusFrame_MotionProfile = 5 + + # Motion Profile status bits + kMotionProfileFlag_ActTraj_IsValid = 0x1 + kMotionProfileFlag_HasUnderrun = 0x2 + kMotionProfileFlag_IsUnderrun = 0x4 + kMotionProfileFlag_ActTraj_IsLast = 0x8 + kMotionProfileFlag_ActTraj_VelOnly = 0x10 + + # Motor output is neutral, Motion Profile Executer is not running. + kMotionProfile_Disable = 0 + # Motor output is updated from Motion Profile Executer, MPE will + # process the buffered points. + kMotionProfile_Enable = 1 + # Motor output is updated from Motion Profile Executer, MPE will + # stay processing current trajectory point. + kMotionProfile_Hold = 2 class TalonSRXParam: # Signal enumeration for generic signal access @@ -262,6 +288,34 @@ class TalonSRXParam: eSettingsChanged = 90 eQuadFilterEn = 91 ePidIaccum = 93 + eStatus1FrameRate = 94 # TALON_Status_1_General_10ms_t + eStatus2FrameRate = 95 # TALON_Status_2_Feedback_20ms_t + eStatus3FrameRate = 96 # TALON_Status_3_Enc_100ms_t + eStatus4FrameRate = 97 # TALON_Status_4_AinTempVbat_100ms_t + eStatus6FrameRate = 98 # TALON_Status_6_Eol_t + eStatus7FrameRate = 99 # TALON_Status_7_Debug_200ms_t + eClearPositionOnIdx = 100 + # reserved + # reserved + # reserved + ePeakPosOutput = 104 + eNominalPosOutput = 105 + ePeakNegOutput = 106 + eNominalNegOutput = 107 + eQuadIdxPolarity = 108 + eStatus8FrameRate = 109 # TALON_Status_8_PulseWid_100ms_t + eAllowPosOverflow = 110 + eProfileParamSlot0_AllowableClosedLoopErr = 111 + eNumberPotTurns = 112 + eNumberEncoderCPR = 113 + ePwdPosition = 114 + eAinPosition = 115 + eProfileParamVcompRate = 116 + eProfileParamSlot1_AllowableClosedLoopErr = 117 + eStatus9FrameRate = 118 # TALON_Status_9_MotProfBuffer_100ms_t + eMotionProfileHasUnderrunErr = 119 + eReserved120 = 120 + eLegacyControlMode = 121 TalonSRXParam_tostr = {getattr(TalonSRXParam, p): p for p in dir(TalonSRXParam) if not p.startswith('__')} diff --git a/hal-sim/hal_impl/functions.py b/hal-sim/hal_impl/functions.py index f6e5e0989..309cdbb2f 100644 --- a/hal-sim/hal_impl/functions.py +++ b/hal-sim/hal_impl/functions.py @@ -1393,6 +1393,139 @@ def clearAllPCMStickyFaults_sol(solenoid_port, status): ############################################################################# # TalonSRX ############################################################################# + +def __create_srx_param_map(): + '''Defines the mappings between CANTalon dict items and raw parameter types''' + TalonSRXParam = constants.TalonSRXParam + return { + TalonSRXParam.eProfileParamSlot0_P : 'profile0_p', + TalonSRXParam.eProfileParamSlot0_I : 'profile0_i', + TalonSRXParam.eProfileParamSlot0_D : 'profile0_d', + TalonSRXParam.eProfileParamSlot0_F : 'profile0_f', + TalonSRXParam.eProfileParamSlot0_IZone : 'profile0_izone', + TalonSRXParam.eProfileParamSlot0_CloseLoopRampRate : 'profile0_closeloopramprate', + TalonSRXParam.eProfileParamSlot1_P : 'profile1_p', + TalonSRXParam.eProfileParamSlot1_I : 'profile1_i', + TalonSRXParam.eProfileParamSlot1_D : 'profile1_d', + TalonSRXParam.eProfileParamSlot1_F : 'profile1_f', + TalonSRXParam.eProfileParamSlot1_IZone : 'profile1_izone', + TalonSRXParam.eProfileParamSlot1_CloseLoopRampRate : 'profile1_closeloopramprate', + TalonSRXParam.eProfileParamSoftLimitForThreshold : 'soft_limit_for', + TalonSRXParam.eProfileParamSoftLimitRevThreshold : 'soft_limit_rev', + TalonSRXParam.eProfileParamSoftLimitForEnable : 'soft_limit_for_enable', + TalonSRXParam.eProfileParamSoftLimitRevEnable : 'soft_limit_rev_enable', + TalonSRXParam.eOnBoot_BrakeMode : 'onboot_brake_mode', + TalonSRXParam.eOnBoot_LimitSwitch_Forward_NormallyClosed : 'onboot_limsw_for_normally_closed', + TalonSRXParam.eOnBoot_LimitSwitch_Reverse_NormallyClosed : 'onboot_limsw_rev_normally_closed', + TalonSRXParam.eOnBoot_LimitSwitch_Forward_Disable : 'onboot_limsw_for_disable', + TalonSRXParam.eOnBoot_LimitSwitch_Reverse_Disable : 'onboot_limsw_rev_disable', + TalonSRXParam.eFault_OverTemp : 'fault_overtemp', + TalonSRXParam.eFault_UnderVoltage : 'fault_undervoltage', + TalonSRXParam.eFault_ForLim : 'fault_forlim', + TalonSRXParam.eFault_RevLim : 'fault_revlim', + TalonSRXParam.eFault_HardwareFailure : 'fault_hwfailure', + TalonSRXParam.eFault_ForSoftLim : 'fault_forsoftlim', + TalonSRXParam.eFault_RevSoftLim : 'fault_revsoftlim', + TalonSRXParam.eStckyFault_OverTemp : 'stickyfault_overtemp', + TalonSRXParam.eStckyFault_UnderVoltage : 'stickyfault_undervoltage', + TalonSRXParam.eStckyFault_ForLim : 'stickyfault_forlim', + TalonSRXParam.eStckyFault_RevLim : 'stickyfault_revlim', + TalonSRXParam.eStckyFault_ForSoftLim : 'stickyfault_forsoftlim', + TalonSRXParam.eStckyFault_RevSoftLim : 'stickyfault_revsoftlim', + TalonSRXParam.eAppliedThrottle : 'value', + TalonSRXParam.eCloseLoopErr : 'closeloop_err', + TalonSRXParam.eFeedbackDeviceSelect : 'feedback_device', + TalonSRXParam.eRevMotDuringCloseLoopEn : 'rev_motor_during_close_loop', + TalonSRXParam.eModeSelect : 'mode_select', + TalonSRXParam.eProfileSlotSelect : 'profile_slot_select', + TalonSRXParam.eRampThrottle : 'ramp_throttle', + TalonSRXParam.eRevFeedbackSensor : 'rev_feedback_sensor', + TalonSRXParam.eLimitSwitchEn : 'limit_switch_en', + TalonSRXParam.eLimitSwitchClosedFor : 'limit_switch_closed_for', + TalonSRXParam.eLimitSwitchClosedRev : 'limit_switch_closed_rev', + TalonSRXParam.eSensorPosition : 'ERR_DONT_USE_THIS', + TalonSRXParam.eSensorVelocity : 'ERR_DONT_USE_THIS', + TalonSRXParam.eCurrent : 'current', + TalonSRXParam.eBrakeIsEnabled : 'brake_enabled', + TalonSRXParam.eEncPosition : 'enc_position', + TalonSRXParam.eEncVel : 'enc_velocity', + TalonSRXParam.eEncIndexRiseEvents : 'enc_index_rise_events', + TalonSRXParam.eQuadApin : 'quad_apin', + TalonSRXParam.eQuadBpin : 'quad_bpin', + TalonSRXParam.eQuadIdxpin : 'quad_idxpin', + TalonSRXParam.eAnalogInWithOv : 'analog_in_position', + TalonSRXParam.eAnalogInVel : 'analog_in_velocity', + TalonSRXParam.eTemp : 'temp', + TalonSRXParam.eBatteryV : 'battery', + TalonSRXParam.eResetCount : 'reset_count', + TalonSRXParam.eResetFlags : 'reset_flags', + TalonSRXParam.eFirmVers : 'firmware_version', + TalonSRXParam.eSettingsChanged : 'settings_changed', + TalonSRXParam.eQuadFilterEn : 'quad_filter_en', + TalonSRXParam.ePidIaccum : 'pid_iaccum', + #TalonSRXParam.eStatus1FrameRate : 94 # TALON_Status_1_General_10ms_t, + #TalonSRXParam.eStatus2FrameRate : 95 # TALON_Status_2_Feedback_20ms_t, + #TalonSRXParam.eStatus3FrameRate : 96 # TALON_Status_3_Enc_100ms_t, + #TalonSRXParam.eStatus4FrameRate : 97 # TALON_Status_4_AinTempVbat_100ms_t, + #TalonSRXParam.eStatus6FrameRate : 98 # TALON_Status_6_Eol_t, + #TalonSRXParam.eStatus7FrameRate : 99 # TALON_Status_7_Debug_200ms_t, + TalonSRXParam.eClearPositionOnIdx : 'clear_position_on_idx', + + TalonSRXParam.ePeakPosOutput : 'peak_pos_output', + TalonSRXParam.eNominalPosOutput : 'nominal_pos_output', + TalonSRXParam.ePeakNegOutput : 'peak_neg_output', + TalonSRXParam.eNominalNegOutput : 'nominal_neg_output', + TalonSRXParam.eQuadIdxPolarity : 'quad_idx_polarity', + #TalonSRXParam.eStatus8FrameRate : 109 # TALON_Status_8_PulseWid_100ms_t, + TalonSRXParam.eAllowPosOverflow : 'allow_pos_overflow', + TalonSRXParam.eProfileParamSlot0_AllowableClosedLoopErr: 'profile0_allowable_closed_loop_err', + TalonSRXParam.eNumberPotTurns : 'number_pot_turns', + TalonSRXParam.eNumberEncoderCPR : 'number_encoder_cpr', + TalonSRXParam.ePwdPosition : 'pulse_width_position', + TalonSRXParam.eAinPosition : 'analog_in_position', + TalonSRXParam.eProfileParamVcompRate : 'profile_vcomp_rate', + TalonSRXParam.eProfileParamSlot1_AllowableClosedLoopErr : 'profile1_allowable_closed_loop_err', + #TalonSRXParam.eStatus9FrameRate : 118 # TALON_Status_9_MotProfBuffer_100ms_t, + TalonSRXParam.eMotionProfileHasUnderrunErr : 'motion_profile_has_underrun', + #TalonSRXParam.eReserved120 : 120, + TalonSRXParam.eLegacyControlMode : 'legacy_mode' + } + +def __create_srx_sensor_position_map(): + '''Used to determine which dict value is returned based on the currently + selected feedback_device''' + TalonSRXConst = constants.TalonSRXConst + return { + TalonSRXConst.kFeedbackDev_DigitalQuadEnc: 'enc_position', + TalonSRXConst.kFeedbackDev_AnalogPot: 'analog_in_position', + TalonSRXConst.kFeedbackDev_AnalogEncoder: 'analog_in_position', + TalonSRXConst.kFeedbackDev_CountEveryRisingEdge: 'analog_in_position', + TalonSRXConst.kFeedbackDev_CountEveryFallingEdge: 'analog_in_position', + TalonSRXConst.kFeedbackDev_CtreMagEncoder_Relative: 'pulse_width_position', + TalonSRXConst.kFeedbackDev_CtreMagEncoder_Absolute: 'pulse_width_position', + TalonSRXConst.kFeedbackDev_PosIsPulseWidth: 'pulse_width_position', + } + +def __create_srx_sensor_velocity_map(): + '''Used to determine which dict value is returned based on the currently + selected feedback_device''' + TalonSRXConst = constants.TalonSRXConst + return { + TalonSRXConst.kFeedbackDev_DigitalQuadEnc: 'enc_velocity', + TalonSRXConst.kFeedbackDev_AnalogPot: 'analog_in_velocity', + TalonSRXConst.kFeedbackDev_AnalogEncoder: 'analog_in_velocity', + TalonSRXConst.kFeedbackDev_CountEveryRisingEdge: 'analog_in_velocity', + TalonSRXConst.kFeedbackDev_CountEveryFallingEdge: 'analog_in_velocity', + TalonSRXConst.kFeedbackDev_CtreMagEncoder_Relative: 'pulse_width_velocity', + TalonSRXConst.kFeedbackDev_CtreMagEncoder_Absolute: 'pulse_width_velocity', + TalonSRXConst.kFeedbackDev_PosIsPulseWidth: 'pulse_width_velocity', + } + +_srx_param_map = __create_srx_param_map() +_srx_pos_map = __create_srx_sensor_position_map() +_srx_vel_map = __create_srx_sensor_velocity_map() + + def c_TalonSRX_Create1(deviceNumber): return c_TalonSRX_Create3(deviceNumber, 0, 0) @@ -1403,56 +1536,35 @@ def c_TalonSRX_Create3(deviceNumber, controlPeriodMs, enablePeriodMs): assert deviceNumber not in hal_data['CAN'] + # Initialize items based on their param type hal_data['CAN'][deviceNumber] = data.NotifyDict({ + v: 0 for v in _srx_param_map.values() + }) + + # Initialize non-zero items or items that don't have an associated parameter + hal_data['CAN'][deviceNumber].update({ 'type': 'talonsrx', - 'value': 0, - 'params': data.NotifyDict({}), # key is param, value is the value - 'fault_overtemp': 0, - 'fault_undervoltage': 0, - 'fault_forlim': 0, - 'fault_revlim': 0, - 'fault_hwfailure': 0, - 'fault_forsoftlim': 0, - 'fault_revsoftlim': 0, - 'stickyfault_overtemp': 0, - 'stickyfault_undervoltage': 0, - 'stickyfault_forlim': 0, - 'stickyfault_revlim': 0, - 'stickyfault_forsoftlim': 0, - 'stickyfault_revsoftlim': 0, - 'applied_throttle': 0, - 'closeloop_err': 0, - 'feedback_device_select': 0, - 'mode_select': 0, - 'limit_switch_en': 0, - 'limit_switch_closed_for': 0, - 'limit_switch_closed_rev': 0, - 'sensor_position': 0, - 'sensor_velocity': 0, - 'current': 0, - 'brake_enabled': 0, - 'enc_position': 0, - 'enc_velocity': 0, - 'enc_index_rise_events': 0, - 'quad_apin': 0, - 'quad_bpin': 0, - 'quad_idxpin': 0, - 'analog_in_with_ov': 0, - 'analog_in_vel': 0, - 'temp': 0, - 'battery': 0, - 'reset_count': 0, - 'reset_flags': 0, - 'firmware_version': 0, - 'override_limit_switch': 0, - 'feedback_device': None, - 'rev_motor_during_close_loop': None, + 'override_limit_switch': 0, 'override_braketype': None, - 'profile_slot_select': None, - 'ramp_throttle': None, - 'rev_feedback_sensor': None + 'pulse_width_velocity': 0, + 'pulse_width_present': 0, + + 'voltage_compensation_rate': 0, + + # Motion profile stuff + 'mp_position': 0, + 'mp_velocity': 0, + 'mp_timeDurMs': 0, + 'mp_profileSlotSelect': 0, + + 'mp_flags': 0, + 'mp_topBufferCnt': 0, + 'mp_btmBufferCnt': 0, + 'mp_topBufferRem': 0, + 'mp_zeroPos': 0, + 'mp_outputEnable': constants.TalonSRXConst.kMotionProfile_Disable }) return types.TalonSRX(deviceNumber) @@ -1462,100 +1574,132 @@ def c_TalonSRX_Destroy(handle): del hal_data['CAN'][handle.id] def c_TalonSRX_Set(handle, value): - assert False + hal_data['CAN'][handle.id]['value'] = int(value*1023) def c_TalonSRX_SetParam(handle, paramEnum, value): - hal_data['CAN'][handle.id]['params'][paramEnum] = value + hal_data['CAN'][handle.id][_srx_param_map[paramEnum]] = value def c_TalonSRX_RequestParam(handle, paramEnum): - params = hal_data['CAN'][handle.id]['params'] - assert paramEnum in params, "Parameter %s not set!" % constants.TalonSRXParam_tostr[paramEnum] # TODO: is this correct? - return params[paramEnum] + return hal_data['CAN'][handle.id][_srx_param_map[paramEnum]] def c_TalonSRX_GetParamResponse(handle, paramEnum): - params = hal_data['CAN'][handle.id]['params'] - assert paramEnum in params, "Parameter %s not set!" % constants.TalonSRXParam_tostr[paramEnum] # TODO: is this correct? - return params[paramEnum] + return hal_data['CAN'][handle.id][_srx_param_map[paramEnum]] def c_TalonSRX_GetParamResponseInt32(handle, paramEnum): - params = hal_data['CAN'][handle.id]['params'] - assert paramEnum in params, "Parameter %s not set!" % constants.TalonSRXParam_tostr[paramEnum] # TODO: is this correct? - return params[paramEnum] + return int(hal_data['CAN'][handle.id][_srx_param_map[paramEnum]]) def c_TalonSRX_SetPgain(handle, slotIdx, gain): - assert False + if slotIdx == 0: + hal_data['CAN'][handle.id]['profile0_p'] = gain + else: + hal_data['CAN'][handle.id]['profile1_p'] = gain def c_TalonSRX_SetIgain(handle, slotIdx, gain): - assert False + if slotIdx == 0: + hal_data['CAN'][handle.id]['profile0_i'] = gain + else: + hal_data['CAN'][handle.id]['profile1_i'] = gain def c_TalonSRX_SetDgain(handle, slotIdx, gain): - assert False + if slotIdx == 0: + hal_data['CAN'][handle.id]['profile0_d'] = gain + else: + hal_data['CAN'][handle.id]['profile1_d'] = gain def c_TalonSRX_SetFgain(handle, slotIdx, gain): - assert False + if slotIdx == 0: + hal_data['CAN'][handle.id]['profile0_f'] = gain + else: + hal_data['CAN'][handle.id]['profile1_f'] = gain def c_TalonSRX_SetIzone(handle, slotIdx, zone): - assert False + if slotIdx == 0: + hal_data['CAN'][handle.id]['profile0_izone'] = zone + else: + hal_data['CAN'][handle.id]['profile1_izone'] = zone def c_TalonSRX_SetCloseLoopRampRate(handle, slotIdx, closeLoopRampRate): - assert False + if slotIdx == 0: + hal_data['CAN'][handle.id]['profile0_closeloopramprate'] = closeLoopRampRate + else: + hal_data['CAN'][handle.id]['profile1_closeloopramprate'] = closeLoopRampRate def c_TalonSRX_SetVoltageCompensationRate(handle, voltagePerMs): - assert False + hal_data['CAN'][handle.id]['voltage_compensation_rate'] = voltagePerMs def c_TalonSRX_SetSensorPosition(handle, pos): - assert False + data = hal_data['CAN'][handle.id] + device_type = data['feedback_device'] + data[_srx_pos_map[device_type]] = pos def c_TalonSRX_SetForwardSoftLimit(handle, forwardLimit): - assert False + hal_data['CAN'][handle.id]['soft_limit_for'] = forwardLimit def c_TalonSRX_SetReverseSoftLimit(handle, reverseLimit): - assert False + hal_data['CAN'][handle.id]['soft_limit_rev'] = reverseLimit def c_TalonSRX_SetForwardSoftEnable(handle, enable): - assert False + hal_data['CAN'][handle.id]['soft_limit_for_enable'] = enable def c_TalonSRX_SetReverseSoftEnable(handle, enable): - assert False + hal_data['CAN'][handle.id]['soft_limit_rev_enable'] = enable def c_TalonSRX_GetPgain(handle, slotIdx): - assert False + if slotIdx == 0: + return hal_data['CAN'][handle.id]['profile0_p'] + else: + return hal_data['CAN'][handle.id]['profile1_p'] def c_TalonSRX_GetIgain(handle, slotIdx): - assert False + if slotIdx == 0: + return hal_data['CAN'][handle.id]['profile0_i'] + else: + return hal_data['CAN'][handle.id]['profile1_i'] def c_TalonSRX_GetDgain(handle, slotIdx): - assert False + if slotIdx == 0: + return hal_data['CAN'][handle.id]['profile0_d'] + else: + return hal_data['CAN'][handle.id]['profile1_d'] def c_TalonSRX_GetFgain(handle, slotIdx): - assert False + if slotIdx == 0: + return hal_data['CAN'][handle.id]['profile0_f'] + else: + return hal_data['CAN'][handle.id]['profile1_f'] def c_TalonSRX_GetIzone(handle, slotIdx): - assert False + if slotIdx == 0: + return hal_data['CAN'][handle.id]['profile0_izone'] + else: + return hal_data['CAN'][handle.id]['profile1_izone'] def c_TalonSRX_GetCloseLoopRampRate(handle, slotIdx): - assert False + if slotIdx == 0: + return hal_data['CAN'][handle.id]['profile0_closeloopramprate'] + else: + return hal_data['CAN'][handle.id]['profile1_closeloopramprate'] def c_TalonSRX_GetVoltageCompensationRate(handle): - assert False + return hal_data['CAN'][handle.id]['voltage_compensation_rate'] def c_TalonSRX_GetForwardSoftLimit(handle): - assert False + return hal_data['CAN'][handle.id]['soft_limit_for'] def c_TalonSRX_GetReverseSoftLimit(handle): - assert False + return hal_data['CAN'][handle.id]['soft_limit_rev'] def c_TalonSRX_GetForwardSoftEnable(handle): - assert False + return hal_data['CAN'][handle.id]['soft_limit_for_enable'] def c_TalonSRX_GetReverseSoftEnable(handle): - assert False + return hal_data['CAN'][handle.id]['soft_limit_rev_enable'] def c_TalonSRX_GetPulseWidthRiseToFallUs(handle): assert False def c_TalonSRX_IsPulseWidthSensorPresent(handle): - assert False + return hal_data['CAN'][handle.id]['pulse_width_present'] def c_TalonSRX_SetStatusFrameRate(handle, frameEnum, periodMs): pass @@ -1569,25 +1713,51 @@ def c_TalonSRX_ClearStickyFaults(handle): hal_data['CAN'][handle.id]['stickyfault_revsoftlim'] = 0 def c_TalonSRX_ChangeMotionControlFramePeriod(handle, periodMs): - assert False + pass def c_TalonSRX_ClearMotionProfileTrajectories(handle): - assert False + pass def c_TalonSRX_GetMotionProfileTopLevelBufferCount(handle): - assert False + return hal_data['CAN'][handle.id]['mp_topBufferCnt'] def c_TalonSRX_IsMotionProfileTopLevelBufferFull(handle): - assert False + return False + +_push_mp_mask = ~(constants.TalonSRXConst.kMotionProfileFlag_ActTraj_VelOnly | \ + constants.TalonSRXConst.kMotionProfileFlag_ActTraj_IsLast) def c_TalonSRX_PushMotionProfileTrajectory(handle, targPos, targVel, profileSlotSelect, timeDurMs, velOnly, isLastPoint, zeroPos): - assert False + data = hal_data['CAN'][handle.id] + data['mp_position'] = targPos + data['mp_velocity'] = targVel + data['mp_profileSlotSelect'] = profileSlotSelect + data['mp_timeDurMs'] = timeDurMs + data['mp_zeroPos'] = zeroPos + + flags = data['mp_flags'] + flags &= _push_mp_mask + + if velOnly: + flags |= constants.TalonSRXConst.kMotionProfileFlag_ActTraj_VelOnly + + if isLastPoint: + flags |= constants.TalonSRXConst.kMotionProfileFlag_ActTraj_IsLast def c_TalonSRX_ProcessMotionProfileBuffer(handle): - assert False + pass def c_TalonSRX_GetMotionProfileStatus(handle): - assert False + data = hal_data['CAN'][handle.id] + + return (data['mp_flags'], + data['mp_profileSlotSelect'], + data['mp_position'], + data['mp_velocity'], + data['mp_topBufferRem'], + data['mp_topBufferCnt'], + data['mp_btmBufferCnt'], + data['mp_outputEnable']) def c_TalonSRX_GetFault_OverTemp(handle): return hal_data['CAN'][handle.id]['fault_overtemp'] @@ -1629,13 +1799,13 @@ def c_TalonSRX_GetStckyFault_RevSoftLim(handle): return hal_data['CAN'][handle.id]['stickyfault_revsoftlim'] def c_TalonSRX_GetAppliedThrottle(handle): - return hal_data['CAN'][handle.id]['applied_throttle'] + return hal_data['CAN'][handle.id]['value'] def c_TalonSRX_GetCloseLoopErr(handle): return hal_data['CAN'][handle.id]['closeloop_err'] def c_TalonSRX_GetFeedbackDeviceSelect(handle): - return hal_data['CAN'][handle.id]['feedback_device_select'] + return hal_data['CAN'][handle.id]['feedback_device'] def c_TalonSRX_GetModeSelect(handle): return hal_data['CAN'][handle.id]['mode_select'] @@ -1650,10 +1820,16 @@ def c_TalonSRX_GetLimitSwitchClosedRev(handle): return hal_data['CAN'][handle.id]['limit_switch_closed_rev'] def c_TalonSRX_GetSensorPosition(handle): - return hal_data['CAN'][handle.id]['sensor_position'] + # this returns different values depending on the feedback device selected + data = hal_data['CAN'][handle.id] + device_type = data['feedback_device'] + return data[_srx_pos_map[device_type]] def c_TalonSRX_GetSensorVelocity(handle): - return hal_data['CAN'][handle.id]['sensor_velocity'] + # this returns different values depending on the feedback device selected + data = hal_data['CAN'][handle.id] + device_type = data['feedback_device'] + return data[_srx_vel_map[device_type]] def c_TalonSRX_GetCurrent(handle): return hal_data['CAN'][handle.id]['current'] @@ -1680,10 +1856,10 @@ def c_TalonSRX_GetQuadIdxpin(handle): return hal_data['CAN'][handle.id]['quad_idxpin'] def c_TalonSRX_GetAnalogInWithOv(handle): - return hal_data['CAN'][handle.id]['analog_in_with_ov'] + return hal_data['CAN'][handle.id]['analog_in_position'] def c_TalonSRX_GetAnalogInVel(handle): - return hal_data['CAN'][handle.id]['analog_in_vel'] + return hal_data['CAN'][handle.id]['analog_in_velocity'] def c_TalonSRX_GetTemp(handle): return hal_data['CAN'][handle.id]['temp'] @@ -1701,34 +1877,39 @@ def c_TalonSRX_GetFirmVers(handle): return hal_data['CAN'][handle.id]['firmware_version'] def c_TalonSRX_GetPulseWidthPosition(handle): - assert False + return hal_data['CAN'][handle.id]['pulse_width_position'] def c_TalonSRX_GetPulseWidthVelocity(handle): - assert False + return hal_data['CAN'][handle.id]['pulse_width_velocity'] def c_TalonSRX_GetPulseWidthRiseToRiseUs(handle): assert False def c_TalonSRX_GetActTraj_IsValid(handle): - assert False - + flags = hal_data['CAN'][handle.id]['mp_flags'] + return (flags & constants.TalonSRXConst.kMotionProfileFlag_ActTraj_IsValid) != 0 + def c_TalonSRX_GetActTraj_ProfileSlotSelect(handle): - assert False + return hal_data['CAN'][handle.id]['mp_profileSlotSelect'] def c_TalonSRX_GetActTraj_VelOnly(handle): - assert False + flags = hal_data['CAN'][handle.id]['mp_flags'] + return (flags & constants.TalonSRXConst.kMotionProfileFlag_ActTraj_VelOnly) != 0 def c_TalonSRX_GetActTraj_IsLast(handle): - assert False + flags = hal_data['CAN'][handle.id]['mp_flags'] + return (flags & constants.TalonSRXConst.kMotionProfileFlag_ActTraj_IsLast) != 0 def c_TalonSRX_GetOutputType(handle): assert False def c_TalonSRX_GetHasUnderrun(handle): - assert False + flags = hal_data['CAN'][handle.id]['mp_flags'] + return (flags & constants.TalonSRXConst.kMotionProfileFlag_HasUnderrun) != 0 def c_TalonSRX_GetIsUnderrun(handle): - assert False + flags = hal_data['CAN'][handle.id]['mp_flags'] + return (flags & constants.TalonSRXConst.kMotionProfileFlag_IsUnderrun) != 0 def c_TalonSRX_GetNextID(handle): assert False @@ -1740,10 +1921,10 @@ def c_TalonSRX_GetCount(handle): assert False def c_TalonSRX_GetActTraj_Velocity(handle): - assert False + return hal_data['CAN'][handle.id]['mp_velocity'] def c_TalonSRX_GetActTraj_Position(handle): - assert False + return hal_data['CAN'][handle.id]['mp_position'] def c_TalonSRX_SetDemand(handle, param): hal_data['CAN'][handle.id]['value'] = param diff --git a/wpilib/tests/test_cantalon.py b/wpilib/tests/test_cantalon.py new file mode 100644 index 000000000..7003cab46 --- /dev/null +++ b/wpilib/tests/test_cantalon.py @@ -0,0 +1,110 @@ + +import pytest + +@pytest.fixture(scope='function') +def cantalon_and_data(wpilib, hal_data): + ct = wpilib.CANTalon(2) + data = hal_data['CAN'][2] + + return ct, data + + +def test_ct_basic(wpilib, cantalon_and_data): + ct, data = cantalon_and_data + + assert data['mode_select'] == wpilib.CANTalon.ControlMode.Disabled + ct.set(0) + + assert data['mode_select'] == wpilib.CANTalon.ControlMode.PercentVbus + assert ct.get() == 0 + + ct.set(1) + assert ct.get() == 1 + + ct.setInverted(True) + ct.set(1) + assert ct.get() == -1 + + # test P/I/D setting + for i in [0, 1]: + ct.setProfile(i) + idx = i*4 + + ct.setP(idx+1) + assert data['profile%s_p' % i] == idx+1 + assert ct.getP() == idx+1 + + ct.setI(idx+2) + assert data['profile%s_i' % i] == idx+2 + assert ct.getI() == idx+2 + + ct.setD(idx+3) + assert data['profile%s_d' % i] == idx+3 + assert ct.getD() == idx+3 + + ct.setF(idx+4) + assert data['profile%s_f' % i] == idx+4 + assert ct.getF() == idx+4 + + # Limit switches + data['limit_switch_closed_for'] = 0 + assert ct.isFwdLimitSwitchClosed() == True + + data['limit_switch_closed_for'] = 1 + assert ct.isFwdLimitSwitchClosed() == False + + data['limit_switch_closed_rev'] = 0 + assert ct.isRevLimitSwitchClosed() == True + + data['limit_switch_closed_rev'] = 1 + assert ct.isRevLimitSwitchClosed() == False + + + # test direct sensor positions, velocities + # encoder + data['enc_position'] = 1 + data['enc_velocity'] = 2 + assert ct.getEncPosition() == 1 + assert ct.getEncVelocity() == 2 + + ct.setEncPosition(11) + assert ct.getEncPosition() == 11 + + # analog + data['analog_in_position'] = 3 + data['analog_in_velocity'] = 4 + assert ct.getAnalogInPosition() == 3 + assert ct.getAnalogInVelocity() == 4 + + ct.setAnalogPosition(33) + assert ct.getAnalogInPosition() == 33 + + # pulse width device + data['pulse_width_position'] = 6 + data['pulse_width_velocity'] = 7 + assert ct.getPulseWidthPosition() == 6 + assert ct.getPulseWidthVelocity() == 7 + + ct.setPulseWidthPosition(66) + assert ct.getPulseWidthPosition() == 66 + + # test sensor position by selecting feedback device + ct.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder) + assert ct.getPosition() == 11 + + ct.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.AnalogPot) + assert ct.getPosition() == 33 + + ct.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.PulseWidth) + assert abs(ct.getPosition() - (66/wpilib.CANTalon.kNativePwdUnitsPerRotation)) < 0.001 + + # test motion profile thing + traj = wpilib.CANTalon.TrajectoryPoint() + + ct.pushMotionProfileTrajectory(traj) + + status = wpilib.CANTalon.MotionProfileStatus() + ct.getMotionProfileStatus(status) + + + \ No newline at end of file diff --git a/wpilib/wpilib/cantalon.py b/wpilib/wpilib/cantalon.py index ecf591356..e0077a2c0 100644 --- a/wpilib/wpilib/cantalon.py +++ b/wpilib/wpilib/cantalon.py @@ -1,6 +1,7 @@ import hal import weakref +from .interfaces import PIDSource from .livewindow import LiveWindow from .livewindowsendable import LiveWindowSendable from .motorsafety import MotorSafety @@ -39,6 +40,18 @@ class CANTalon(LiveWindowSendable, MotorSafety): only need to set them in a periodic fashion as a function of what motion the application is attempting. If this API is used, be mindful of the CAN utilization reported in the driver station. + + If calling application has used the config routines to configure the + selected feedback sensor, then all positions are measured in floating point + precision rotations. All sensor velocities are specified in floating point + precision RPM. + + .. seealso: :meth:`configPotentiometerTurns`, :meth:`configEncoderCodesPerRev` + + HOWEVER, if calling application has not called the config routine for + selected feedback sensor, then all getters/setters for position/velocity use + the native engineering units of the Talon SRX firm (just like in 2015). + Signals explained below. Encoder position is measured in encoder edges. Every edge is counted (similar to roboRIO 4X mode). Analog position is 10 bits, meaning 1024 @@ -113,6 +126,20 @@ class CANTalon(LiveWindowSendable, MotorSafety): ramping. Works the same as RampThrottle but only is in effect when a close loop mode and profile slot is selected. """ + + #: Number of adc engineering units per 0 to 3.3V sweep. + #: This is necessary for scaling Analog Position in rotations/RPM. + kNativeAdcUnitsPerRotation = 1024 + + #: Number of pulse width engineering units per full rotation. + #: This is necessary for scaling Pulse Width Decoded Position in rotations/RPM. + kNativePwdUnitsPerRotation = 4096.0 + + #: Number of minutes per 100ms unit. Useful for scaling velocities + #: measured by Talon's 100ms timebase to rotations per minute. + kMinutesPer100msUnit = 1.0/600.0 + + PIDSourceType = PIDSource.PIDSourceType class ControlMode: PercentVbus = hal.TalonSRXConst.kMode_DutyCycle @@ -121,6 +148,7 @@ class ControlMode: Current = hal.TalonSRXConst.kMode_CurrentCloseLoop Voltage = hal.TalonSRXConst.kMode_VoltCompen Follower = hal.TalonSRXConst.kMode_SlaveFollower + MotionProfile = hal.TalonSRXConst.kMode_MotionProfile Disabled = hal.TalonSRXConst.kMode_NoDrive class FeedbackDevice: @@ -129,6 +157,14 @@ class FeedbackDevice: AnalogEncoder = hal.TalonSRXConst.kFeedbackDev_AnalogEncoder EncRising = hal.TalonSRXConst.kFeedbackDev_CountEveryRisingEdge EncFalling = hal.TalonSRXConst.kFeedbackDev_CountEveryFallingEdge + CtreMagEncoder_Relative = hal.TalonSRXConst.kFeedbackDev_CtreMagEncoder_Relative + CtreMagEncoder_Absolute = hal.TalonSRXConst.kFeedbackDev_CtreMagEncoder_Absolute + PulseWidth = hal.TalonSRXConst.kFeedbackDev_PosIsPulseWidth + + class FeedbackDeviceStatus: + Unknown = hal.TalonSRXConst.kFeedbackDevStatus_Unknown + Present = hal.TalonSRXConst.kFeedbackDevStatus_Present + NotPresent = hal.TalonSRXConst.kFeedbackDevStatus_NotPresent class StatusFrameRate: """enumerated types for frame rate ms""" @@ -136,26 +172,189 @@ class StatusFrameRate: Feedback = hal.TalonSRXConst.kStatusFrame_Feedback QuadEncoder = hal.TalonSRXConst.kStatusFrame_Encoder AnalogTempVbat = hal.TalonSRXConst.kStatusFrame_AnalogTempVbat + PulseWidth = hal.TalonSRXConst.kStatusFrame_PulseWidth + + class SetValueMotionProfile: + """Enumerated types for Motion Control Set Values. + + When in Motion Profile control mode, these constants are paseed + into set() to manipulate the motion profile executer. + + When changing modes, be sure to read the value back using :meth:`.getMotionProfileStatus` + to ensure changes in output take effect before performing buffering actions. + + Disable will signal Talon to put motor output into neutral drive. + + Talon will stop processing motion profile points. This means the buffer is + effectively disconnected from the executer, allowing the robot to gracefully + clear and push new traj points. isUnderrun will get cleared. + The active trajectory is also cleared. + + Enable will signal Talon to pop a trajectory point from it's buffer and process it. + If the active trajectory is empty, Talon will shift in the next point. + If the active traj is empty, and so is the buffer, the motor drive is neutral and + isUnderrun is set. When active traj times out, and buffer has at least one point, + Talon shifts in next one, and isUnderrun is cleared. When active traj times out, + and buffer is empty, Talon keeps processing active traj and sets IsUnderrun. + + Hold will signal Talon keep processing the active trajectory indefinitely. + If the active traj is cleared, Talon will neutral motor drive. Otherwise + Talon will keep processing the active traj but it will not shift in + points from the buffer. This means the buffer is effectively disconnected + from the executer, allowing the robot to gracefully clear and push + new traj points. + isUnderrun is set if active traj is empty, otherwise it is cleared. + isLast signal is also cleared. + + Typical workflow: + + - set(Disable), + - Confirm Disable takes effect, + - clear buffer and push buffer points, + - set(Enable) when enough points have been pushed to ensure no underruns, + - wait for MP to finish or decide abort, + - If MP finished gracefully set(Hold) to hold position servo and disconnect buffer, + - If MP is being aborted set(Disable) to neutral the motor and disconnect buffer, + - Confirm mode takes effect, + - clear buffer and push buffer points, and rinse-repeat. + """ + + Disable = hal.TalonSRXConst.kMotionProfile_Disable + Enable = hal.TalonSRXConst.kMotionProfile_Enable + Hold = hal.TalonSRXConst.kMotionProfile_Hold + + class TrajectoryPoint: + '''This is a data transfer object''' + + #: (double) the position to servo to + position = 0 + + #: (double) The velocity to feed-forward + velocity = 0 + + #: (int) Time in milliseconds to process this point. + #: Value should be between 1ms and 255ms. If value is zero + #: then Talon will default to 1ms. If value exceeds 255ms API will cap it. + timeDurMs = 0 + + #: (int) Which slot to get PIDF gains. + #: PID is used for position servo. + #: F is used as the Kv constant for velocity feed-forward. + #: Typically this is hardcoded to the a particular slot, but you are free + #: gain schedule if need be. + profileSlotSelect = 0 + + #: Set to true to only perform the velocity feed-forward and not perform + #: position servo. This is useful when learning how the position servo + #: changes the motor response. The same could be accomplish by clearing the + #: PID gains, however this is synchronous the streaming, and doesn't require restoing + #: gains when finished. + #: + #: Additionaly setting this basically gives you direct control of the motor output + #: since motor output = targetVelocity X Kv, where Kv is our Fgain. + #: This means you can also scheduling straight-throttle curves without relying on + #: a sensor. + velocityOnly = False + + #: (bool) Set to true to signal Talon that this is the final point, so do not + #: attempt to pop another trajectory point from out of the Talon buffer. + #: Instead continue processing this way point. Typically the velocity + #: member variable should be zero so that the motor doesn't spin indefinitely. + isLastPoint = False + + #: (bool) Set to true to signal Talon to zero the selected sensor. + #: When generating MPs, one simple method is to make the first target position zero, + #: and the final target position the target distance from the current position. + #: Then when you fire the MP, the current position gets set to zero. + #: If this is the intent, you can set zeroPos on the first trajectory point. + #: + #: Otherwise you can leave this false for all points, and offset the positions + #: of all trajectory points so they are correct. + zeroPos = False + + class MotionProfileStatus: + '''This is simply a data transfer object''' + + _flags = 0 + + #: (int) The available empty slots in the trajectory buffer. + #: + #: The robot API holds a "top buffer" of trajectory points, so your applicaion + #: can dump several points at once. The API will then stream them into the Talon's + #: low-level buffer, allowing the Talon to act on them. + topBufferRem = 0 + + #: (int) The number of points in the top trajectory buffer. + topBufferCnt = 0 + + #: (int) The number of points in the low level Talon buffer. + btmBufferCnt = 0 + + @property + def hasUnderrun(self): + """(bool) Set if isUnderrun ever gets set. + Only is cleared by :meth:`clearMotionProfileHasUnderrun` to ensure + robot logic can react or instrument it. + + .. seealso:: :meth:`clearMotionProfileHasUnderrun`""" + return (self._flags & hal.TalonSRXConst.kMotionProfileFlag_HasUnderrun) != 0 + + @property + def isUnderrun(self): + """(bool) This is set if Talon needs to shift a point from its buffer into + the active trajectory point however the buffer is empty. This gets cleared + automatically when is resolved.""" + return (self._flags & hal.TalonSRXConst.kMotionProfileFlag_IsUnderrun) != 0 + + @property + def activePointValid(self): + """(bool) True if the active trajectory point has not empty, false otherwise. + The members in activePoint are only valid if this signal is set.""" + return (self._flags & hal.TalonSRXConst.kMotionProfileFlag_ActTraj_IsValid) != 0 + + #: (:class:`.TrajectoryPoint`) The number of points in the low level Talon buffer. + activePoint = None + + #: (SetValueMotionProfile) The current output mode of the motion profile executer (disabled, enabled, or hold). + #: When changing the set() value in MP mode, it's important to check this signal to + #: confirm the change takes effect before interacting with the top buffer. + outputEnable = hal.TalonSRXConst.kMotionProfile_Disable + kDelayForSolicitedSignals = 0.004 def __init__(self, deviceNumber, - controlPeriodMs=hal.TalonSRXConst.kDefaultControlPeriodMs): + controlPeriodMs=None, + enablePeriodMs=None): MotorSafety.__init__(self) + self.pidSource = self.PIDSourceType.kDisplacement self.deviceNumber = deviceNumber + # HAL bounds period to be within [1 ms,95 ms] - self._handle = hal.TalonSRX_Create2(deviceNumber, controlPeriodMs) + if controlPeriodMs is None: + self._handle = hal.TalonSRX_Create1(deviceNumber) + elif enablePeriodMs is None: + self._handle = hal.TalonSRX_Create2(deviceNumber, controlPeriodMs) + else: + self._handle = hal.TalonSRX_Create3(deviceNumber, controlPeriodMs, enablePeriodMs) + self.__finalizer = weakref.finalize(self, _freeCANTalon, self._handle) + self.controlMode = self.ControlMode.PercentVbus + self.minimumInput = 0 + self.maximumInput = 0 self.controlEnabled = True self.isInverted = False self.profile = 0 self.setPoint = 0.0 + self.codesPerRev = 0 + self.numPotTurns = 0 + self.feedbackDevice = self.FeedbackDevice.QuadEncoder self.setProfile(self.profile) self._applyControlMode(self.ControlMode.PercentVbus) - LiveWindow.addActuatorChannel("CANTalonSRX", deviceNumber, self) + LiveWindow.addActuatorChannel("CANTalon", deviceNumber, self) hal.HALReport(hal.HALUsageReporting.kResourceType_CANTalonSRX, deviceNumber) @@ -177,6 +376,15 @@ def pidWrite(self, output): self.set(output) else: raise ValueError("PID only supported in PercentVbus mode") + + def setPIDSourceType(self, pidSource): + self.pidSource = pidSource + + def getPIDSourceType(self): + return self.pidSource + + def pidGet(self): + return self.getPosition() def set(self, outputValue, syncGroup=0): """ @@ -198,27 +406,78 @@ def set(self, outputValue, syncGroup=0): :param outputValue: The setpoint value, as described above. """ + self.feed() if not self.controlEnabled: return - self.setPoint = outputValue - outputValue = -outputValue if self.isInverted else outputValue + + self.setPoint = outputValue # cache set point for getSetpoint() + if self.controlMode == self.ControlMode.PercentVbus: - if outputValue > 1: - outputValue = 1 - elif outputValue < -1: - outputValue = -1 - hal.TalonSRX_SetDemand(self.handle, int(1023*outputValue)) + hal.TalonSRX_Set(self.handle, -outputValue if self.isInverted else outputValue) elif self.controlMode == self.ControlMode.Follower: hal.TalonSRX_SetDemand(self.handle, int(outputValue)) elif self.controlMode == self.ControlMode.Voltage: # Voltage is an 8.8 fixed point number. - volts = int(outputValue * 256) + volts = int((-outputValue if self.isInverted else outputValue) * 256) hal.TalonSRX_SetDemand(self.handle, volts) elif self.controlMode == self.ControlMode.Speed: - hal.TalonSRX_SetDemand(self.handle, int(outputValue)) + hal.TalonSRX_SetDemand(self.handle, self._scaleVelocityToNativeUnits(self.feedbackDevice, -outputValue if self.isInverted else outputValue)) elif self.controlMode == self.ControlMode.Position: - hal.TalonSRX_SetDemand(self.handle, int(outputValue)) + hal.TalonSRX_SetDemand(self.handle, self._scaleRotationsToNativeUnits(self.feedbackDevice, -outputValue if self.isInverted else outputValue)) + elif self.controlMode == self.ControlMode.Current: + mA = (-outputValue if self.isInverted else outputValue) * 1000.0; # mA + hal.TalonSRX_SetDemand(self.handle, int(mA)) + elif self.controlMode == self.ControlMode.MotionProfile: + hal.TalonSRX_SetDemand(self.handle, outputValue) + hal.TalonSRX_SetModeSelect(self.handle, self.controlMode) + + def setInverted(self, isInverted): + """ + Inverts the direction of the motor's rotation. Only works in PercentVbus + mode. + + :param isInverted: The state of inversion (True is inverted). + """ + self.isInverted = bool(isInverted) + + def getInverted(self): + """ + Common interface for the inverting direction of a speed controller. + + :returns: The state of inversion (True is inverted). + """ + return self.isInverted + + def reset(self): + """Resets the accumulated integral error and disables the controller. + + The only difference between this and {@link PIDController#reset} is that + the PIDController also resets the previous error for the D term, but the + difference should have minimal effect as it will only last one cycle. + """ + self.disable() + self.clearIaccum() + + def isEnabled(self): + """Return true if Talon is enabled. + + :returns: true if the Talon is enabled and may be applying power to the motor + """ + return self.isControlEnabled() + + def getError(self): + """Returns the difference between the setpoint and the current position. + + :returns: The error in units corresponding to whichever mode we are in. + + .. seealso:: :meth:`set` for a detailed description of the various units. + """ + return self.getClosedLoopError() + + def setSetpoint(self, setpoint): + """Calls :meth:`set`""" + self.set(setpoint) def reverseSensor(self, flip): """ @@ -250,7 +509,7 @@ def get(self): In Speed mode: returns current speed. - In Position omde: returns current sensor position. + In Position mode: returns current sensor position. In Throttle and Follower modes: returns current applied throttle. @@ -261,28 +520,12 @@ def get(self): elif self.controlMode == self.ControlMode.Current: return self.getOutputCurrent() elif self.controlMode == self.ControlMode.Speed: - return float(hal.TalonSRX_GetSensorVelocity(self.handle)) + return self._scaleNativeUnitsToRpm(self.feedbackDevice, hal.TalonSRX_GetSensorVelocity(self.handle)) elif self.controlMode == self.ControlMode.Position: - return float(hal.TalonSRX_GetSensorPosition(self.handle)) + return self._scaleNativeUnitsToRotations(self.feedbackDevice, hal.TalonSRX_GetSensorPosition(self.handle)) else: # PercentVbus return hal.TalonSRX_GetAppliedThrottle(self.handle) / 1023.0 - def setInverted(self, isInverted): - """ - Common interface for inverting direction of a speed controller. - - :param isInverted: The state of inversion (True is inverted). - """ - self.isInverted = isInverted - - def getInverted(self): - """ - Common interface for the inverting direction of a speed controller. - - :returns: The state of inversion (True is inverted). - """ - return self.isInverted - def getEncPosition(self): """ Get the current encoder position, regardless of whether it is the @@ -291,6 +534,9 @@ def getEncPosition(self): :returns: The current position of the encoder. """ return hal.TalonSRX_GetEncPosition(self.handle) + + def setEncPosition(self, newPosition): + self.setParameter(hal.TalonSRXParam.eEncPosition, newPosition) def getEncVelocity(self): """ @@ -300,6 +546,48 @@ def getEncVelocity(self): :returns: The current speed of the encoder. """ return hal.TalonSRX_GetEncVel(self.handle) + + def getPulseWidthPosition(self): + return hal.TalonSRX_GetPulseWidthPosition(self.handle) + + def setPulseWidthPosition(self, newPosition): + self.setParameter(hal.TalonSRXParam.ePwdPosition, newPosition) + + def getPulseWidthVelocity(self): + return hal.TalonSRX_GetPulseWidthVelocity(self.handle) + + def getPulseWidthRiseToFallUs(self): + return hal.TalonSRX_GetPulseWidthRiseToFallUs(self.handle) + + def getPulseWidthRiseToRiseUs(self): + return hal.TalonSRX_GetPulseWidthRiseToRiseUs(self.handle) + + def isSensorPresent(self, feedbackDevice): + """:param feedbackDevice: which feedback sensor to check it if is connected. + :returns: status of caller's specified sensor type. + """ + retval = self.FeedbackDeviceStatus.Unknown; + # detecting sensor health depends on which sensor caller cares about + if feedbackDevice in [self.FeedbackDevice.QuadEncoder, + self.FeedbackDevice.AnalogPot, + self.FeedbackDevice.AnalogEncoder, + self.FeedbackDevice.EncRising, + self.FeedbackDevice.EncFalling]: + # no real good way to tell if these sensor + # are actually present so return status unknown. + pass + elif feedbackDevice in [self.FeedbackDevice.PulseWidth, + self.FeedbackDevice.CtreMagEncoder_Relative, + self.FeedbackDevice.CtreMagEncoder_Absolute]: + # all of these require pulse width signal to be present. + if hal.TalonSRX_IsPulseWidthSensorPresent(self.handle) == 0: + # Talon not getting a signal + retval = self.FeedbackDeviceStatus.NotPresent; + else: + # getting good signal + retval = self.FeedbackDeviceStatus.Present; + + return retval; def getNumberOfQuadIdxRises(self): """ @@ -326,6 +614,9 @@ def getPinStateQuadIdx(self): :returns: IO level of QUAD Index pin. """ return hal.TalonSRX_GetQuadIdxpin(self.handle) + + def setAnalogPosition(self, newPosition): + self.setParameter(hal.TalonSRXParam.eAinPosition, newPosition) def getAnalogInPosition(self): """ @@ -363,6 +654,18 @@ def getClosedLoopError(self): """ return hal.TalonSRX_GetCloseLoopErr(self.handle) + def setAllowableClosedLoopErr(self, allowableCloseLoopError): + """Set the allowable closed loop error. + + :param allowableCloseLoopError: allowable closed loop error for selected profile. + mA for Curent closed loop. + Talon Native Units for position and velocity. + """ + if self.profile == 0: + self.setParameter(hal.TalonSRXParam.eProfileParamSlot0_AllowableClosedLoopErr, allowableCloseLoopError) + else: + self.setParameter(hal.TalonSRXParam.eProfileParamSlot1_AllowableClosedLoopErr, allowableCloseLoopError) + def isFwdLimitSwitchClosed(self): """Returns True if limit switch is closed. False if open.""" return hal.TalonSRX_GetLimitSwitchClosedFor(self.handle) == 0 @@ -374,16 +677,37 @@ def isRevLimitSwitchClosed(self): def getBrakeEnableDuringNeutral(self): """Returns True if break is enabled during neutral. False if coast.""" return hal.TalonSRX_GetBrakeIsEnabled(self.handle) != 0 + + def configEncoderCodesPerRev(self, codesPerRev): + """Configure how many codes per revolution are generated by your encoder. + + :param codesPerRev: The number of counts per revolution. + """ + # first save the scalar so that all getters/setter work as the user expects + self.codesPerRev = codesPerRev + # next send the scalar to the Talon over CAN. This is so that the Talon can report + # it to whoever needs it, like the webdash. Don't bother checking the return, + # this is only for instrumentation and is not necessary for Talon functionality. + self.setParameter(hal.TalonSRXParam.eNumberEncoderCPR, self.codesPerRev) + + def configPotentiometerTurns(self, turns): + """Configure the number of turns on the potentiometer. + + :param turns: The number of turns of the potentiometer. + """ + # first save the scalar so that all getters/setter work as the user expects + self.numPotTurns = turns + # next send the scalar to the Talon over CAN. This is so that the Talon can report + # it to whoever needs it, like the webdash. Don't bother checking the return, + # this is only for instrumentation and is not necessary for Talon functionality. + self.setParameter(hal.TalonSRXParam.eNumberPotTurns, self.numPotTurns) - def getTemp(self): + def getTemperature(self): """Returns temperature of Talon, in degrees Celsius.""" return hal.TalonSRX_GetTemp(self.handle) - - def getSensorPosition(self): - return hal.TalonSRX_GetSensorPosition(self.handle) - - def getSensorVelocity(self): - return hal.TalonSRX_GetSensorVelocity(self.handle) + + # deprecated + getTemp = getTemperature def getOutputCurrent(self): """Returns the current going through the Talon, in Amperes.""" @@ -402,17 +726,39 @@ def getBusVoltage(self): return hal.TalonSRX_GetBatteryV(self.handle) def getPosition(self): - return hal.TalonSRX_GetSensorPosition(self.handle) + """ + :returns: The position of the sensor currently providing feedback. When using + analog sensors, 0 units corresponds to 0V, 1023 units corresponds + to 3.3V When using an analog encoder (wrapping around 1023 to 0 is + possible) the units are still 3.3V per 1023 units. When using + quadrature, each unit is a quadrature edge (4X) mode. + """ + return self._scaleNativeUnitsToRotations(self.feedbackDevice, hal.TalonSRX_GetSensorPosition(self.handle)) def setPosition(self, pos): - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eSensorPosition, - int(pos)) + nativePos = self._scaleRotationsToNativeUnits(self.feedbackDevice, pos) + hal.TalonSRX_SetSensorPosition(self.handle, nativePos) def getSpeed(self): - return hal.TalonSRX_GetSensorVelocity(self.handle) + """:returns: The speed of the sensor currently providing feedback. + + The speed units will be in the sensor's native ticks per 100ms. + + For analog sensors, 3.3V corresponds to 1023 units. So a speed of + 200 equates to ~0.645 dV per 100ms or 6.451 dV per second. If this + is an analog encoder, that likely means 1.9548 rotations per sec. + For quadrature encoders, each unit corresponds a quadrature edge + (4X). So a 250 count encoder will produce 1000 edge events per + rotation. An example speed of 200 would then equate to 20% of a + rotation per 100ms, or 10 rotations per second. + """ + return self._scaleNativeUnitsToRpm(self.feedbackDevice, hal.TalonSRX_GetSensorVelocity(self.handle)) def getControlMode(self): return self.controlMode + + def setControlMode(self, controlMode): + self.changeControlMode(controlMode) def _applyControlMode(self, controlMode): """ @@ -433,6 +779,9 @@ def changeControlMode(self, controlMode): self._applyControlMode(controlMode) def setFeedbackDevice(self, device): + # save the selection so that future setters/getters know which scalars to apply + self.feedbackDevice = device + # pass feedback to actual CAN frame hal.TalonSRX_SetFeedbackDeviceSelect(self.handle, device) def setStatusFrameRateMs(self, stateFrame, periodMs): @@ -443,6 +792,9 @@ def setStatusFrameRateMs(self, stateFrame, periodMs): def enableControl(self): self.changeControlMode(self.controlMode) self.controlEnabled = True + + def enable(self): + self.enableControl() def disableControl(self): hal.TalonSRX_SetModeSelect(self.handle, self.ControlMode.Disabled) @@ -474,6 +826,8 @@ def getP(self): Get the current proportional constant. :returns: double proportional constant for current profile. + + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) """ if self.profile == 0: return self._getParam(hal.TalonSRXParam.eProfileParamSlot0_P) @@ -481,24 +835,36 @@ def getP(self): return self._getParam(hal.TalonSRXParam.eProfileParamSlot1_P) def getI(self): + """ + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) + """ if self.profile == 0: return self._getParam(hal.TalonSRXParam.eProfileParamSlot0_I) else: return self._getParam(hal.TalonSRXParam.eProfileParamSlot1_I) def getD(self): + """ + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) + """ if self.profile == 0: return self._getParam(hal.TalonSRXParam.eProfileParamSlot0_D) else: return self._getParam(hal.TalonSRXParam.eProfileParamSlot1_D) def getF(self): + """ + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) + """ if self.profile == 0: return self._getParam(hal.TalonSRXParam.eProfileParamSlot0_F) else: return self._getParam(hal.TalonSRXParam.eProfileParamSlot1_F) def getIZone(self): + """ + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) + """ if self.profile == 0: return self._getParamInt(hal.TalonSRXParam.eProfileParamSlot0_IZone) else: @@ -514,6 +880,8 @@ def getCloseLoopRampRate(self): :returns: rampRate Maximum change in voltage, in volts / sec. :see: #setProfile For selecting a certain profile. + + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) """ if self.profile == 0: throttlePerMs = self._getParamInt(hal.TalonSRXParam.eProfileParamSlot0_CloseLoopRampRate) @@ -525,10 +893,15 @@ def getCloseLoopRampRate(self): def getFirmwareVersion(self): """ :returns: The version of the firmware running on the Talon + + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) """ return self._getParamInt(hal.TalonSRXParam.eFirmVers) def getIaccum(self): + """ + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) + """ return self._getParamInt(hal.TalonSRXParam.ePidIaccum) def clearIaccum(self): @@ -542,48 +915,36 @@ def setP(self, p): Set the proportional value of the currently selected profile. :param p: Proportional constant for the currently selected PID profile. - :see: #setProfile For selecting a certain profile. + :see: :meth:`setProfile` For selecting a certain profile. """ - if self.profile == 0: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot0_P, p) - else: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot1_P, p) + hal.TalonSRX_SetPgain(self.handle, self.profile, p) def setI(self, i): """ Set the integration constant of the currently selected profile. :param i: Integration constant for the currently selected PID profile. - :see: #setProfile For selecting a certain profile. + :see: :meth:`setProfile` For selecting a certain profile. """ - if self.profile == 0: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot0_I, i) - else: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot1_I, i) + hal.TalonSRX_SetIgain(self.handle, self.profile, i) def setD(self, d): """ Set the derivative constant of the currently selected profile. :param d: Derivative constant for the currently selected PID profile. - :see: #setProfile For selecting a certain profile. + :see: :meth:`setProfile` For selecting a certain profile. """ - if self.profile == 0: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot0_D, d) - else: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot1_D, d) + hal.TalonSRX_SetDgain(self.handle, self.profile, d) def setF(self, f): """ Set the feedforward value of the currently selected profile. :param f: Feedforward constant for the currently selected PID profile. - :see: #setProfile For selecting a certain profile. + :see: :meth:`setProfile` For selecting a certain profile. """ - if self.profile == 0: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot0_F, f) - else: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot1_F, f) + hal.TalonSRX_SetFgain(self.handle, self.profile, f) def setIZone(self, izone): """ @@ -596,12 +957,9 @@ def setIZone(self, izone): An izone value of 0 means no difference from a standard PIDF loop. :param izone: Width of the integration zone. - :see: #setProfile For selecting a certain profile. + :see: :meth:`setProfile` For selecting a certain profile. """ - if self.profile == 0: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot0_IZone, izone) - else: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot1_IZone, izone) + hal.TalonSRX_SetIzone(self.handle, self.profile, izone) def setCloseLoopRampRate(self, rampRate): """ @@ -611,14 +969,11 @@ def setCloseLoopRampRate(self, rampRate): Only affects position and speed closed loop modes. :param rampRate: Maximum change in voltage, in volts / sec. - :see: #setProfile For selecting a certain profile. + :see: :meth:`setProfile` For selecting a certain profile. """ # CanTalonSRX takes units of Throttle (0 - 1023) / 1ms. rate = int(rampRate * 1023.0 / 12.0 / 1000.0) - if self.profile == 0: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot0_CloseLoopRampRate, rate) - else: - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSlot1_CloseLoopRampRate, rate) + hal.TalonSRX_SetCloseLoopRampRate(self.handle, self.profile, rate) def setVoltageRampRate(self, rampRate): """ @@ -632,6 +987,9 @@ def setVoltageRampRate(self, rampRate): # CanTalonSRX takes units of Throttle (0 - 1023) / 10ms. rate = int(rampRate * 1023.0 / 12.0 /100.0) hal.TalonSRX_SetRampThrottle(self.handle, rate) + + def setVoltageCompensationRampRate(self, rampRate) : + hal.TalonSRX_SetVoltageCompensationRate(self.handle, rampRate / 1000) def setPID(self, p, i, d, f=0, izone=0, closeLoopRampRate=0, profile=None): @@ -671,6 +1029,9 @@ def setProfile(self, profile): """ Select which closed loop profile to use, and uses whatever PIDF gains and the such that are already there. + + :param profile: Selected profile (0 or 1) + :type profile: int """ if profile not in (0, 1): raise ValueError("Talon PID profile must be 0 or 1.") @@ -691,20 +1052,87 @@ def getDeviceID(self): # TODO: Documentation for all these accessors/setters for misc. stuff. - def setSensorPosition(self, pos): - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eSensorPosition, pos) - def setForwardSoftLimit(self, forwardLimit): - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSoftLimitForThreshold, forwardLimit) + nativeLimitPos = self._scaleRotationsToNativeUnits(self.feedbackDevice, forwardLimit) + hal.TalonSRX_SetForwardSoftLimit(self.handle, nativeLimitPos) + + def getForwardSoftLimit(self): + return hal.TalonSRX_GetForwardSoftLimit(self.handle) def enableForwardSoftLimit(self, enable): - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSoftLimitForEnable, 1 if enable else 0) + hal.TalonSRX_SetForwardSoftEnable(self.handle, 1 if enable else 0) + + def isForwardSoftLimitEnabled(self): + return hal.TalonSRX_GetForwardSoftEnable(self.handle) != 0 def setReverseSoftLimit(self, reverseLimit): - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSoftLimitRevThreshold, reverseLimit) + nativeLimitPos = self._scaleRotationsToNativeUnits(self.feedbackDevice, reverseLimit) + hal.TalonSRX_SetReverseSoftLimit(self.handle, nativeLimitPos) + + def getReverseSoftLimit(self): + return hal.TalonSRX_GetReverseSoftLimit(self.handle) def enableReverseSoftLimit(self, enable): - hal.TalonSRX_SetParam(self.handle, hal.TalonSRXParam.eProfileParamSoftLimitRevEnable, 1 if enable else 0) + hal.TalonSRX_SetReverseSoftEnable(self.handle, 1 if enable else 0) + + def isReverseSoftLimitEnabled(self): + return hal.TalonSRX_GetReverseSoftEnable(self.handle) != 0 + + def configMaxOutputVoltage(self, voltage): + """Configure the maximum voltage that the Jaguar will ever output. + + This can be used to limit the maximum output voltage in all modes so that + motors which cannot withstand full bus voltage can be used safely. + + :param voltage: The maximum voltage output by the Jaguar. + """ + self.configPeakOutputVoltage(voltage, -voltage) + + def configPeakOutputVoltage(self, forwardVoltage, reverseVoltage): + # bounds checking + if forwardVoltage > 12: + forwardVoltage = 12 + elif forwardVoltage < 0: + forwardVoltage = 0 + if reverseVoltage > 0: + reverseVoltage = 0 + elif reverseVoltage < -12: + reverseVoltage = -12 + + # config calls + self.setParameter(hal.TalonSRXParam.ePeakPosOutput,1023*forwardVoltage/12.0) + self.setParameter(hal.TalonSRXParam.ePeakNegOutput,1023*reverseVoltage/12.0) + + def configNominalOutputVoltage(self, forwardVoltage, reverseVoltage): + # bounds checking + if forwardVoltage > 12: + forwardVoltage = 12 + elif forwardVoltage < 0: + forwardVoltage = 0 + if reverseVoltage > 0: + reverseVoltage = 0 + elif reverseVoltage < -12: + reverseVoltage = -12 + + # config calls + self.setParameter(hal.TalonSRXParam.eNominalPosOutput,1023*forwardVoltage/12.0) + self.setParameter(hal.TalonSRXParam.eNominalNegOutput,1023*reverseVoltage/12.0) + + def setParameter(self, param, value): + """General set frame. Since the parameter is a general integral type, this can + be used for testing future features. + """ + hal.TalonSRX_SetParam(self.handle, param, value) + + def getParameter(self, param): + """General get frame. Since the parameter is a general integral type, this can + be used for testing future features. + + .. warning:: Calls Timer.delay(:const:`kDelayForSolicitedSignals`) + """ + hal.TalonSRX_RequestParam(self.handle, param) + Timer.delay(self.kDelayForSolicitedSignals) + return hal.TalonSRX_GetParamResponse(self.handle, param) def clearStickyFaults(self): hal.TalonSRX_ClearStickyFaults(self.handle) @@ -781,6 +1209,304 @@ def getStickyFaultForSoftLim(self): def getStickyFaultRevSoftLim(self): return hal.TalonSRX_GetStckyFault_RevSoftLim(self.handle) + # Internal helper function + + + def _getNativeUnitsPerRotationScalar(self, devToLookup): + """:returns: Number of native units per rotation if scaling info is available. + Zero if scaling information is not available. + """ + retval = 0 + scalingAvail = False + if devToLookup == self.FeedbackDevice.QuadEncoder: + # When caller wants to lookup Quadrature, the QEI may be in 1x if the selected feedback is edge counter. + # Additionally if the quadrature source is the CTRE Mag encoder, then the CPR is known. + # This is nice in that the calling app does not require knowing the CPR at all. + # So do both checks here. + qeiPulsePerCount = 4 # default to 4x + if self.feedbackDevice in [self.FeedbackDevice.CtreMagEncoder_Relative, + self.FeedbackDevice.CtreMagEncoder_Absolute]: + # we assume the quadrature signal comes from the MagEnc, + # of which we know the CPR already + retval = self.kNativePwdUnitsPerRotation + scalingAvail = True + elif self.feedbackDevice in [self.FeedbackDevice.EncRising, # Talon's QEI is setup for 1x, so perform 1x math + self.FeedbackDevice.EncFalling]: + qeiPulsePerCount = 1 + + # QuadEncoder: Talon's QEI is 4x + # pulse width and everything else, assume its regular quad use. + + + if scalingAvail: + # already deduced the scalar above, we're done. + pass + else: + # we couldn't deduce the scalar just based on the selection + if 0 == self.codesPerRev: + # caller has never set the CPR. Most likely caller + # is just using engineering units so fall to the + # bottom of this func + pass + else: + # Talon expects PPR units + retval = qeiPulsePerCount * self.codesPerRev + scalingAvail = True + + elif devToLookup in [self.FeedbackDevice.EncRising, + self.FeedbackDevice.EncFalling]: + if 0 == self.codesPerRev: + # caller has never set the CPR. Most likely caller + # is just using engineering units so fall to the + # bottom of this func. + pass + else: + # Talon expects PPR units + retval = 1 * self.codesPerRev + scalingAvail = True + + elif devToLookup in [self.FeedbackDevice.AnalogPot, + self.FeedbackDevice.AnalogEncoder]: + if 0 == self.numPotTurns: + # caller has never set the CPR. Most likely caller + # is just using engineering units so fall to the + # bottom of this func. + pass + else : + retval = self.kNativeAdcUnitsPerRotation / self.numPotTurns + scalingAvail = True + + elif devToLookup in [self.FeedbackDevice.CtreMagEncoder_Relative, + self.FeedbackDevice.CtreMagEncoder_Absolute, + self.FeedbackDevice.PulseWidth]: + retval = self.kNativePwdUnitsPerRotation + scalingAvail = True + + # if scaling info is not available give caller zero + if False == scalingAvail: + return 0 + return retval + + def _scaleRotationsToNativeUnits(self, devToLookup, fullRotations): + """:param fullRotations: double precision value representing number of rotations of selected feedback sensor. + + If user has never called the config routine for the selected sensor, then the caller + is likely passing rotations in engineering units already, in which case it is returned + as is. + + .. seealso:: :meth:`configPotentiometerTurns`, :meth:`configEncoderCodesPerRev` + + :returns: fullRotations in native engineering units of the Talon SRX firmware. + """ + + # first assume we don't have config info, prep the default return + retval = fullRotations + # retrieve scaling info + scalar = self._getNativeUnitsPerRotationScalar(devToLookup) + # apply scalar if its available + if scalar > 0: + retval = fullRotations*scalar + return int(retval) + + def _scaleVelocityToNativeUnits(self, devToLookup, rpm): + """:param rpm: double precision value representing number of rotations per minute of selected feedback sensor. + + If user has never called the config routine for the selected sensor, then the caller + is likely passing rotations in engineering units already, in which case it is returned + as is. + + .. seealso:: :meth:`configPotentiometerTurns`, :meth:`configEncoderCodesPerRev` + + :returns: sensor velocity in native engineering units of the Talon SRX firmware. + """ + + # first assume we don't have config info, prep the default return + retval = rpm + # retrieve scaling info + scalar = self._getNativeUnitsPerRotationScalar(devToLookup) + # apply scalar if its available + if scalar > 0: + retval = rpm*scalar + return int(retval) + + def _scaleNativeUnitsToRotations(self, devToLookup, nativePos): + """:param nativePos: integral position of the feedback sensor in native Talon SRX units. + If user has never called the config routine for the selected sensor, then the return + will be in TALON SRX units as well to match the behavior in the 2015 season. + + .. seealso:: :meth:`configPotentiometerTurns`, :meth:`configEncoderCodesPerRev` + + :returns: double precision number of rotations, unless config was never performed. + """ + + # first assume we don't have config info, prep the default return + retval = float(nativePos) + # retrieve scaling info + scalar = self._getNativeUnitsPerRotationScalar(devToLookup) + # apply scalar if its available + if scalar > 0: + retval = nativePos / scalar + return retval + + def _scaleNativeUnitsToRpm(self, devToLookup, nativeVel): + """:param nativeVel: integral velocity of the feedback sensor in native Talon SRX units. + If user has never called the config routine for the selected sensor, then the return + will be in TALON SRX units as well to match the behavior in the 2015 season. + + .. seealso:: :meth:`configPotentiometerTurns`, :meth:`configEncoderCodesPerRev` + + :returns: double precision of sensor velocity in RPM, unless config was never performed. + """ + + # first assume we don't have config info, prep the default return + retval = float(nativeVel) + # retrieve scaling info + scalar = self._getNativeUnitsPerRotationScalar(devToLookup) + # apply scalar if its available + if scalar > 0: + retval = nativeVel / (self.kMinutesPer100msUnit*scalar) + return retval + + def enableZeroSensorPositionOnIndex(self, enable, risingEdge): + """Enables Talon SRX to automatically zero the Sensor Position whenever an + edge is detected on the index signal. + + :param enable: boolean input, pass true to enable feature or false to disable. + :type enable: bool + :param risingEdge: boolean input, pass true to clear the position on rising edge, + + pass false to clear the position on falling edge. + :type risingEdge: bool + """ + if enable: + # enable the feature, update the edge polarity first to ensure + # it is correct before the feature is enabled. + self.setParameter(hal.TalonSRXParam.eQuadIdxPolarity, 1 if risingEdge else 0) + self.setParameter(hal.TalonSRXParam.eClearPositionOnIdx, 1) + else: + # disable the feature first, then update the edge polarity. + self.setParameter(hal.TalonSRXParam.eClearPositionOnIdx, 0) + self.setParameter(hal.TalonSRXParam.eQuadIdxPolarity, 1 if risingEdge else 0) + + def changeMotionControlFramePeriod(self, periodMs): + """Calling application can opt to speed up the handshaking between the robot API and the Talon to increase the + download rate of the Talon's Motion Profile. Ideally the period should be no more than half the period + of a trajectory point. + """ + hal.TalonSRX_ChangeMotionControlFramePeriod(self.handle, periodMs) + + def clearMotionProfileTrajectories(self): + """Clear the buffered motion profile in both Talon RAM (bottom), and in the API (top). + Be sure to check :meth:`getMotionProfileStatus` to know when the buffer is actually cleared. + """ + hal.TalonSRX_ClearMotionProfileTrajectories(self.handle) + + def getMotionProfileTopLevelBufferCount(self): + """Retrieve just the buffer count for the api-level (top) buffer. + This routine performs no CAN or data structure lookups, so its fast and ideal + if caller needs to quickly poll the progress of trajectory points being emptied + into Talon's RAM. Otherwise just use :meth:`getMotionProfileStatus`. + + :returns: number of trajectory points in the top buffer. + """ + return hal.TalonSRX_GetMotionProfileTopLevelBufferCount(self.handle) + + def pushMotionProfileTrajectory(self, trajPt): + """Push another trajectory point into the top level buffer (which is emptied into + the Talon's bottom buffer as room allows). + + :param targPos: servo position in native Talon units (sensor units). + :param targVel: velocity to feed-forward in native Talon units (sensor units per 100ms). + :param profileSlotSelect: which slot to pull PIDF gains from. Currently supports 0 or 1. + :param timeDurMs: time in milliseconds of how long to apply this point. + :param velOnly: set to nonzero to signal Talon that only the feed-foward velocity should be + used, i.e. do not perform PID on position. This is equivalent to setting + PID gains to zero, but much more efficient and synchronized to MP. + :param isLastPoint: set to nonzero to signal Talon to keep processing this trajectory point, + instead of jumping to the next one when timeDurMs expires. Otherwise + MP executer will eventuall see an empty buffer after the last point expires, + causing it to assert the IsUnderRun flag. However this may be desired + if calling application nevers wants to terminate the MP. + :param zeroPos: set to nonzero to signal Talon to "zero" the selected position sensor before executing + this trajectory point. Typically the first point should have this set only thus allowing + the remainder of the MP positions to be relative to zero. + + :returns: True if trajectory point push ok. False if buffer is full due to kMotionProfileTopBufferCapacity. + """ + # check if there is room + if self.isMotionProfileTopLevelBufferFull(): + return False + + # convert position and velocity to native units + targPos = self._scaleRotationsToNativeUnits(self.feedbackDevice, trajPt.position) + targVel = self._scaleVelocityToNativeUnits(self.feedbackDevice, trajPt.velocity) + # bounds check signals that require it + profileSlotSelect = 1 if trajPt.profileSlotSelect > 0 else 0 + timeDurMs = trajPt.timeDurMs + # cap time to [0ms, 255ms], 0 and 1 are both interpreted as 1ms. + if timeDurMs > 255: + timeDurMs = 255 + if timeDurMs < 0: + timeDurMs = 0 + # send it to the top level buffer + hal.TalonSRX_PushMotionProfileTrajectory(self.handle, targPos, targVel, profileSlotSelect, timeDurMs, + 1 if trajPt.velocityOnly else 0, + 1 if trajPt.isLastPoint else 0, + 1 if trajPt.zeroPos else 0) + return True + + def isMotionProfileTopLevelBufferFull(self): + """:returns: true if api-level (top) buffer is full.""" + return hal.TalonSRX_IsMotionProfileTopLevelBufferFull(self.handle) + + def processMotionProfileBuffer(self): + """This must be called periodically to funnel the trajectory points from the API's top level buffer to + the Talon's bottom level buffer. Recommendation is to call this twice as fast as the executation rate of the motion profile. + So if MP is running with 20ms trajectory points, try calling this routine every 10ms. All motion profile functions are thread-safe + through the use of a mutex, so there is no harm in having the caller utilize threading. + """ + hal.TalonSRX_ProcessMotionProfileBuffer(self.handle) + + def getMotionProfileStatus(self, motionProfileStatus): + """Retrieve all Motion Profile status information. + + Since this all comes from one CAN frame, its ideal to have one routine to retrieve the frame once and decode everything. + + :param motionProfileStatus: contains all progress information on the currently running MP. Caller should + must instantiate the motionProfileStatus object first then pass into this routine to be filled. + :type motionProfileStatus: :class:`.MotionProfileStatus` + """ + if motionProfileStatus.activePoint is None: + motionProfileStatus.activePoint = self.TrajectoryPoint() + + flags, \ + motionProfileStatus.activePoint.profileSlotSelect, \ + targPos, targVel, \ + motionProfileStatus.topBufferRem, \ + motionProfileStatus.topBufferCnt, \ + motionProfileStatus.btmBufferCnt, \ + motionProfileStatus.outputEnable = \ + hal.TalonSRX_GetMotionProfileStatus(self.handle) + + motionProfileStatus._flags = flags + motionProfileStatus.activePoint.isLastPoint = (flags & hal.TalonSRXConst.kMotionProfileFlag_ActTraj_IsLast) > 0 + motionProfileStatus.activePoint.velocityOnly = (flags & hal.TalonSRXConst.kMotionProfileFlag_ActTraj_VelOnly) > 0 + motionProfileStatus.activePoint.position = self._scaleNativeUnitsToRotations(self.feedbackDevice, targPos) + motionProfileStatus.activePoint.velocity = self._scaleNativeUnitsToRpm(self.feedbackDevice, targVel) + motionProfileStatus.activePoint.zeroPos = False # this signal is only used sending pts to Talon + motionProfileStatus.activePoint.timeDurMs = 0 # this signal is only used sending pts to Talon + + def clearMotionProfileHasUnderrun(self): + """Clear the hasUnderrun flag in Talon's Motion Profile Executer when MPE is ready for another point, + but the low level buffer is empty. + + Once the Motion Profile Executer sets the hasUnderrun flag, it stays set until + Robot Application clears it with this routine, which ensures Robot Application + gets a chance to instrument or react. Caller could also check the isUnderrun flag + which automatically clears when fault condition is removed. + """ + self.setParameter(hal.TalonSRXParam.eMotionProfileHasUnderrunErr, 0) + def getDescription(self): return "CANTalon ID %d" % self.deviceNumber