' ====================================================================
'     Name:	WXM_XYZ_SAMSUNG_Solder_7.txt          Date: 09 August 2020
'    Autor: Leonardo DE PALO Ph.D.
'    Scope: Reuse the older ScrewMachine cartesian system to be used
'           to dispense the solder paste for the SAMSUNG filters.
' Hardware: Using old cartesian system, i5 Intel processor.
'  Release: 3, added SEQUENCES.
' ====================================================================

Load images\XYZ_Cartesian.jpg
Option Speed 9

' ---------------------------------------- Software limits
Xmax =  205
Xmin =    1
Ymax =  323
Ymin =    1
Zmax =  102
Zmin =    2

' ---------------------------------------- Slots
NumMotors = 3

Slot_X = 1
Slot_Y = 3
Slot_Z = 5
Slot_SensorX = 7
Slot_SensorY = 8
Slot_SensorZ = 9
Slot_Dispenser = 11

Slot_WhiteFront = 16
Slot_LedR = 13
Slot_LedG = 14
Slot_LedB = 15

Slot_JoyPadX = 200
Slot_JoyPadY = 201
Slot_JoyPadZ = 206
Slot_JoyPadStartStop = 220
Slot_JoyPadDispenser = 210
Slot_JoyPadSpeed = 202
Slot_JoyPadHome6 = 215
Slot_JoyPadTexBox = 212
Slot_JoyPadSaveCoord = 213

Slot_JoyPadFrontWhite = 216
Slot_JoyPadRed = 217
Slot_JoyPadGreen = 219
Slot_JoyPadBlu = 221

' -------------------------------------- BUTTONS
Button  1 Text HOME ALL
Button  1 Color SkyBlue
Button  2 Text Home X
Button  2 Color SkyBlue
Button  3 Text Home Y
Button  3 Color SkyBlue
Button  4 Text Home Z
Button  4 Color SkyBlue

'Button  6 Text Stress Test
'Button  6 Color Yellow
'Button  7 Text Cross Test
'Button  7 Color Yellow
Button  8 Text Square Test
Button  8 Color Yellow
Button  9 Text Circle Test
Button  9 Color Yellow
Button 10 Text Triangle Test
Button 10 Color Yellow
Button 11 Text PARK
Button 11 Color White
'Button 11 Text Enable Motors
'Button 11 Color Tan
'Button 12 Text Disable Motors
'Button 12 Color Tan

Button 13 Text  JoyPad
Button 13 Color Orange
Button 13 Slot  Slot_JoyPadStartStop
Button 14 Text  CLOSE ALL
Button 14 Color Red

Gosub DisableAllButtons

' -------------------------------------- KEYS
Key Right		Gosub ExecKeys
Key Left 		Gosub ExecKeys
Key Up 			Gosub ExecKeys
Key Down 		Gosub ExecKeys
Key Pageup		Gosub ExecKeys
Key Pagedown	Gosub ExecKeys

Gosub DisableAllButtons

' -------------------------------------- LOAD EXTERNAL PROGRAMS
Load "Theremino_HAL\Theremino_HAL.exe"
'Load "Microvideo\Theremino_MicroVideo.exe"
Load "Theremino_HAL\Theremino_SlotViewer1.exe"
Load "Theremino_HAL\Theremino_SlotViewer2.exe"
Load "Theremino_HAL\Theremino_SlotViewer3.exe"
Load "Theremino_HAL\Theremino_SlotViewer4.exe"
Load "Theremino_JoyPad\Theremino_JoyPad\Theremino_JoyPad.exe"

' -------------------------------------- TEST MASTER PRESENT
For v1 = 1 To 9e99
	For v2 = 1 To 20 ' <<< testing for 2 seconds
		If (Slot(0) >= 1 And Slot(0) <= 3) Or Key("CTRL")
			Exit 2
		EndIf
		Wait Seconds 0.1
	Next
	Print " Invalid master number: " + Slot(0)
	Slot 0 = Recognize
	Beep
Next
Print ""

