
SlotMotor1 = 1
SlotMotor2 = 3
SlotSwitchStart1 = 11
SlotSwitchEnd1 = 7
SlotSwitchStart2 = 12
SlotSwitchEnd2 = 8

Button 1 Text HOME MOTORS
Button 2 Text Test1
Button 3 Text Test2
Button 5 Text Loop

Key Right		Gosub MovePos1
Key Left 		Gosub MoveNeg1
Key Up 			Gosub MovePos2
Key Down 		Gosub MoveNeg2

'Gosub "HOME MOTORS"

Load Vars
Pos1 = v1
Pos2 = v2
Gosub PrintValues

Stop

Label EventStop
	v1 = Pos1
	v2 = Pos2
	Save Vars
End

' ==========================================
'  VARS
' ==========================================
Variable Numeric SlotMotor1
Variable Numeric SlotMotor2
Variable Numeric SlotSwitchStart1
Variable Numeric SlotSwitchEnd1
Variable Numeric SlotSwitchStart2
Variable Numeric SlotSwitchEnd2
Variable Numeric Pos1
Variable Numeric Pos2

' ====================================================================
' ENABLE/DISABLE BUTTONS
' ====================================================================
Label EnableAllButtons
	For v1 = 1 To 32
		Button v1 Enabled
	Next
Return

Label DisableAllButtons
	For v1 = 1 To 32
		Button v1 Disabled
	Next
Return


' ==========================================
'  LOOP TEST
' ==========================================
'
Label Test1
	Pos1 = 177.69
	Gosub MoveMotors
	Wait Seconds 3
Return

Label Test2
	Pos1 = 46
	Gosub MoveMotors
	Wait Seconds 3
Return

Label Loop
	
	Pos1 = 414.69
	Pos2 = -68.70
	Gosub MoveMotors
	
	Pos1 = 404.69
	Pos2 = -20
	Gosub MoveMotors
	
Goto Loop


' ==========================================================
'  MOVE MOTORS
' ==========================================================
Label MoveMotors
	Slot (SlotMotor1) = Pos1
	Slot (SlotMotor2) = Pos2
	SlotMotor = SlotMotor1
	Gosub WaitMotor
	SlotMotor = SlotMotor2
	Gosub WaitMotor
	Gosub PrintValues
Return

' ==========================================================
'  HOME MOTORS
' ==========================================================
Label HOME MOTORS
	SlotMotor = SlotMotor1
	SlotSensor = SlotSwitchStart1
	SpeedSearch = -0.25     ' search direction and speed
	SpeedReturn = +0.05     ' return direction and speed
	DeclaredSwitchPos = 0
	InitialPosition = 5     ' relative to DeclaredZero
	Gosub HomeMotor
	
	SlotMotor = SlotMotor2
	SlotSensor = SlotSwitchStart2
	SpeedSearch = -0.25    ' search direction and speed
	SpeedReturn = +0.05    ' returndirection and speed
	DeclaredSwitchPos = 0
	InitialPosition = 5    ' relative to DeclaredZero
	Gosub HomeMotor
	
	Gosub PrintValues
Return

' ==========================================================
'  Home the Motor(SlotMotor) to the Sensor(SlotSensor)
' ==========================================================
Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredSwitchPos
Variable Numeric InitialPosition

Label HomeMotor
	Gosub DisableAllButtons
	Button 15 Enabled
	' ------------------------- detach if sensor is active
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
    ' ------------------------- search the sensor
Label SearchHome
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
	Wait Seconds 0.001
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		Slot(SlotMotor) = Reset
		Wait Seconds 0.1
		Slot(SlotMotor) = 0
		Wait Seconds 0.1
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' ------------------------- detach from the sensor
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Slot(SlotMotor) = Reset
		Wait Seconds 0.1
		Slot(SlotMotor) = DeclaredSwitchPos
		Wait Seconds 0.1
		Goto SetFinalValues
	EndIf
Goto DetachSensor
    ' ------------------------- reset and move to InitialPos
Label SetFinalValues
	Slot(SlotMotor) = InitialPosition
	Gosub WaitMotor
	Gosub EnableAllButtons
Return

' ====================================================================
'   Wait until the motor(SlotMotor) is stopped
' ====================================================================
Label WaitMotor
	Wait Seconds 0.1
	Variable Numeric WaitMot1
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return


' ================================================
'  JOG WITH keyboard
' ================================================
Label MovePos1
	Gosub SpeedModifiers
	Pos1 = Pos1 + JogSpeed
	Slot(SlotMotor1) = Pos1
	Gosub PrintValues
Return

Label MoveNeg1
	Gosub SpeedModifiers
	Pos1 = Pos1 - JogSpeed
	Slot(SlotMotor1) = Pos1
	Gosub PrintValues
Return

Label MovePos2
	Gosub SpeedModifiers
	Pos2 = Pos2 + JogSpeed
	Slot(SlotMotor2) = Pos2
	Gosub PrintValues
Return

Label MoveNeg2
	Gosub SpeedModifiers
	Pos2 = Pos2 - JogSpeed
	Slot(SlotMotor2) = Pos2
	Gosub PrintValues
Return

Label PrintValues
	Print "Pos1 = " + Format(Pos1, "0.00") + "  Pos2 = " + + Format(Pos2, "0.00")
Return

Variable Numeric JogSpeed
Label SpeedModifiers
	JogSpeed = 0.1
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 100
		Case Key("CTRL")
			JogSpeed = JogSpeed * 10
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.1
	EndSelect
Return


'WATCH Speed
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'WATCH Wait Seconds 2
'EXEC Slot(2) = 0
