'===================================================================
'  RECOLOR SPEED TESTER - 1950 LINES - NEW TIMES ONLY
'===================================================================

' ------- ColorAllLines_StartingWith_And_Containing       41 mS
Label pippo
Return

' ------- ColorAllLines_StartingWith                      46 mS
For v1 = 0 To 9
Next

' ------- ColorAllLines_StartingWith                      40 mS
Variable String Info
Variable String Result

' ------- ColorAllLines_StartingWith                      80 mS
If v1 = v2
EndIf


'===================================================================

For v1 = 0 To 3
	v1 = 4
Next

Button 1 Text Home
Button 1 Text "Home"

Option Speed 9
Button 1 Text "Home M0(X)"
Button 2 Text "Home M1(Y)"
Button 3 Text "Home M2(Z)"
Button 4 Text "Home M3(A)"

Button 7 Text "START TEST"
Button 8 Text "Lower Handler"

Load Theremino_HAL\Theremino_HAL.exe
Load Theremino_HAL\Theremino_SlotViewer.exe
'Load Theremino_HAL\Theremino_Terminal.exe
'Load Theremino_HAL\Theremino_SlotsToSpeech.exe
'Load Theremino_HAL\Theremino_MicroVideo.exe
'Load Theremino_HAL\Theremino_SignalScope.exe

Variable Numeric DigitTime
Variable Numeric NumTest
Variable Numeric NumCycles

Variable Numeric Coord_Z
Variable Numeric Coord_A
Variable Numeric Coord_XP
Variable Numeric Coord_XR

Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredZero
Variable Numeric InitialPosition

Variable String Info
Variable String Result

DigitTime = 1.4

Save StringToFile s9 s2			'<<<<<<<<<<<<<<

Key Right		Gosub MoveRightM0
Key Left 		Gosub MoveLeftM0
Key Up 			Gosub MoveRightM1
Key Down 		Gosub MoveLeftM1
Key Pageup		Gosub MoveUpM2
Key Pagedown	Gosub MoveDownM2
Key S			Gosub "START_TEST"

Gosub RED
Stop

' ===============  P I N    T E S T  ==================================
Label "START_TEST"
	Save StringToFile "-=-=-=-START-=-=-=-" s2
	NumTest = Input "Tell me how many cycle:"
	NumCycles = 0
	Label LoopTest
		NumCycles = NumCycles + 1
		If NumCycles > NumTest
			Return
		EndIf
		Gosub "HOME All"
		Gosub "Raise Handler"
		' --------------------- Button ON
		Coord_Z =   18
		Coord_A =  123
		Coord_XP = 194
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button PADLOCK"
		Coord_Z =   38
		Coord_A =   98
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 1
		Coord_Z =   67
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 2
		Coord_Z =   67
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 3
		Coord_Z =   67
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 4
		Coord_Z =   57
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 5
		Coord_Z =   57
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 6
		Coord_Z =   57
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		'' --------------------- Button 7
		'Coord_Z =   47
		'Coord_A =   93
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 8
		'Coord_Z =   47
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 9
		'Coord_Z =   47
		'Coord_A =  122
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 0
		'Coord_Z =   37
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button ENTER
		Coord_Z =   38
		Coord_A =  122
		Coord_XP = 196
		Coord_XR = 175
		Gosub "EXEC"
		' ----------------------
		Gosub "Lower Handler"
		Gosub "Check Latch"
		Gosub "HOUSE"
		s1 = Now
		s1 = s1 + " " + Format(NumCycles, "000") + " " + Result
		Print s1
		Save StringToFile s1 s2
	Goto LoopTest
Return

Label "EXEC"
	Slot(5)  = Coord_Z
	Gosub WaitAllMotors
	Gosub Pause
	Slot(25) = Coord_A
	Gosub WaitAllMotors
	Gosub Pause
	Slot(1)  = Coord_XP
	Wait Seconds DigitTime
	Gosub Pause
	Slot(1)  = Coord_XR
	Gosub Pause
Return

Label "Check Latch"
	Slot(25) =  62     	'A Check if latch is out
	Gosub WaitAllMotors
	Wait Seconds 0.5
	If Slot(32) > 500
		Result = " PASS" 				' latch inside
	Else
		Result = " <<< F A I L >>>"		' latch outside
	EndIf
Return

' =============== HANDLER MANAGEMENT ==================================
Label "Raise Handler"
	Slot(25) = 180     	'A go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  =  15	 	'Z pepare for raiSing area
	Gosub WaitAllMotors
	Slot(1)  = 188     	'X go to raising area
	Gosub WaitAllMotors
	Slot(5)  = 100	   	'Z raise the handle
	Gosub WaitAllMotors
	Wait Seconds 0.5
	Slot(5)  =  18	   	'Z Lower the handle
	Gosub WaitAllMotors
	Slot(1)  = 175     	'X goto safe area
Return