' -------------------------------------- COLORS
Gosub NOCOLOR
Wait Seconds 0.1
Gosub RedOn
Wait Seconds 0.1
Gosub GreenOn
Wait Seconds 0.1
Gosub BluON
Wait Seconds 0.1
Gosub NOCOLOR
Wait Seconds 0.1
Gosub WhiteFrontOn
Wait Seconds 0.2
Gosub NOCOLOR

Gosub EnableAllButtons
Load Hide
Stop

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_X
Variable Numeric Slot_Y
Variable Numeric Slot_Z
Variable Numeric Slot_SensorX
Variable Numeric Slot_SensorY
Variable Numeric Slot_SensorZ

Variable Numeric Slot_WhiteFront
Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB

Variable Numeric Slot_Dispenser
Variable Numeric DispenserTime

Variable String FileName

Variable Numeric Xmax
Variable Numeric Xmin
Variable Numeric Ymax
Variable Numeric Ymin
Variable Numeric Zmax
Variable Numeric Zmin

' ====================================================================
'  SEQUENCES
' ====================================================================
Label SAMSUNG front
	SequenceFileName = "SEQUENCES\SAMSUNG_front.seq"
	Gosub ExecSequence
Return

Label SAMSUNG back
	SequenceFileName = "SEQUENCES\SAMSUNG_back.seq"
	Gosub ExecSequence
Return

Label TIP calibration
	For v9 = 1 To 10    	'9e99
		SequenceFileName = "SEQUENCES\SAMSUNG_TipCalibration.seq"
		'SequenceFileName = "SEQUENCES\SAMSUNG_TipMeasurement.seq"
		Gosub ExecSequence
	Next
Return

Label Stress Test
	Button 6 Color Flashing8 Black White
	Gosub Enable Motors
	For v9 = 1 To 50		'9e99
		SequenceFileName = "SEQUENCES\StressTest.seq"
		Gosub ExecSequence
	Next
	Button 6 Color Yellow
Return

Label Square Test
	Button 8 Color Flashing8 Black White
	Gosub Enable Motors
	For v9 = 1 To 2			'9e99
		SequenceFileName = "SEQUENCES\SquareTest.seq"
		Print "Cycle Num. " + v1
		Wait seconds 0.3
		Gosub ExecSequence
	Next
	Button 8 Color Yellow
Return

Label Circle Test
	Button 9 Color Flashing8 Black White
	Gosub Enable Motors
	For v1 = 1 To 2    	'9e99
		SequenceFileName = "SEQUENCES\CircleTest.seq"
		Gosub ExecSequence
	Next
	Button 9 Color Yellow
Return

Label Triangle Test
	Button 10 Color Flashing8 Black White
	Gosub Enable Motors
	For v9 = 1 To 2    	'9e99
		SequenceFileName = "SEQUENCES\TriangleTest.seq"
		Gosub ExecSequence
	Next
	Button 10 Color Yellow
Return

Label PARK
	Button 11 Color Flashing8 Black White
	Gosub Enable Motors
	'For v9 = 1 To 2    	'9e99
	SequenceFileName = "SEQUENCES\Park.seq"
	Gosub ExecSequence
	'Next
	Button 11 Color White
Return

' ====================================================================
'  SEQUENCE LOADING AND EXECUTION
' ====================================================================
Variable String SequenceFileName
Variable String Sequence
Variable String SequenceCmd
Variable Numeric SequenceIndex
Variable Numeric SequenceLength

