
' ====================================================================
'  INITIALIZATION - DO NOT MODIFY
' ====================================================================
Gosub InitAll
Stop


' ====================================================================
'  WORKING LOOP
' ====================================================================
Label Loop
	MoveSlow_Dest1     = 400
	MoveSlow_Dest2     = 300
	MoveSlow_DestRot1  = -20
	MoveSlow_DestRot2  = 40
	MoveSlow_DestInch1 = -40
	MoveSlow_DestInch2 = 10
	MoveSlow_Speed     = 200
	Gosub MoveSlow
	Wait Seconds 2
	'
	MoveSlow_Dest1     = 400
	MoveSlow_Dest2     = 300
	MoveSlow_DestRot1  = 20
	MoveSlow_DestRot2  = 20
	MoveSlow_DestInch1 = 100
	MoveSlow_DestInch2 = -10
	MoveSlow_Speed     = 200
	Gosub MoveSlow
	Wait Seconds 2
	'
	MoveSlow_Dest1     = 570
	MoveSlow_Dest2     = 142
	MoveSlow_DestRot1  = 20
	MoveSlow_DestRot2  = 20
	MoveSlow_DestInch1 = 0
	MoveSlow_DestInch2 = 0
	MoveSlow_Speed     = 200
	Gosub MoveSlow
	Wait Seconds 2
Goto Loop


' ====================================================================
'  TEST FUNCTIONS
' ====================================================================
Label Test1
	MoveSlow_Dest1     = 400
	MoveSlow_Dest2     = 250
	MoveSlow_DestRot1  = 25
	MoveSlow_DestRot2  = 35
	MoveSlow_DestInch1 = -10
	MoveSlow_DestInch2 = 10
	MoveSlow_Speed     = 300
	Gosub MoveSlow
	'
	MoveSlow_Dest1     = 200
	MoveSlow_Dest2     = 500
	MoveSlow_DestRot1  = 35
	MoveSlow_DestRot2  = 25
	MoveSlow_DestInch1 = 10
	MoveSlow_DestInch2 = -10
	MoveSlow_Speed     = 300
	Gosub MoveSlow
Goto Test1

Label Test2
	
Return


' ####################################################################
'  END OF USER AREA - DO NOT MODIFY THE FOLLOWING PART
' ####################################################################

Label InitAll
	' -------------------------------------- Execution speed
	Option Speed 9
	' -------------------------------------- Speed for keyboard Jog
	KeyJog_Speed = 20
	' -------------------------------------- Slots
	SlotMotor1 = 1
	SlotMotor2 = 3
	SlotSwitchStart1 = 5
	SlotSwitchStart2 = 6
	SlotSwitchEnd1 = 7
	SlotSwitchEnd2 = 8
	SlotMotorRot1 = 9
	SlotMotorInch1 = 10
	SlotMotorRot2 = 11
	SlotMotorInch2 = 12
	' -------------------------------------- Limits Min and Max
	Mot1Min = 0
	Mot1Max = 650
	Mot2Min = 0
	Mot2Max = 650
	Mot1RotMin = -90
	Mot1RotMax = +90
	Mot2RotMin = -90
	Mot2RotMax = +90
	Mot1InchMin = -90
	Mot1InchMax = +90
	Mot2InchMin = -90
	Mot2InchMax = +90
	' -------------------------------------- Buttons
	Button 1 Text HOME MOTORS
	Button 2 Text Loop
	
	Button 4 Text Test1
	Button 5 Text Test2
	' -------------------------------------- Init Keys
	Gosub InitKeys
	' -------------------------------------- Ensure motor stopped
	Gosub WaitAllMotors
	' -------------------------------------- Restore old motor positions
	Gosub RestorePositions
	' -------------------------------------- Eventually do HOME MOTORS and LOOP
	'Gosub HOME MOTORS
	'Gosub Loop
Return