Label "Lower Handler"
	Slot(5)  =  80	 	'Z pepare for lower area
	Slot(1)  = 188     	'X go to lower area
	Gosub WaitAllMotors
	Slot(25) = 180     	'A go to handle management area
	Wait Seconds 0.5
	Slot(5)  =   5	   	'Z lower the handle
	Gosub WaitAllMotors
	Slot(5)  =  80	 	'Z return handler to Middle
	Gosub WaitAllMotors
Return


'----------------------------------------------------------------------
'----- S U B R O U T I N E   A R E A  ---------------------------------
'----------------------------------------------------------------------
' =============== HOME ALL MOTORs M0, M1, M2 ==========================
Label "HOME All"
	Slot(1) = Slot(1) - 5
	Gosub "Home M3(A)"
	Gosub "Home M2(Z)"
	Gosub "Home M1(Y)"
	Gosub "Home M0(X)"
Return

Label "Home M0(X)"
	SlotMotor = 1
	SlotSensor = 19
	SpeedSearch = -0.1
	SpeedReturn = +0.005
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M1(Y)"
	SlotMotor = 3
	SlotSensor = 20
	SpeedSearch = -0.1
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M2(Z)"
	SlotMotor = 5
	SlotSensor = 21
	SpeedSearch = +0.1
	SpeedReturn = -0.005
	DeclaredZero = 103
	InitialPosition = 100
	Gosub "Home"
Return

Label "Home M3(A)"
	SlotMotor = 25
	SlotSensor = 31
	SpeedSearch = -0.08
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

' =============== HOUSE ALL MOTORS M0, M1, M3 ========================
Label "HOUSE"
	Slot(5)  =  98	 	'Z Raise
	Gosub WaitAllMotors
	Slot(1)  =  2     	'X go to HOUSE
	Slot(25) =  2     	'A go to HOUSE
	Gosub WaitAllMotors
Return

' =============== HOME MOTOR ==========================================
Label "Home"
	Gosub RED
	If Slot(SlotSensor) > 500      ' detach If sensor active
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
	Label SearchHome
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
		Wait Seconds 0.001
		If Slot(SlotSensor) > 500 	' limit switch
			Gosub WaitMotor			' WAIT UNTIL MOTOR STOP
			Goto DetachSensor
		EndIf
		Goto SearchHome
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn ' direction/speed return
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
	Goto DetachSensor
Label AssignZero
	Slot(SlotMotor) = Reset			' Reset the Slot value
	Wait Seconds 0.08		  		' DO NOT TOUCH, needed to the Master!!!!
	Slot(SlotMotor) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Gosub YEL
Return


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor

' =============== W A I T  all MOTORS are STOPPED =======================
Label WaitAllMotors
' ------------------ this Wait ensures that the motors are really started
	Wait Seconds 0.1
	Label WaitAllMotors2
		'If Abs(Slot(2)) < 0.01 And Abs(Slot(4) ) < 0.01 And Abs(Slot(6)) < 0.01
		If Abs(Slot(2)) < 0.01 And Abs(Slot(4)) < 0.01 And Abs(Slot(6)) < 0.01 And Abs(Slot(26)) < 0.01
			Return
		EndIf
		Wait Seconds 0.01
	Goto WaitAllMotors2
' ------------------ this line is required to indent correctly
Goto WaitAllMotors

' =============== ENABLE / DISABLE MOTORS =============================
'Label EnableMotor
'	Slot 11 = 0
'	Wait Seconds 0.2
'Return
'
'Label DisableMotor
'	Slot 11 = 1000
'	Wait Seconds 0.2
'Return

' =============== PAUSE ===============================================
Label Pause
	Label PauseLoop
		If Slot(12) > 500
			Wait Seconds 0.2
			Gosub YEL
		Else
			Return
		EndIf
		Gosub RED
	Goto PauseLoop
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== MOVE USING KEYBOARD ================================
Label MoveRightM0
	Slot(1) = Slot(1) + 1
	Return
Label MoveLeftM0
	Slot(1) = Slot(1) - 1
	Return
Label MoveRightM1
	Slot(25) = Slot(25) + 1
	Return
Label MoveLeftM1
	Slot(25) = Slot(25) - 1
	Return
Label MoveUpM2
	Slot(5) = Slot(5) + 1
	Return
Label MoveDownM2
	Slot(5) = Slot(5) - 1
Return

'====== DUMMY AREA ====================================================

'====== TO DO AREA ====================================================

'====== SET UP AREA ===================================================


' ==================================================================
'     Name:	WXM_DoorLockq1_V5.6.txt     Date:  21 September 2019
'    Autor: Leonardo DE PALO Ph.D.
'    Scope: Generating a test robot for DoorLock system
' Hardware: Using the old cartesian system.
' ==================================================================
'
' =========== MICROSTEP HARDWARE SET UP  ===========================

'Load xxxxxx.jpg

'================ GENERATING THE LOG FILE NAME =====================
s2 = Test_Lock.txt
'===================================================================