Label ExecSequence
	Load Sequence SequenceFileName
	If Len(Sequence) = 0
		Controls OpenTextBox
		Print CRLF + " Sequence file not found: " + SequenceFileName
		Beep
		Stop
	EndIf
	' ------------------------------------------
	Variable Numeric ExecSequenceStartTime
	ExecSequenceStartTime = ElapsedTime
	' ------------------------------------------
	SequenceLength = GetSeparatedStringCount(Sequence, CRLF)
	For SequenceIndex = 0 To SequenceLength-1
		SequenceCmd = GetSeparatedString(Sequence, SequenceIndex, CRLF)
		SequenceCmd = Replace(SequenceCmd, Chr(9), " ")
		SequenceCmd = RemoveComments(SequenceCmd)
		SequenceCmd = Trim(SequenceCmd)
		'
		Print " Executing command: " + SequenceCmd
		'
		Select UCase(SequenceCmd)
            ' ----------------------------------------------- End sequence
			Case "ENDSEQ"
				Exit
		   ' ------------------------------------------------ ACTIONS
			Case "ENABLEMOTORS"
				Gosub Enable Motors
			Case "DISABLEMOTORS"
				Gosub Disable Motors
			Case "HOME"
				Gosub HOME
			Case "HOME X"
				Gosub Home X
			Case "HOME Y"
				Gosub Home Y
			Case "HOME Z"
				Gosub Home Z
			Case "CLOSEALL"
				Gosub CLOSE ALL
			' ---------------------------------------------- COLORS
			Case "REDON"
				Gosub RedOn
			Case "REDOFF"
				Gosub RedOff
			Case "GREENON"
				Gosub GreenOn
			Case "GREENOFF"
				Gosub GreenOff
			Case "BLUON"
				Gosub BluON
			Case "BLUOFF"
				Gosub BluOff
			Case "YELLOWON"
				Gosub YellowOn
			Case "YELLOWOFF"
				Gosub YellowOff
			Case "WHITEON"
				Gosub WhiteOn
			Case "WHITEOFF"
				Gosub WhiteOff
			Case "WHITEFRONTON"
				Gosub WhiteFrontOn
			Case "WHITEFRONTOFF"
				Gosub WhiteFrontOFF
			Case "NOCOLOR"
				Gosub NOCOLOR
			'Case "STARTVIDEO"
			'	Gosub StartVideo
			'Case "STOPVIDEO"
			'	Gosub StopVideo
			'Case "SAVEVIDEO"
			'	Gosub SaveVideo
			'Case "TAKEIMAGE"
			'	Gosub TakeImage
			' ---------------------------------------------- WAIT MOTORS
			Case "WAITALLMOTORS"
				Gosub WaitAllMotors
            ' ---------------------------------------------- With parameters
			CaseElse
				s1 = GetSeparatedString(SequenceCmd, 0)
				Select UCase(s1)
					Case "MOVESLOWXY"
						MoveSlowXY_DestX = GetSeparatedString(SequenceCmd, 1)
						MoveSlowXY_DestY = GetSeparatedString(SequenceCmd, 2)
						MoveSlowXY_Speed = GetSeparatedString(SequenceCmd, 3)
						Gosub MoveSlowXY
					Case "MOVESLOWZ"
						MoveSlowZ_Dest = GetSeparatedString(SequenceCmd, 1)
						MoveSlowZ_Speed = GetSeparatedString(SequenceCmd, 2)
						Gosub MoveSlowZ
					Case "GOTOCIRCLESTARTPOINT"
						DrawCircle_X = GetSeparatedString(SequenceCmd, 1)
						DrawCircle_Y = GetSeparatedString(SequenceCmd, 2)
						DrawCircle_Radius = GetSeparatedString(SequenceCmd, 3)
						DrawCircle_Speed = GetSeparatedString(SequenceCmd, 4)
						MoveSlowXY_DestX = DrawCircle_X
						MoveSlowXY_DestY = DrawCircle_Y + DrawCircle_Radius
						MoveSlowXY_Speed = DrawCircle_Speed
						Gosub MoveSlowXY
					Case "DRAWCIRCLE"
						DrawCircle_X = GetSeparatedString(SequenceCmd, 1)
						DrawCircle_Y = GetSeparatedString(SequenceCmd, 2)
						DrawCircle_Radius = GetSeparatedString(SequenceCmd, 3)
						DrawCircle_Speed = GetSeparatedString(SequenceCmd, 4)
						Gosub DrawCircle
					Case "PAUSE"
						Gosub RedOn
						Wait Seconds GetSeparatedString(SequenceCmd, 1)
						Gosub GreenOn
					Case "DISPENSER"
						DispenserTime = GetSeparatedString(SequenceCmd, 1)
						Gosub Dispenser
					CaseElse
						If s1 <> ""
							Controls OpenTextBox
							Print CRLF + " Unrecognized command: " + SequenceCmd
							Beep
							Stop
						EndIf
				EndSelect
		EndSelect
	Next
	SequenceFileName = Replace(SequenceFileName,".seq","")
	Print SequenceFileName + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds"
