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

KeyJog_Speed = 20

Mot1Min = 0
Mot1Max = 650
Mot2Min = 0
Mot2Max = 650

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

Key Right		Gosub ExecKeys 
Key Left 		Gosub ExecKeys
Key Up 			Gosub ExecKeys
Key Down 		Gosub ExecKeys

'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 Mot1Min
Variable Numeric Mot1Max
Variable Numeric Mot2Min
Variable Numeric Mot2Max

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

Label Test2
	
Return

Label Loop
	
	MoveSlow_Dest1 = 600
	MoveSlow_Dest2 = 10
	MoveSlow_Speed = 100
	Gosub MoveSlow
	
	MoveSlow_Dest1 = 600
	MoveSlow_Dest2 = 600
	MoveSlow_Speed = 100
	Gosub MoveSlow
	
	MoveSlow_Dest1 = 100
	MoveSlow_Dest2 = 600
	MoveSlow_Speed = 100
	Gosub MoveSlow
	
Goto Loop


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

Variable Numeric MSV1
Variable Numeric MSV2
Variable Numeric MSV3
Variable Numeric MSV4

Label MoveSlow
	' ----------------------------------- test for out of limit
	Slot(SlotMotor1) = Limit(Slot(SlotMotor1), Mot1Min, Mot1Max)
	Slot(SlotMotor2) = Limit(Slot(SlotMotor2), Mot2Min, Mot2Max)
	' ----------------------------------- 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)
	MSV3 = MSV1 * MSV1
	MSV3 = MSV3 + MSV2 * MSV2
	MSV3 = Sqrt(MSV3)
	' ----------------------------------- calc the loops count
	MSV4 = MSV3 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(SlotMotor1) = Limit(MoveSlow_Dest1, Mot1Min, Mot1Max)
		Slot(SlotMotor2) = Limit(MoveSlow_Dest2, Mot2Min, Mot2Max)
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlow_Dest1 - Slot(SlotMotor1)) / MSV4
	MSV2 = (MoveSlow_Dest2 - Slot(SlotMotor2)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(SlotMotor1) = Limit(Slot(SlotMotor1) + MSV1, Mot1Min, Mot1Max)
		Slot(SlotMotor2) = Limit(Slot(SlotMotor2) + MSV2, Mot2Min, Mot2Max)
		Wait Seconds 0.01 '<<< loop time
		Gosub PrintValues
	Next
	'Gosub WaitAllMotorsAndRecovery
	Gosub WaitAllMotors
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


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


' ====================================================================
'  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("Right")
		Slot SlotMotor1 = Limit(Slot(SlotMotor1) + JogSpeed, Mot1Min, Mot1Max)
	EndIf
	If Key("Left")
		Slot SlotMotor1 = Limit(Slot(SlotMotor1) - JogSpeed, Mot1Min, Mot1Max)
	EndIf
	'
	If Key("Up")
		Slot SlotMotor2 = Limit(Slot(SlotMotor2) + JogSpeed, Mot2Min, Mot2Max)
	EndIf
	If Key("Down")
		Slot SlotMotor2 = Limit(Slot(SlotMotor2) - JogSpeed, Mot2Min, Mot2Max)
	EndIf
	'
	Gosub PrintValues
Return


' ====================================================================
'  PRINT VALUES
' ====================================================================
Label PrintValues
	s1 =       "X=" + Format(Slot(SlotMotor1), "0.000")
	s1 = s1 + " Y=" + Format(Slot(SlotMotor2), "0.000")
	Print CRLF + 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