Option Speed 9
Button 1 Text "Home M0(X)"
Button 2 Text "Home M1(Y)"
Button 3 Text "Home M2(Z)"
Button 4 Text "Home M3(A)"

Button 7 Text "START TEST"
Button 8 Text "Lower Handler"
Load Theremino_HAL\Theremino_HAL.exe
Load Theremino_HAL\Theremino_SlotViewer.exe
'Load Theremino_HAL\Theremino_Terminal.exe
'Load Theremino_HAL\Theremino_SlotsToSpeech.exe
'Load Theremino_HAL\Theremino_MicroVideo.exe
'Load Theremino_HAL\Theremino_SignalScope.exe

Variable Numeric DigitTime
Variable Numeric NumTest
Variable Numeric NumCycles

Variable Numeric Coord_Z
Variable Numeric Coord_A
Variable Numeric Coord_XP
Variable Numeric Coord_XR

Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredZero
Variable Numeric InitialPosition

Variable String Info
Variable String Result

DigitTime = 1.4

Save StringToFile s9 s2			'<<<<<<<<<<<<<<

Key Right		Gosub MoveRightM0
Key Left 		Gosub MoveLeftM0
Key Up 			Gosub MoveRightM1
Key Down 		Gosub MoveLeftM1
Key Pageup		Gosub MoveUpM2
Key Pagedown	Gosub MoveDownM2
Key S			Gosub "START TEST"

Gosub RED
Stop

' ===============  P I N    T E S T  ==================================
Label "START TEST"
	Save StringToFile "-=-=-=-START-=-=-=-" s2
	NumTest = Input "Tell me how many cycle:"
	NumCycles = 0
	Label LoopTest
		NumCycles = NumCycles + 1
		If NumCycles > NumTest
			Return
		EndIf
		Gosub "HOME All"
		Gosub "Raise Handler"
		' --------------------- Button ON
		Coord_Z =   18
		Coord_A =  123
		Coord_XP = 194
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button PADLOCK"
		Coord_Z =   38
		Coord_A =   98
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 1
		Coord_Z =   67
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 2
		Coord_Z =   67
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 3
		Coord_Z =   67
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 4
		Coord_Z =   57
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 5
		Coord_Z =   57
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 6
		Coord_Z =   57
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		'' --------------------- Button 7
		'Coord_Z =   47
		'Coord_A =   93
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 8
		'Coord_Z =   47
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 9
		'Coord_Z =   47
		'Coord_A =  122
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 0
		'Coord_Z =   37
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button ENTER
		Coord_Z =   38
		Coord_A =  122
		Coord_XP = 196
		Coord_XR = 175
		Gosub "EXEC"
		' ----------------------
		Gosub "Lower Handler"
		Gosub "Check Latch"
		Gosub "HOUSE"
		s1 = Now
		s1 = s1 + " " + Format(NumCycles, "000") + " " + Result
		Print s1
		Save StringToFile s1 s2
	Goto LoopTest
Return

Label "EXEC"
	Slot(5)  = Coord_Z
	Gosub WaitAllMotors
	Gosub Pause
	Slot(25) = Coord_A
	Gosub WaitAllMotors
	Gosub Pause
	Slot(1)  = Coord_XP
	Wait Seconds DigitTime
	Gosub Pause
	Slot(1)  = Coord_XR
	Gosub Pause
Return

Label "Check Latch"
	Slot(25) =  62     	'A Check If latch is out
	Gosub WaitAllMotors
	Wait Seconds 0.5
	If Slot(32) > 500
		Result = " PASS" 				' latch inside
	Else
		Result = " <<< F A I L >>>"		' latch outside
	EndIf
Return

' =============== HANDLER MANAGEMENT ==================================
Label "Raise Handler"
	Slot(25) = 180     	'A go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  =  15	 	'Z pepare for raiSing area
	Gosub WaitAllMotors
	Slot(1)  = 188     	'X go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  = 100	   	'Z raise the handle
	Gosub WaitAllMotors
	Wait Seconds 0.5
	Slot(5)  =  18	   	'Z Lower the handle
	Gosub WaitAllMotors
	Slot(1)  = 175     	'X goto safe area
Return

Label "Lower Handler"
	Slot(5)  =  80	 	'Z pepare for lower area
	Slot(1)  = 188     	'X go to lower area
	Gosub WaitAllMotors
	Slot(25) = 180     	'A go to handle management area
	Wait Seconds 0.5
	Slot(5)  =   5	   	'Z lower the handle
	Gosub WaitAllMotors
	Slot(5)  =  80	 	'Z return handler to Middle
	Gosub WaitAllMotors
Return


'----------------------------------------------------------------------
'----- S U B R O U T I N E   A R E A  ---------------------------------
'----------------------------------------------------------------------
' =============== HOME ALL MOTORs M0, M1, M2 ==========================
Label "HOME All"
	Slot(1) = Slot(1) - 5
	Gosub "Home M3(A)"
	Gosub "Home M2(Z)"
	Gosub "Home M1(Y)"
	Gosub "Home M0(X)"