Return

' ====================================================================
' 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 CLOSE ALL
	Gosub SavePositions
	Load CloseApps
Return

Label EventStop
	Gosub SavePositions
	Gosub Disable Motors
	Gosub EnableAllButtons
End

Label SavePositions
	v4 = Slot(Slot_X)
	v5 = Slot(Slot_Y)
	v6 = Slot(Slot_Z)
	Save Vars
Return

Label UPDATE_FILE
	s1 = ""
	s1 = Now
	's1 = s1 + " " + Format(NumCycles, "000") + " " + Result
	Print s1
	Save StringToFile s1 s2
Return

' ====================================================================
'  HOME  X Y Z
' ====================================================================
Label HOME ALL
	Gosub BLU
	Gosub Home Z
	Gosub Home Y
	Gosub Home X
Return

Label Home X
	SlotMotor = Slot_X
	SlotSensor = Slot_SensorX
	SpeedSearch = -0.2		'-0.1
	SpeedReturn = +0.004
	DeclaredSwitchPos = 0
	InitialPosition = 2
	Gosub HomeMotor
Return

Label Home Y
	SlotMotor = Slot_Y
	SlotSensor = Slot_SensorY
	SpeedSearch = -0.2		'-0.1
	SpeedReturn = +0.004
	DeclaredSwitchPos = 0
	InitialPosition = 2
	Gosub HomeMotor
Return

Label Home Z
	SlotMotor = Slot_Z
	SlotSensor = Slot_SensorZ
	SpeedSearch = +0.1
	SpeedReturn = -0.004
	DeclaredSwitchPos = 113
	InitialPosition = 110
	Gosub HomeMotor
Return

' ========================================= HOME MOTOR
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
	For MotorNumber = 0 To NumMotors - 1
		Gosub MotorNumberToSlot
		If Abs(Slot(MotorSlotFromNumber + 1)) > 0.01
			MotorNumber = -1
		EndIf
		Wait Seconds 0.01
	Next
Return

' ====================================================================
'  MOTOR NUMBER TO SLOT CONVERSION
' ====================================================================
Variable Numeric NumMotors
Variable Numeric MotorNumber
Variable Numeric MotorSlotFromNumber

Label MotorNumberToSlot
	Select MotorNumber
		Case 0
			MotorSlotFromNumber = Slot_X
		Case 1
			MotorSlotFromNumber = Slot_Y
		Case 2
			MotorSlotFromNumber = Slot_Z
	EndSelect
Return

' ====================================================================
'  TRAFFIC LIGHT COLORS
' ====================================================================
Label RedOn
	Slot Slot_LedR = 800
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label RedOff
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label GreenOn
	Slot Slot_LedR = 0
	Slot Slot_LedG = 800
	Slot Slot_LedB = 0
Return

Label GreenOff
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label BluON
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 800
Return

Label BluOff
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label YellowOn
	Slot Slot_LedR = 800
	Slot Slot_LedG = 600
	Slot Slot_LedB = 0
Return

Label YellowOff
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label WhiteOn
	Slot Slot_LedR = 800
	Slot Slot_LedG = 800
	Slot Slot_LedB = 800
	Gosub WhiteFrontOn
Return

Label WhiteOff
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label NOCOLOR
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
	Slot Slot_WhiteFront = 0
Return

Label WhiteFrontOn
	Slot Slot_WhiteFront = 1000
Return

Label WhiteFrontOFF
	Slot Slot_WhiteFront = 0
Return

Label Dispenser
	Gosub BluON
	Slot Slot_Dispenser = 1000
	Wait Seconds DispenserTime
	Slot Slot_Dispenser = 0
	Gosub GreenOn
Return


' =====================================================================
'  STEPPER-MOTORS MOVE SLOW
' =====================================================================
Variable Numeric MSV1
Variable Numeric MSV2
Variable Numeric MSV3
Variable Numeric MSV4