' ====================================================================
'  VARS
' ====================================================================
Variable Numeric SlotMotor1
Variable Numeric SlotMotor2
Variable Numeric SlotMotorRot1
Variable Numeric SlotMotorRot2
Variable Numeric SlotMotorInch1
Variable Numeric SlotMotorInch2
Variable Numeric SlotSwitchStart1
Variable Numeric SlotSwitchEnd1
Variable Numeric SlotSwitchStart2
Variable Numeric SlotSwitchEnd2

Variable Numeric Mot1Min
Variable Numeric Mot1Max
Variable Numeric Mot1RotMin
Variable Numeric Mot1RotMax
Variable Numeric Mot1InchMin
Variable Numeric Mot1InchMax

Variable Numeric Mot2Min
Variable Numeric Mot2Max
Variable Numeric Mot2RotMin
Variable Numeric Mot2RotMax
Variable Numeric Mot2InchMin
Variable Numeric Mot2InchMax

' ====================================================================
' 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

' =====================================================================
'  CLOSING EVENTS
' =====================================================================
Label EventStop
	Gosub SavePositions
	Gosub EnableAllButtons
End

' ====================================================================
'  SAVE and RESTORE POSITIONS
' ====================================================================
Label SavePositions
	v1 = Slot(SlotMotor1)
	v2 = Slot(SlotMotor2)
	v3 = Slot(SlotMotorRot1)
	v4 = Slot(SlotMotorRot2)
	Save Vars
Return

Label RestorePositions
	Load Vars
	If v1 <> 0 And v2 <> 0 And v3 <> 0 And v4 = 0
		Slot(SlotMotor1) = Reset
		Wait Seconds 0.01
		Slot(SlotMotor1) = v1
		Wait Seconds 0.01
		'
		Slot(SlotMotor2) = Reset
		Wait Seconds 0.01
		Slot(SlotMotor2) = v2
		Wait Seconds 0.01
		'
		Slot(SlotMotorRot1) = Reset
		Wait Seconds 0.01
		Slot(SlotMotorRot1) = v3
		Wait Seconds 0.01
		'
		Slot(SlotMotorRot2) = Reset
		Wait Seconds 0.01
		Slot(SlotMotorRot2) = v4
		Wait Seconds 0.01
	EndIf
	Gosub PrintValues
Return

' ====================================================================
'  MOVE SLOW XY
' ====================================================================
Variable Numeric MoveSlow_Dest1 	 ' destination in mm
Variable Numeric MoveSlow_Dest2 	 ' destination in mm
Variable Numeric MoveSlow_DestRot1   ' destination in mm
Variable Numeric MoveSlow_DestRot2   ' destination in mm
Variable Numeric MoveSlow_DestInch1  ' destination in mm
Variable Numeric MoveSlow_DestInch2  ' destination in mm
Variable Numeric MoveSlow_Speed 	 ' speed in mm/sec

Variable Numeric MSV1
Variable Numeric MSV2
Variable Numeric MSVR1
Variable Numeric MSVR2
Variable Numeric MSVI1
Variable Numeric MSVI2
Variable Numeric MSV7
Variable Numeric MSV8