Return

Label "Home M0(X)"
	SlotMotor = 1
	SlotSensor = 19
	SpeedSearch = -0.1
	SpeedReturn = +0.005
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M1(Y)"
	SlotMotor = 3
	SlotSensor = 20
	SpeedSearch = -0.1
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M2(Z)"
	SlotMotor = 5
	SlotSensor = 21
	SpeedSearch = +0.1
	SpeedReturn = -0.005
	DeclaredZero = 103
	InitialPosition = 100
	Gosub "Home"
Return

Label "Home M3(A)"
	SlotMotor = 25
	SlotSensor = 31
	SpeedSearch = -0.08
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

' =============== HOUSE ALL MOTORS M0, M1, M3 ========================
Label "HOUSE"
	Slot(5)  =  98	 	'Z Raise
	Gosub WaitAllMotors
	Slot(1)  =  2     	'X go to HOUSE
	Slot(25) =  2     	'A go to HOUSE
	Gosub WaitAllMotors
Return

' =============== HOME MOTOR ==========================================
Label "Home"
	Gosub RED
	If Slot(SlotSensor) > 500      ' detach If sensor active
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
	Label SearchHome
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
		Wait Seconds 0.001
		If Slot(SlotSensor) > 500 	' limit switch
			Gosub WaitMotor			' WAIT UNTIL MOTOR STOP
			Goto DetachSensor
		EndIf
		Goto SearchHome
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn ' direction/speed return
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
	Goto DetachSensor
Label AssignZero
	Slot(SlotMotor) = Reset			' Reset the Slot value
	Wait Seconds 0.08		  		' DO NOT TOUCH, needed to the Master!!!!
	Slot(SlotMotor) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Gosub YEL
Return


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor

' =============== W A I T  all MOTORS are STOPPED =======================
Label WaitAllMotors
' ------------------ this Wait ensures that the motors are really started
	Wait Seconds 0.1
	Label WaitAllMotors2
		'If Abs(Slot(2)) < 0.01 And Abs(Slot(4) ) < 0.01 And Abs(Slot(6)) < 0.01
		If Abs(Slot(2)) < 0.01 And Abs(Slot(4)) < 0.01 And Abs(Slot(6)) < 0.01 And Abs(Slot(26)) < 0.01
			Return
		EndIf
		Wait Seconds 0.01
	Goto WaitAllMotors2
' ------------------ this line is required to indent correctly
Goto WaitAllMotors

' =============== ENABLE / DISABLE MOTORS =============================
'Label EnableMotor
'	Slot 11 = 0
'	Wait Seconds 0.2
'Return
'
'Label DisableMotor
'	Slot 11 = 1000
'	Wait Seconds 0.2
'Return

' =============== PAUSE ===============================================
Label Pause
	Label PauseLoop
		If Slot(12) > 500
			Wait Seconds 0.2
			Gosub YEL
		Else
			Return
		EndIf
		Gosub RED
	Goto PauseLoop
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return


' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return




Option Speed 9
Button 1 Text "Home M0(X)"
Button 2 Text "Home M1(Y)"
Button 3 Text "Home M2(Z)"
Button 4 Text "Home M3(A)"

Button 7 Text "START TEST"
Button 8 Text "Lower Handler"
Load Theremino_HAL\Theremino_HAL.exe
Load Theremino_HAL\Theremino_SlotViewer.exe
'Load Theremino_HAL\Theremino_Terminal.exe
'Load Theremino_HAL\Theremino_SlotsToSpeech.exe
'Load Theremino_HAL\Theremino_MicroVideo.exe
'Load Theremino_HAL\Theremino_SignalScope.exe

Variable Numeric DigitTime
Variable Numeric NumTest
Variable Numeric NumCycles

Variable Numeric Coord_Z
Variable Numeric Coord_A
Variable Numeric Coord_XP
Variable Numeric Coord_XR

Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredZero
Variable Numeric InitialPosition

Variable String Info
Variable String Result

DigitTime = 1.4

Save StringToFile s9 s2			'<<<<<<<<<<<<<<

Key Right		Gosub MoveRightM0
Key Left 		Gosub MoveLeftM0
Key Up 			Gosub MoveRightM1
Key Down 		Gosub MoveLeftM1
Key Pageup		Gosub MoveUpM2
Key Pagedown	Gosub MoveDownM2
Key S			Gosub "START_TEST"

Gosub RED
Stop