' =====================================================================
'  MOVE SLOW XY
' =====================================================================
Variable Numeric MoveSlowXY_DestX 	' destination in mm
Variable Numeric MoveSlowXY_DestY 	' destination in mm
Variable Numeric MoveSlowXY_Speed 	' speed in mm/sec

Label MoveSlowXY
	' ----------------------------------- test for very low speed
	If MoveSlowXY_Speed < 0.1
		MoveSlowXY_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlowXY_DestX - Slot(Slot_X)
	MSV2 = MoveSlowXY_DestY - Slot(Slot_Y)
	MSV3 = MSV1 * MSV1
	MSV3 = MSV3 + MSV2 * MSV2
	MSV3 = Sqrt(MSV3)
	' ----------------------------------- calc the loops count
	MSV4 = MSV3 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlowXY_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(Slot_X) = Limit(MoveSlowXY_DestX, Xmin, Xmax)
		Slot(Slot_Y) = Limit(MoveSlowXY_DestY, Ymin, Ymax)
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlowXY_DestX - Slot(Slot_X)) / MSV4
	MSV2 = (MoveSlowXY_DestY - Slot(Slot_Y)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_X) = Limit(Slot(Slot_X) + MSV1, Xmin, Xmax)
		Slot(Slot_Y) = Limit(Slot(Slot_Y) + MSV2, Ymin, Ymax)
		Wait Seconds 0.01 '<<< loop time
	Next
	'Gosub WaitAllMotorsAndRecovery
	Gosub WaitAllMotors
Return

' =====================================================================
'  MOVE SLOW Z
' =====================================================================
Variable Numeric MoveSlowZ_Dest 	' destination in mm
Variable Numeric MoveSlowZ_Speed 	' speed in mm/sec

Label MoveSlowZ
	' ----------------------------------- test for very low speed
	If MoveSlowZ_Speed < 0.1
		MoveSlowZ_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlowZ_Dest - Slot(Slot_Z)
	MSV1 = Abs(MSV1)
	' ----------------------------------- calc the loops count
	MSV4 = MSV1 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlowZ_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(Slot_Z) = Limit(MoveSlowZ_Dest, Zmin, Zmax)
		Return
	EndIf
	' ----------------------------------- prepare the increment
	MSV1 = (MoveSlowZ_Dest - Slot(Slot_Z)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_Z) = Limit(Slot(Slot_Z) + MSV1, Zmin, Zmax)
		Wait Seconds 0.01 '<<< loop time
	Next
	'Gosub WaitAllMotorsAndRecovery
	Gosub WaitAllMotors
Return

' =====================================================================
'  DRAW CIRCLE
' =====================================================================
Variable Numeric DrawCircle_X 	    ' Center X in mm
Variable Numeric DrawCircle_Y 	    ' Center Y in mm
Variable Numeric DrawCircle_Radius  ' Radius in mm
Variable Numeric DrawCircle_Speed 	' Speed in mm/sec

Variable Numeric DCV1
Variable Numeric DCV2
Variable Numeric DCV3
Variable Numeric DCV4

Label DrawCircle
	' ----------------------------------- test for very low speed
	If DrawCircle_Speed < 0.1
		DrawCircle_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	DCV3 = DrawCircle_Radius * Math_PI * 2
	' ----------------------------------- calc the loops count
	DCV4 = DCV3 / 0.01 '<<< loop time
	DCV4 = Int(DCV4 / DrawCircle_Speed)
	' ----------------------------------- too much speed ?
	If DCV4 < 16
		DCV4 = 16
	EndIf
	' ----------------------------------- prepare the increments
	DCV1 = 2 * Math_PI / DCV4
	' ----------------------------------- loop for v4 times
	For DCV3 = 0 To DCV4
		' ------------------------------- calc circle
		DCV2 = DCV3 * DCV1
		Slot(Slot_X) = Limit(DrawCircle_X + DrawCircle_Radius * Sin(DCV2), Xmin, Xmax)
		Slot(Slot_Y) = Limit(DrawCircle_Y + DrawCircle_Radius * Cos(DCV2), Ymin, Ymax)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return