Label MoveSlow
	' ----------------------------------- test for out of limit
	Slot(SlotMotor1) = Limit(Slot(SlotMotor1), Mot1Min, Mot1Max)
	Slot(SlotMotor2) = Limit(Slot(SlotMotor2), Mot2Min, Mot2Max)
	Slot(SlotMotorRot1) = Limit(Slot(SlotMotorRot1), Mot1RotMin, Mot1RotMax)
	Slot(SlotMotorRot2) = Limit(Slot(SlotMotorRot2), Mot2RotMin, Mot2RotMax)
	Slot(SlotMotorInch1) = Limit(Slot(SlotMotorInch1), Mot1InchMin, Mot1InchMax)
	Slot(SlotMotorInch2) = Limit(Slot(SlotMotorInch2), Mot2InchMin, Mot2InchMax)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_Dest1 - Slot(SlotMotor1)
	MSV2 = MoveSlow_Dest2 - Slot(SlotMotor2)
	MSVR1 = MoveSlow_DestRot1 - Slot(SlotMotorRot1)
	MSVR2 = MoveSlow_DestRot2 - Slot(SlotMotorRot2)
	MSVI1 = MoveSlow_DestInch1 - Slot(SlotMotorInch1)
	MSVI2 = MoveSlow_DestInch2 - Slot(SlotMotorInch2)
	
	MSV7 = MSV1 * MSV1
	MSV7 = MSV7 + MSV2 * MSV2
	MSV7 = MSV7 + MSVR1 * MSVR1
	MSV7 = MSV7 + MSVR2 * MSVR2
	MSV7 = MSV7 + MSVI1 * MSVI1
	MSV7 = MSV7 + MSVI2 * MSVI2
	MSV7 = Sqrt(MSV7)
	' ----------------------------------- calc the loops count
	MSV8 = MSV7 / 0.01 '<<< loop time
	MSV8 = Int(MSV8 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV8 = 0
		Slot(SlotMotor1) = Limit(MoveSlow_Dest1, Mot1Min, Mot1Max)
		Slot(SlotMotor2) = Limit(MoveSlow_Dest2, Mot2Min, Mot2Max)
		Slot(SlotMotorRot1) = Limit(MoveSlow_DestRot1, Mot1RotMin, Mot1RotMax)
		Slot(SlotMotorRot2) = Limit(MoveSlow_DestRot2, Mot2RotMin, Mot2RotMax)
		Slot(SlotMotorInch1) = Limit(MoveSlow_DestInch1, Mot1InchMin, Mot1InchMax)
		Slot(SlotMotorInch2) = Limit(MoveSlow_DestInch2, Mot2InchMin, Mot2InchMax)
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlow_Dest1 - Slot(SlotMotor1)) / MSV8
	MSV2 = (MoveSlow_Dest2 - Slot(SlotMotor2)) / MSV8
	MSVR1 = (MoveSlow_DestRot1 - Slot(SlotMotorRot1)) / MSV8
	MSVR2 = (MoveSlow_DestRot2 - Slot(SlotMotorRot2)) / MSV8
	MSVI1 = (MoveSlow_DestInch1 - Slot(SlotMotorInch1)) / MSV8
	MSVI2 = (MoveSlow_DestInch2 - Slot(SlotMotorInch2)) / MSV8
	' ----------------------------------- loop for MSV6 times
	For MSV7 = 1 To MSV8
		' ------------------------------- increment, move and wait
		Slot(SlotMotor1) = Limit(Slot(SlotMotor1) + MSV1, Mot1Min, Mot1Max)
		Slot(SlotMotor2) = Limit(Slot(SlotMotor2) + MSV2, Mot2Min, Mot2Max)
		Slot(SlotMotorRot1) = Limit(Slot(SlotMotorRot1) + MSVR1, Mot1RotMin, Mot1RotMax)
		Slot(SlotMotorRot2) = Limit(Slot(SlotMotorRot2) + MSVR2, Mot2RotMin, Mot2RotMax)
		Slot(SlotMotorInch1) = Limit(Slot(SlotMotorInch1) + MSVI1, Mot1InchMin, Mot1InchMax)
		Slot(SlotMotorInch2) = Limit(Slot(SlotMotorInch2) + MSVI2, Mot2InchMin, Mot2InchMax)
		Wait Seconds 0.01 '<<< loop time
		Gosub PrintValues
	Next
	Gosub WaitAllMotors
Return

' ====================================================================
'  HOME MOTORS
' ====================================================================
Label HOME MOTORS
	'
	Slot(SlotMotorRot1) = 0
	Slot(SlotMotorRot2) = 0
	Slot(SlotMotorInch1) = 0
	Slot(SlotMotorInch2) = 0
	'
	SlotMotor = SlotMotor1
	SlotSensor = SlotSwitchStart1
	SpeedSearch = -0.25     ' search direction and speed
	SpeedReturn = +0.05     ' return direction and speed
	DeclaredSwitchPos = 0
	InitialPosition = 5		' relative to DeclaredSwitchPos
	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 DeclaredSwitchPos
	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
	' ------------------------- 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


