! EncZoomSdoSubs.scr
	! Encoded Zoom SDO Subroutines
		
	! -----------------------------------------------------------------------------------------------
	SDO_GET_CUR_POS_AVG:
		! Read the current position register, sets var SdoCurPos
		_Sum = 0
		for _i% = 1 to SdoPosAvgCount%
			_Pos% = readsdoint32(SdoZoomCanNode%, SdoCurPosIndex%, SdoCurPosSubindex%)
			_Sum = _Sum + _Pos%
		next _i%
		SdoCurPos = _Sum / SdoPosAvgCount%
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_GET_SET_POS:
		! Read the set position register, sets var SdoSetPos
		SetPos = readsdoint32(SdoZoomCanNode%, SdoSetPosIndex%, SdoCurSetSubindex%)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_SET_MOTION_MODE:
		! Sets motion mode, uses var SdoMotionMode%
		writesdoint8(SdoZoomCanNode%, SdoMotionModeIndex%, SdoMotionModeSubindex%, SdoMotionMode%)
		return
	
	! -----------------------------------------------------------------------------------------------
	SDO_SET_MOTION_MODE_WAND:
		SdoMotionMode% = 3
		gosub SDO_SET_MOTION_MODE ! Uses var SdoMotionMode%
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_SET_MOTION_MODE_PROG:
		SdoMotionMode% = 4
		gosub SDO_SET_MOTION_MODE ! Uses var SdoMotionMode%
		return

	! -----------------------------------------------------------------------------------------------
	SDO_GET_STATUS1:
		! Get value of status register 1 into var Status1%
		Status1% = readsdoint8(SdoZoomCanNode%, SdoStatus1Index%, SdoStatus1SubIndex%)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_WAIT_FOR_STOP:
		! Wait for the motor moving status bit to clear
		_t1 = timedate
	SDO_WAIT_FOR_STOP_LOOP:
		gosub SDO_GET_STATUS1 ! Sets var Status1%
		if bitand(Status1%, SdoStatus1MotorMovingBit%) = 0 then
			return
		endif
		_t2 = timedate
		if (_t2 - _t1) > SdoMaxMoveTime then
			msg$ = " Timeout waiting for stop "
			gosub SHOW_ERROR_DIALOG
			stop
		endif
		wait 0.25
		goto SDO_WAIT_FOR_STOP_LOOP
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_GET_BACKLASH_COMP:
		! Gets firmware backlash compensation value
		! Sets var BacklashComp
		BacklashComp = readsdoint16(SdoZoomCanNode%, BacklashIndex%, BacklashSubindex%)
		return
	
	! -----------------------------------------------------------------------------------------------
	SDO_SET_BACKLASH_COMP:
		! Sets firmware backlash compensation value
		! Uses var BacklashComp
		writesdoint16(SdoZoomCanNode%, BacklashIndex%, BacklashSubindex%, BacklashComp)
		return
	
	! -----------------------------------------------------------------------------------------------
	SDO_DISABLE_BACKLASH_COMP:
		! Disables firmware backlash compensation
		BacklashComp = 0
		gosub SDO_SET_BACKLASH_COMP ! Uses var BacklashComp
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_SET_TARG_CURRENT:
		! Sets target current, uses var TargCurrent%
		writesdoint16(SdoZoomCanNode%, SdoCurrentIndex%, SdoCurrentSubindex%, TargCurrent%)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_DOWN_BUTTON:
		! Simulates wand Zoom Down keypress
		writesdoint32(SdoZoomCanNode%, SdoKeypressIndex%, SdoKeypressSubindex%, SdoKeypressZoomDown%)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_UP_BUTTON:
		! Simulates wand Zoom Up keypress
		writesdoint32(SdoZoomCanNode%, SdoKeypressIndex%, SdoKeypressSubindex%, SdoKeypressZoomUp%)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_NO_BUTTON:
		! Simulates wand Zoom Up keypress
		writesdoint32(SdoZoomCanNode%, SdoKeypressIndex%, SdoKeypressSubindex%, 0)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_UP_TO_LIMIT:
		gosub SDO_WAND_ZOOM_UP_BUTTON
		wait 1
		gosub SDO_WAIT_FOR_STOP
		gosub SDO_WAND_ZOOM_NO_BUTTON
		return
	
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_DOWN_TO_LIMIT:
		gosub SDO_WAND_ZOOM_DOWN_BUTTON
		wait 1
		gosub SDO_WAIT_FOR_STOP
		gosub SDO_WAND_ZOOM_NO_BUTTON
		return
	
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_UP_RANDOM:
		gosub SDO_WAND_ZOOM_UP_BUTTON
		gosub SDO_WAND_ZOOM_RANDOM_WAIT
		gosub SDO_WAND_ZOOM_NO_BUTTON
		gosub SDO_WAIT_FOR_STOP
		return
	
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_DOWN_RANDOM:
		gosub SDO_WAND_ZOOM_DOWN_BUTTON
		gosub SDO_WAND_ZOOM_RANDOM_WAIT
		gosub SDO_WAND_ZOOM_NO_BUTTON
		gosub SDO_WAIT_FOR_STOP
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_WAND_ZOOM_RANDOM_WAIT:
		wait SdoWandZoomRandomWaitMin + ((SdoWandZoomRandomWaitMax - SdoWandZoomRandomWaitMin) * rnd)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_SET_LOWER_SOFT_LIMIT:
		! Sets software negative limit, uses var LowerSoftLimit%
		writesdoint32(SdoZoomCanNode%, SdoSoftNegLimitIndex%, SdoSoftNegLimitSubindex%, LowerSoftLimit%)
		return

	! -----------------------------------------------------------------------------------------------
	SDO_GET_LOWER_SOFT_LIMIT:
		! Gets software negative limit, sets var LowerSoftLimit%
		LowerSoftLimit% = readsdoint32(SdoZoomCanNode%, SdoSoftNegLimitIndex%, SdoSoftNegLimitSubindex%)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_SET_UPPER_SOFT_LIMIT:
		! Sets software positive limit, uses var UpperSoftLimit%
		writesdoint32(SdoZoomCanNode%, SdoSoftPosLimitIndex%, SdoSoftPosLimitSubindex%, UpperSoftLimit%)
		return

	! -----------------------------------------------------------------------------------------------
	SDO_GET_UPPER_SOFT_LIMIT:
		! Gets software positive limit, sets var UpperSoftLimit%
		UpperSoftLimit% = readsdoint32(SdoZoomCanNode%, SdoSoftPosLimitIndex%, SdoSoftPosLimitSubindex%)
		return
		
	! -----------------------------------------------------------------------------------------------
	SDO_DISABLE_SOFT_LIMITS:
		! Disables software limits
		LowerSoftLimit% = 0
		gosub SDO_SET_LOWER_SOFT_LIMIT
		UpperSoftLimit% = 0
		gosub SDO_SET_UPPER_SOFT_LIMIT
		return