' ===================================================================
'  JOG with keyboard
' ===================================================================
Label ExecKeys
	Gosub SpeedModifiers
	'
	If Key("Right")
		Slot Slot_X = Limit(Slot(Slot_X) + JogSpeed, Xmin, Xmax)
	EndIf
	If Key("Left")
		Slot Slot_X = Limit(Slot(Slot_X) - JogSpeed, Xmin, Xmax)
	EndIf
	'
	If Key("Up")
		Slot Slot_Y = Limit(Slot(Slot_Y) + JogSpeed, Ymin, Ymax)
	EndIf
	If Key("Down")
		Slot Slot_Y = Limit(Slot(Slot_Y) - JogSpeed, Ymin, Ymax)
	EndIf
	'
	If Key("PageUp")
		Slot Slot_Z = Limit(Slot(Slot_Z) + JogSpeed, Zmin, Zmax)
	EndIf
	If Key("PageDown")
		Slot Slot_Z = Limit(Slot(Slot_Z) - JogSpeed, Zmin, Zmax)
	EndIf
	'
	Gosub PrintValues
Return

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


' ===================================================================
'  GAMEPAD
' ===================================================================
Variable Numeric Slot_JoyPadX
Variable Numeric Slot_JoyPadY
Variable Numeric Slot_JoyPadZ
Variable Numeric Slot_JoyPadStartStop
Variable Numeric Slot_JoyPadSpeed
Variable Numeric Slot_JoyPadTexBox
Variable Numeric Slot_JoyPadSaveCoord
Variable Numeric Slot_JoyPadDispenser
Variable Numeric JoyPad_Speed
Variable Numeric JoyPad_SpeedMult
Variable Numeric JoyPad_DeadBand
Variable Numeric Slot_JoyPadFrontWhite
Variable Numeric Slot_JoyPadRed
Variable Numeric Slot_JoyPadGreen
Variable Numeric Slot_JoyPadBlu
Variable Numeric Slot_JoyPadHome6


Variable Numeric JoyPad_X0
Variable Numeric JoyPad_Y0
Variable Numeric JoyPad_V1
Variable Numeric JoyPad_V2
Variable Numeric JoyPad_V3
Variable Numeric JoyPad_Running