' ====================================================================
'   Wait until All-Motors are stopped
' ====================================================================
Label WaitAllMotors
	Wait Seconds 0.1 ' <<< ensures that motors are really started
	Variable Numeric WaitMot2
	For WaitMot2 = 1 To 9e99
		If Abs(Slot(SlotMotor1 + 1)) < 0.01 And Abs(Slot(SlotMotor2 + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return


' ====================================================================
'  INIT KEYS
' ====================================================================
Label InitKeys
	Key Q	Gosub ExecKeys
	Key A 	Gosub ExecKeys
	Key W 	Gosub ExecKeys
	Key S 	Gosub ExecKeys
	Key E 	Gosub ExecKeys
	Key D 	Gosub ExecKeys
	Key R	Gosub ExecKeys
	Key F 	Gosub ExecKeys
	Key T 	Gosub ExecKeys
	Key G 	Gosub ExecKeys
	Key Y 	Gosub ExecKeys
	Key H 	Gosub ExecKeys
Return

' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	Variable Numeric KeyJog_Speed
	Variable Numeric JogSpeed
	JogSpeed = KeyJog_Speed / 100
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 100
		Case Key("CTRL")
			JogSpeed = JogSpeed * 10
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.1
	EndSelect
	'
	If Key("Q")
		Slot SlotMotor1 = Limit(Slot(SlotMotor1) + JogSpeed, Mot1Min, Mot1Max)
	EndIf
	If Key("A")
		Slot SlotMotor1 = Limit(Slot(SlotMotor1) - JogSpeed, Mot1Min, Mot1Max)
	EndIf
	'
	If Key("W")
		Slot SlotMotor2 = Limit(Slot(SlotMotor2) + JogSpeed, Mot2Min, Mot2Max)
	EndIf
	If Key("S")
		Slot SlotMotor2 = Limit(Slot(SlotMotor2) - JogSpeed, Mot2Min, Mot2Max)
	EndIf
	'
	If Key("E")
		Slot SlotMotorRot1 = Limit(Slot(SlotMotorRot1) + JogSpeed, Mot1RotMin, Mot1RotMax)
	EndIf
	If Key("D")
		Slot SlotMotorRot1 = Limit(Slot(SlotMotorRot1) - JogSpeed, Mot1RotMin, Mot1RotMax)
	EndIf
	'
	If Key("R")
		Slot SlotMotorRot2 = Limit(Slot(SlotMotorRot2) + JogSpeed, Mot2RotMin, Mot2RotMax)
	EndIf
	If Key("F")
		Slot SlotMotorRot2 = Limit(Slot(SlotMotorRot2) - JogSpeed, Mot2RotMin, Mot2RotMax)
	EndIf
		'
	If Key("T")
		Slot SlotMotorInch1 = Limit(Slot(SlotMotorInch1) + JogSpeed, Mot1InchMin, Mot1InchMax)
	EndIf
	If Key("G")
		Slot SlotMotorInch1 = Limit(Slot(SlotMotorInch1) - JogSpeed, Mot1InchMin, Mot1InchMax)
	EndIf
	'
	If Key("Y")
		Slot SlotMotorInch2 = Limit(Slot(SlotMotorInch2) + JogSpeed, Mot2InchMin, Mot2InchMax)
	EndIf
	If Key("H")
		Slot SlotMotorInch2 = Limit(Slot(SlotMotorInch2) - JogSpeed, Mot1InchMin, Mot2InchMax)
	EndIf
	'
	Gosub PrintValues
Return

' ====================================================================
'  PRINT VALUES
' ====================================================================
Label PrintValues
	s1 =      "Motor1=" + Format(Slot(SlotMotor1), "0.0")
	s1 = s1 + "   MotorRot1=" + Format(Slot(SlotMotorRot1), "0.0")
	s1 = s1 + "   MotorInch1=" + Format(Slot(SlotMotorInch1), "0.0") + CRLF
	s1 = s1 + "Motor2=" + Format(Slot(SlotMotor2), "0.0")
	s1 = s1 + "   MotorRot2=" + Format(Slot(SlotMotorRot2), "0.0")
	s1 = s1 + "   MotorInch2=" + Format(Slot(SlotMotorInch2), "0.0")
	Print s1
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