' ===============  P I N    T E S T  ==================================
Label "START_TEST"
	Save StringToFile "-=-=-=-START-=-=-=-" s2
	NumTest = Input "Tell me how many cycle:"
	NumCycles = 0
	Label LoopTest
		NumCycles = NumCycles + 1
		If NumCycles > NumTest
			Return
		EndIf
		Gosub "HOME All"
		Gosub "Raise Handler"
		' --------------------- Button ON
		Coord_Z =   18
		Coord_A =  123
		Coord_XP = 194
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button PADLOCK"
		Coord_Z =   38
		Coord_A =   98
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 1
		Coord_Z =   67
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 2
		Coord_Z =   67
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 3
		Coord_Z =   67
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 4
		Coord_Z =   57
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 5
		Coord_Z =   57
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 6
		Coord_Z =   57
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		'' --------------------- Button 7
		'Coord_Z =   47
		'Coord_A =   93
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 8
		'Coord_Z =   47
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 9
		'Coord_Z =   47
		'Coord_A =  122
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 0
		'Coord_Z =   37
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button ENTER
		Coord_Z =   38
		Coord_A =  122
		Coord_XP = 196
		Coord_XR = 175
		Gosub "EXEC"
		' ----------------------
		Gosub "Lower Handler"
		Gosub "Check Latch"
		Gosub "HOUSE"
		s1 = Now
		s1 = s1 + " " + Format(NumCycles, "000") + " " + Result
		Print s1
		Save StringToFile s1 s2
	Goto LoopTest
Return

Label "EXEC"
	Slot(5)  = Coord_Z
	Gosub WaitAllMotors
	Gosub Pause
	Slot(25) = Coord_A
	Gosub WaitAllMotors
	Gosub Pause
	Slot(1)  = Coord_XP
	Wait Seconds DigitTime
	Gosub Pause
	Slot(1)  = Coord_XR
	Gosub Pause
Return

Label "Check Latch"
	Slot(25) =  62     	'A Check If latch is out
	Gosub WaitAllMotors
	Wait Seconds 0.5
	If Slot(32) > 500
		Result = " PASS" 				' latch inside
	Else
		Result = " <<< F A I L >>>"		' latch outside
	EndIf
Return

' =============== HANDLER MANAGEMENT ==================================
Label "Raise Handler"
	Slot(25) = 180     	'A go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  =  15	 	'Z pepare for raiSing area
	Gosub WaitAllMotors
	Slot(1)  = 188     	'X go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  = 100	   	'Z raise the handle
	Gosub WaitAllMotors
	Wait Seconds 0.5
	Slot(5)  =  18	   	'Z Lower the handle
	Gosub WaitAllMotors
	Slot(1)  = 175     	'X goto safe area
Return

Label "Lower Handler"
	Slot(5)  =  80	 	'Z pepare for lower area
	Slot(1)  = 188     	'X go to lower area
	Gosub WaitAllMotors
	Slot(25) = 180     	'A go to handle management area
	Wait Seconds 0.5
	Slot(5)  =   5	   	'Z lower the handle
	Gosub WaitAllMotors
	Slot(5)  =  80	 	'Z return handler to Middle
	Gosub WaitAllMotors
Return


'----------------------------------------------------------------------
'----- S U B R O U T I N E   A R E A  ---------------------------------
'----------------------------------------------------------------------
' =============== HOME ALL MOTORs M0, M1, M2 ==========================
Label "HOME All"
	Slot(1) = Slot(1) - 5
	Gosub "Home M3(A)"
	Gosub "Home M2(Z)"
	Gosub "Home M1(Y)"
	Gosub "Home M0(X)"
Return

Label "Home M0(X)"
	SlotMotor = 1
	SlotSensor = 19
	SpeedSearch = -0.1
	SpeedReturn = +0.005
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M1(Y)"
	SlotMotor = 3
	SlotSensor = 20
	SpeedSearch = -0.1
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M2(Z)"
	SlotMotor = 5
	SlotSensor = 21
	SpeedSearch = +0.1
	SpeedReturn = -0.005
	DeclaredZero = 103
	InitialPosition = 100
	Gosub "Home"
Return

Label "Home M3(A)"
	SlotMotor = 25
	SlotSensor = 31
	SpeedSearch = -0.08
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

' =============== HOUSE ALL MOTORS M0, M1, M3 ========================
Label "HOUSE"
	Slot(5)  =  98	 	'Z Raise
	Gosub WaitAllMotors
	Slot(1)  =  2     	'X go to HOUSE
	Slot(25) =  2     	'A go to HOUSE
	Gosub WaitAllMotors
Return

' =============== HOME MOTOR ==========================================
Label "Home"
	Gosub RED
	If Slot(SlotSensor) > 500      ' detach If sensor active
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
	Label SearchHome
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
		Wait Seconds 0.001
		If Slot(SlotSensor) > 500 	' limit switch
			Gosub WaitMotor			' WAIT UNTIL MOTOR STOP
			Goto DetachSensor
		EndIf
		Goto SearchHome
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn ' direction/speed return
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
	Goto DetachSensor
Label AssignZero
	Slot(SlotMotor) = Reset			' Reset the Slot value
	Wait Seconds 0.08		  		' DO NOT TOUCH, needed to the Master!!!!
	Slot(SlotMotor) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Gosub YEL
Return


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor

' =============== W A I T  all MOTORS are STOPPED =======================
Label WaitAllMotors
' ------------------ this Wait ensures that the motors are really started
	Wait Seconds 0.1
	Label WaitAllMotors2
		'If Abs(Slot(2)) < 0.01 And Abs(Slot(4) ) < 0.01 And Abs(Slot(6)) < 0.01
		If Abs(Slot(2)) < 0.01 And Abs(Slot(4)) < 0.01 And Abs(Slot(6)) < 0.01 And Abs(Slot(26)) < 0.01
			Return
		EndIf
		Wait Seconds 0.01
	Goto WaitAllMotors2
' ------------------ this line is required to indent correctly
Goto WaitAllMotors

' =============== ENABLE / DISABLE MOTORS =============================
'Label EnableMotor
'	Slot 11 = 0
'	Wait Seconds 0.2
'Return
'
'Label DisableMotor
'	Slot 11 = 1000
'	Wait Seconds 0.2
'Return

' =============== PAUSE ===============================================
Label Pause
	Label PauseLoop
		If Slot(12) > 500
			Wait Seconds 0.2
			Gosub YEL
		Else
			Return
		EndIf
		Gosub RED
	Goto PauseLoop
Return

' =============== TRAFFIC LIGHT COLOR =================================
Label RED
	Slot 16 = 1000		'RED
	Slot 17 =    0		'YEL
	Slot 18 =    0		'GRE
Return

Label YEL
	Slot 16 =    0		'RED
	Slot 17 = 1000		'YEL
	Slot 18 =    0		'GRE
Return

Label GRE
	Slot 16 =    0		'RED
	Slot 17 =    0		'YEL
	Slot 18 = 1000		'GRE
Return

' =============== MOVE USING KEYBOARD ================================
Label MoveRightM0
	Slot(1) = Slot(1) + 1
	Return
Label MoveLeftM0
	Slot(1) = Slot(1) - 1
	Return
Label MoveRightM1
	Slot(25) = Slot(25) + 1
	Return
Label MoveLeftM1
	Slot(25) = Slot(25) - 1
	Return
Label MoveUpM2
	Slot(5) = Slot(5) + 1
	Return
Label MoveDownM2
	Slot(5) = Slot(5) - 1
Return

'====== DUMMY AREA ====================================================

'====== TO DO AREA ====================================================

'====== SET UP AREA ===================================================


' ==================================================================
'     Name:	WXM_DoorLockq1_V5.6.txt     Date:  21 September 2019
'    Autor: Leonardo DE PALO Ph.D.
'    Scope: Generating a test robot for DoorLock system
' Hardware: Using the old cartesian system.
' ==================================================================
'
' =========== MICROSTEP HARDWARE SET UP  ===========================

'Load xxxxxx.jpg

'================ GENERATING THE LOG FILE NAME =====================
s2 = Test_Lock.txt
'===================================================================

Option Speed 9
Button 1 Text "Home M0(X)"
Button 2 Text "Home M1(Y)"
Button 3 Text "Home M2(Z)"
Button 4 Text "Home M3(A)"

Button 7 Text "START TEST"
Button 8 Text "Lower Handler"
Load Theremino_HAL\Theremino_HAL.exe
Load Theremino_HAL\Theremino_SlotViewer.exe
'Load Theremino_HAL\Theremino_Terminal.exe
'Load Theremino_HAL\Theremino_SlotsToSpeech.exe
'Load Theremino_HAL\Theremino_MicroVideo.exe
'Load Theremino_HAL\Theremino_SignalScope.exe

Variable Numeric DigitTime
Variable Numeric NumTest
Variable Numeric NumCycles

Variable Numeric Coord_Z
Variable Numeric Coord_A
Variable Numeric Coord_XP
Variable Numeric Coord_XR

Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredZero
Variable Numeric InitialPosition

Variable String Info
Variable String Result

DigitTime = 1.4

Save StringToFile s9 s2			'<<<<<<<<<<<<<<

Key Right		Gosub MoveRightM0
Key Left 		Gosub MoveLeftM0
Key Up 			Gosub MoveRightM1
Key Down 		Gosub MoveLeftM1
Key Pageup		Gosub MoveUpM2
Key Pagedown	Gosub MoveDownM2
Key S			Gosub "START TEST"

Gosub RED
Stop