Label JoyPad
	' --------------------------------------- JoyPad Start/Stop
	If JoyPad_Running = 1
		JoyPad_Running = 0
		Gosub WhiteFrontOn
		Gosub BluON
		Return
	Else
		JoyPad_Running = 1
		Gosub WhiteFrontOFF
		Gosub BluOff
	EndIf
	' --------------------------------------- JoyPad Initializations
	Button 13 Color Green9
	Button 13 Text JoyPad RUNNING
	JoyPad_X0 = Slot(Slot_JoyPadX)
	JoyPad_Y0 = Slot(Slot_JoyPadY)
	' --------------------------------------- JoyPad Zero-Test
	If JoyPad_X0 > 550 Or JoyPad_X0 < 450
		Print "JoyPad Error : X = " + Int(JoyPad_X0)
		JoyPad_Running = 0
	EndIf
	If JoyPad_Y0 > 550 Or JoyPad_Y0 < 450
		Print "JoyPad Error : Y = " + Int(JoyPad_Y0)
		JoyPad_Running = 0
	EndIf
	' --------------------------------------- JoyPad Execution
	For JoyPad_V1 = 1 To 9e99
		
		' -----------------------------------
		Wait Seconds 0.01
		If JoyPad_Running = 0
			Exit
		EndIf
		' ----------------------------------- Logaritmic speed adjustment
		JoyPad_SpeedMult = (1000 - Slot(Slot_JoyPadSpeed)) / 1000 - 0.5
		JoyPad_SpeedMult = Pow(10, JoyPad_SpeedMult)
		' ----------------------------------- JoyPad Axis X
		JoyPad_V2 = Slot(Slot_JoyPadX) - JoyPad_X0
		JoyPad_V3 = Abs(JoyPad_V2) - JoyPad_DeadBand
		If JoyPad_V3 > 0
			JoyPad_V3 = JoyPad_V3 * JoyPad_Speed * JoyPad_SpeedMult * 0.00001
			Slot Slot_X = Limit(Slot(Slot_X) + JoyPad_V3 * Sign(JoyPad_V2), Xmin, Xmax)
			Gosub PrintValues
		EndIf
		' ----------------------------------- JoyPad Axis Y
		JoyPad_V2 = Slot(Slot_JoyPadY) - JoyPad_Y0
		JoyPad_V3 = Abs(JoyPad_V2) - JoyPad_DeadBand
		If JoyPad_V3 > 0
			JoyPad_V3 = JoyPad_V3 * JoyPad_Speed * JoyPad_SpeedMult * 0.00001
			Slot Slot_Y = Limit(Slot(Slot_Y) + JoyPad_V3 * Sign(JoyPad_V2), Ymin, Ymax)
			Gosub PrintValues
		EndIf
		' ----------------------------------- JoyPad Axis Z
		Select Slot(Slot_JoyPadZ)
			Case 360
				Slot Slot_Z = Limit(Slot(Slot_Z) + JoyPad_Speed * JoyPad_SpeedMult * 0.001, Zmin, Zmax)
				Gosub PrintValues
			Case 180
				Slot Slot_Z = Limit(Slot(Slot_Z) - JoyPad_Speed * JoyPad_SpeedMult * 0.001, Zmin, Zmax)
				Gosub PrintValues
		EndSelect
		'
		' ----------------------------------- JoyPad Dispenser Button
		Slot(Slot_Dispenser) = Slot(Slot_JoyPadDispenser)
		' ----------------------------------- JoyPad White Front button
		If Slot(Slot_JoyPadFrontWhite) > 500
			Slot(Slot_WhiteFront) = 1000 - Slot(Slot_WhiteFront)
			Wait Slot(Slot_JoyPadFrontWhite) < 500
		EndIf
		' ----------------------------------- JoyPad Open/Close coordinate text box
		Variable Numeric TextBoxIsOpened
		If Slot(Slot_JoyPadTexBox) > 500
			If TextBoxIsOpened = 0
				Controls OpenTextBox
				TextBoxIsOpened = 1
			Else
				Controls CloseTextBox
				TextBoxIsOpened = 0
			EndIf
			Wait Slot(Slot_JoyPadTexBox) < 500
		EndIf
		' ----------------------------------- JoyPad Save coordinate on a file
		If Slot(Slot_JoyPadSaveCoord) > 500
			s1 =        "X  = " + Format(Slot(Slot_X), "0.00")
			s1 = s1 + "  Y  = " + Format(Slot(Slot_Y), "0.00")
			s1 = s1 + "  Z  = " + Format(Slot(Slot_Z), "0.00")
			Save StringToFile s1 FileName
			Wait Slot(Slot_JoyPadSaveCoord) < 500
		EndIf
		'
		' ----------------------------------- JoyPad Red button
		Slot(Slot_LedR) = Slot(Slot_JoyPadRed)
		' ----------------------------------- JoyPad Green button
		Slot(Slot_LedG) = Slot(Slot_JoyPadGreen)
		' ----------------------------------- JoyPad Blu button
		Slot(Slot_LedB) = Slot(Slot_JoyPadBlu)
		' ----------------------------------- JoyPad Home button
		If Slot(Slot_JoyPadHome6) > 500
			Wait Slot(Slot_JoyPadHome6) < 500
			Gosub Home
		EndIf
		' ----------------------------------- JoyPad Dispenser Button
		Slot(Slot_Dispenser) = Slot(Slot_JoyPadDispenser)
	Next
	' --------------------------------------- JoyPad Finalizations
	Button 13 Text JoyPad
	Button 13 Color Orange
	Gosub WhiteFrontOFF
	Gosub BluOff
Return

' ===================================================================
'  PRINT COORDINATES
' ===================================================================
Label PrintValues
	s1 =       "X=" + Format(Slot(Slot_X), "0.000")
	s1 = s1 + " Y=" + Format(Slot(Slot_Y), "0.000")
	s1 = s1 + " Z=" + Format(Slot(Slot_Z), "0.000")
	Print CRLF + s1
Return


'WATCH SlotMotor
'WATCH Abs(Slot(SlotMotor))
'WATCH v1
'WATCH Load
'EXEC Sin2 = Sin(33)