' ===============  P I N    T E S T  ==================================
Label "START TEST"
	Save StringToFile "-=-=-=-START-=-=-=-" s2
	NumTest = Input "Tell me how many cycle:"
	NumCycles = 0
	Label LoopTest
		NumCycles = NumCycles + 1
		If NumCycles > NumTest
			Return
		EndIf
		Gosub "HOME All"
		Gosub "Raise Handler"
		' --------------------- Button ON
		Coord_Z =   18
		Coord_A =  123
		Coord_XP = 194
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button PADLOCK"
		Coord_Z =   38
		Coord_A =   98
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 1
		Coord_Z =   67
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 2
		Coord_Z =   67
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 3
		Coord_Z =   67
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 4
		Coord_Z =   57
		Coord_A =   93
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 5
		Coord_Z =   57
		Coord_A =  108
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		' --------------------- Button 6
		Coord_Z =   57
		Coord_A =  122
		Coord_XP = 197
		Coord_XR = 175
		Gosub "EXEC"
		'' --------------------- Button 7
		'Coord_Z =   47
		'Coord_A =   93
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 8
		'Coord_Z =   47
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 9
		'Coord_Z =   47
		'Coord_A =  122
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button 0
		'Coord_Z =   37
		'Coord_A =  108
		'Coord_XP = 197
		'Coord_XR = 175
		'Gosub "EXEC"
		'' --------------------- Button ENTER
		Coord_Z =   38
		Coord_A =  122
		Coord_XP = 196
		Coord_XR = 175
		Gosub "EXEC"
		' ----------------------
		Gosub "Lower Handler"
		Gosub "Check Latch"
		Gosub "HOUSE"
		s1 = Now
		s1 = s1 + " " + Format(NumCycles, "000") + " " + Result
		Print s1
		Save StringToFile s1 s2
	Goto LoopTest
Return

Label "EXEC"
	Slot(5)  = Coord_Z
	Gosub WaitAllMotors
	Gosub Pause
	Slot(25) = Coord_A
	Gosub WaitAllMotors
	Gosub Pause
	Slot(1)  = Coord_XP
	Wait Seconds DigitTime
	Gosub Pause
	Slot(1)  = Coord_XR
	Gosub Pause
Return

Label "Check Latch"
	Slot(25) =  62     	'A Check If latch is out
	Gosub WaitAllMotors
	Wait Seconds 0.5
	If Slot(32) > 500
		Result = " PASS" 				' latch inside
	Else
		Result = " <<< F A I L >>>"		' latch outside
	EndIf
Return

' =============== HANDLER MANAGEMENT ==================================
Label "Raise Handler"
	Slot(25) = 180     	'A go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  =  15	 	'Z pepare for raiSing area
	Gosub WaitAllMotors
	Slot(1)  = 188     	'X go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  = 100	   	'Z raise the handle
	Gosub WaitAllMotors
	Wait Seconds 0.5
	Slot(5)  =  18	   	'Z Lower the handle
	Gosub WaitAllMotors
	Slot(1)  = 175     	'X goto safe area
Return

Label "Lower Handler"
	Slot(5)  =  80	 	'Z pepare for lower area
	Slot(1)  = 188     	'X go to lower area
	Gosub WaitAllMotors
	Slot(25) = 180     	'A go to handle management area
	Wait Seconds 0.5
	Slot(5)  =   5	   	'Z lower the handle
	Gosub WaitAllMotors
	Slot(5)  =  80	 	'Z return handler to Middle
	Gosub WaitAllMotors
Return


'----------------------------------------------------------------------
'----- S U B R O U T I N E   A R E A  ---------------------------------
'----------------------------------------------------------------------
' =============== HOME ALL MOTORs M0, M1, M2 ==========================
Label "HOME All"
	Slot(1) = Slot(1) - 5
	Gosub "Home M3(A)"
	Gosub "Home M2(Z)"
	Gosub "Home M1(Y)"
	Gosub "Home M0(X)"
Return

Label "Home M0(X)"
	SlotMotor = 1
	SlotSensor = 19
	SpeedSearch = -0.1
	SpeedReturn = +0.005
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M1(Y)"
	SlotMotor = 3
	SlotSensor = 20
	SpeedSearch = -0.1
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M2(Z)"
	SlotMotor = 5
	SlotSensor = 21
	SpeedSearch = +0.1
	SpeedReturn = -0.005
	DeclaredZero = 103
	InitialPosition = 100
	Gosub "Home"
Return

Label "Home M3(A)"
	SlotMotor = 25
	SlotSensor = 31
	SpeedSearch = -0.08
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

' =============== HOUSE ALL MOTORS M0, M1, M3 ========================
Label "HOUSE"
	Slot(5)  =  98	 	'Z Raise
	Gosub WaitAllMotors
	Slot(1)  =  2     	'X go to HOUSE
	Slot(25) =  2     	'A go to HOUSE
	Gosub WaitAllMotors
Return

' =============== HOME MOTOR ==========================================
Label "Home"
	Gosub RED
	If Slot(SlotSensor) > 500      ' detach If sensor active
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
	Label SearchHome
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
		Wait Seconds 0.001
		If Slot(SlotSensor) > 500 	' limit switch
			Gosub WaitMotor			' WAIT UNTIL MOTOR STOP
			Goto DetachSensor
		EndIf
		Goto SearchHome
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn ' direction/speed return
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
	Goto DetachSensor
Label AssignZero
	Slot(SlotMotor) = Reset			' Reset the Slot value
	Wait Seconds 0.08		  		' DO NOT TOUCH, needed to the Master!!!!
	Slot(SlotMotor) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Gosub YEL
Return


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor

' =============== W A I T  all MOTORS are STOPPED =======================
Label WaitAllMotors
' ------------------ this Wait ensures that the motors are really started
	Wait Seconds 0.1
	Label WaitAllMotors2
		'If Abs(Slot(2)) < 0.01 And Abs(Slot(4) ) < 0.01 And Abs(Slot(6)) < 0.01
		If Abs(Slot(2)) < 0.01 And Abs(Slot(4)) < 0.01 And Abs(Slot(6)) < 0.01 And Abs(Slot(26)) < 0.01
			Return
		EndIf
		Wait Seconds 0.01
	Goto WaitAllMotors2
' ------------------ this line is required to indent correctly
Goto WaitAllMotors

' =============== ENABLE / DISABLE MOTORS =============================
'Label EnableMotor
'	Slot 11 = 0
'	Wait Seconds 0.2
'Return
'
'Label DisableMotor
'	Slot 11 = 1000
'	Wait Seconds 0.2
'Return

' =============== HANDLER MANAGEMENT ==================================
Label "Raise Handler"
	Slot(25) = 180     	'A go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  =  15	 	'Z pepare for raiSing area
	Gosub WaitAllMotors
	Slot(1)  = 188     	'X go to raiSing area
	Gosub WaitAllMotors
	Slot(5)  = 100	   	'Z raise the handle
	Gosub WaitAllMotors
	Wait Seconds 0.5
	Slot(5)  =  18	   	'Z Lower the handle
	Gosub WaitAllMotors
	Slot(1)  = 175     	'X goto safe area
Return

Label "Lower Handler"
	Slot(5)  =  80	 	'Z pepare for lower area
	Slot(1)  = 188     	'X go to lower area
	Gosub WaitAllMotors
	Slot(25) = 180     	'A go to handle management area
	Wait Seconds 0.5
	Slot(5)  =   5	   	'Z lower the handle
	Gosub WaitAllMotors
	Slot(5)  =  80	 	'Z return handler to Middle
	Gosub WaitAllMotors
Return


'----------------------------------------------------------------------
'----- S U B R O U T I N E   A R E A  ---------------------------------
'----------------------------------------------------------------------
' =============== HOME ALL MOTORs M0, M1, M2 ==========================
Label "HOME All"
	Slot(1) = Slot(1) - 5
	Gosub "Home M3(A)"
	Gosub "Home M2(Z)"
	Gosub "Home M1(Y)"
	Gosub "Home M0(X)"
Return

Label "Home M0(X)"
	SlotMotor = 1
	SlotSensor = 19
	SpeedSearch = -0.1
	SpeedReturn = +0.005
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M1(Y)"
	SlotMotor = 3
	SlotSensor = 20
	SpeedSearch = -0.1
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

Label "Home M2(Z)"
	SlotMotor = 5
	SlotSensor = 21
	SpeedSearch = +0.1
	SpeedReturn = -0.005
	DeclaredZero = 103
	InitialPosition = 100
	Gosub "Home"
Return

Label "Home M3(A)"
	SlotMotor = 25
	SlotSensor = 31
	SpeedSearch = -0.08
	SpeedReturn = +0.05
	DeclaredZero = 0
	InitialPosition = 1
	Gosub "Home"
Return

' =============== HOUSE ALL MOTORS M0, M1, M3 ========================
Label "HOUSE"
	Slot(5)  =  98	 	'Z Raise
	Gosub WaitAllMotors
	Slot(1)  =  2     	'X go to HOUSE
	Slot(25) =  2     	'A go to HOUSE
	Gosub WaitAllMotors
Return

' =============== HOME MOTOR ==========================================
Label "Home"
	Gosub RED
	If Slot(SlotSensor) > 500      ' detach If sensor active
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
	Label SearchHome
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
		Wait Seconds 0.001
		If Slot(SlotSensor) > 500 	' limit switch
			Gosub WaitMotor			' WAIT UNTIL MOTOR STOP
			Goto DetachSensor
		EndIf
		Goto SearchHome
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn ' direction/speed return
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
	Goto DetachSensor
Label AssignZero
	Slot(SlotMotor) = Reset			' Reset the Slot value
	Wait Seconds 0.08		  		' DO NOT TOUCH, needed to the Master!!!!
	Slot(SlotMotor) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Gosub YEL
Return


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor


' =============== W A I T until the motor(SlotMotor) is stopped ========
Label WaitMotor
' ------------------ this Wait ensures that the motors ia running
	Wait Seconds 0.1
	Label Wait1
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Return			' now the motor is not running
		EndIf
		'Wait Seconds 0.005
	Goto Wait1
' ------------------ this line is required to indent correctly
Goto WaitMotor


'WATCH SlotMotor
'WATCH Abs(Slot(SlotMotor))
'WATCH n12345
'WATCH s12345
'WATCH v1
'WATCH 323
'WATCH v1
'WATCH s1
'WATCH str
'WATCH Sin2
'EXEC Sin2 = Sin(33)
