' ====================================================================
'     Name:	WXM_XYZ_SAMSUNG_Solder_5.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_GamepadX = 200
Slot_GamepadY = 201
Slot_GamepadZ = 206
Slot_GamepadStartStop = 220
Slot_GamepadDispenser = 210
Slot_GamepadSpeed = 202
Slot_GamepadHome6 = 215
Slot_GamepadTexBox = 212
Slot_GamepadSaveCoord = 213

Slot_GamepadFrontWhite = 216
Slot_GamepadRed = 217
Slot_GamepadGreen = 219
Slot_GamepadBlu = 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 14 Text CLOSE ALL
Button 14 Color Red

Gosub DisableAllButtons

' -------------------------------------- KEYS
Key Right		Gosub MoveRight
Key Left 		Gosub MoveLeft
Key Up 			Gosub MoveFar
Key Down 		Gosub MoveNear
Key Pageup		Gosub MoveUp
Key Pagedown	Gosub MoveDown

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_Gamepad\Theremino_Gamepad\Theremino_Gamepad.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

' ====================================================================
'  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 "MOVE"
						MotorNumber = GetSeparatedString(SequenceCmd, 1)
						Gosub MotorNumberToSlot
						Slot(MotorSlotFromNumber) = GetSeparatedString(SequenceCmd, 2)
						Gosub WaitAllMotors
					Case "MOVENOWAIT"
						MotorNumber = GetSeparatedString(SequenceCmd, 1)
						Gosub MotorNumberToSlot
						Slot(MotorSlotFromNumber) = GetSeparatedString(SequenceCmd, 2)
					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) = MoveSlowXY_DestX
		Slot(Slot_Y) = MoveSlowXY_DestY
		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) = Slot(Slot_X) + MSV1
		Slot(Slot_Y) = Slot(Slot_Y) + MSV2
		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) = MoveSlowZ_Dest
		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) = Slot(Slot_Z) + MSV1
		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) = DrawCircle_X + DrawCircle_Radius * Sin(DCV2)
		Slot(Slot_Y) = DrawCircle_Y + DrawCircle_Radius * Cos(DCV2)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return

' ===================================================================
'  JOG with keyboard
' ===================================================================
Label MoveRight
	Gosub Enable Motors
	Gosub SpeedModifiers
	Slot Slot_X = Slot(Slot_X) + JogSpeed
	Gosub SoftwareLimits
Return

Label MoveLeft
	Gosub Enable Motors
	Gosub SpeedModifiers
	Slot Slot_X = Slot(Slot_X) - JogSpeed
	Gosub SoftwareLimits
Return

Label MoveFar
	Gosub Enable Motors
	Gosub SpeedModifiers
	Slot Slot_Y = Slot(Slot_Y) + JogSpeed
	Gosub SoftwareLimits
Return

Label MoveNear
	Gosub Enable Motors
	Gosub SpeedModifiers
	Slot Slot_Y = Slot(Slot_Y) - JogSpeed
	Gosub SoftwareLimits
Return

Label MoveUp
	Gosub Enable Motors
	Gosub SpeedModifiers
	Slot Slot_Z = Slot(Slot_Z) + JogSpeed
	Gosub SoftwareLimits
Return

Label MoveDown
	Gosub Enable Motors
	Gosub SpeedModifiers
	Slot Slot_Z = Slot(Slot_Z) - JogSpeed
	Gosub SoftwareLimits
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_GamepadX
Variable Numeric Slot_GamepadY
Variable Numeric Slot_GamepadZ
Variable Numeric Slot_GamepadStartStop
Variable Numeric Slot_GamepadSpeed
Variable Numeric Slot_GamepadTexBox
Variable Numeric Slot_GamepadSaveCoord
Variable Numeric Slot_GamepadDispenser
Variable Numeric Gamepad_Speed
Variable Numeric Gamepad_SpeedMult
Variable Numeric Gamepad_DeadBand
Variable Numeric Slot_GamepadFrontWhite
Variable Numeric Slot_GamepadRed
Variable Numeric Slot_GamepadGreen
Variable Numeric Slot_GamepadBlu
Variable Numeric Slot_GamepadHome6


Variable Numeric Gamepad_X0
Variable Numeric Gamepad_Y0
Variable Numeric Gamepad_V1
Variable Numeric Gamepad_V2
Variable Numeric Gamepad_V3
Variable Numeric Gamepad_Running

Label Gamepad
	' --------------------------------------- Gamepad Start/Stop
	If Gamepad_Running = 1
		Gamepad_Running = 0
		Gosub WhiteFrontOn
		Gosub BluON
		Return
	Else
		Gamepad_Running = 1
		Gosub WhiteFrontOFF
		Gosub BluOff
	EndIf
	' --------------------------------------- Gamepad Initializations
	Button 13 Color Green9
	Button 13 Text Gamepad RUNNING
	Gamepad_X0 = Slot(Slot_GamepadX)
	Gamepad_Y0 = Slot(Slot_GamepadY)
	' --------------------------------------- Gamepad Zero-Test
	If Gamepad_X0 > 550 Or Gamepad_X0 < 450
		Print "Gamepad Error : X = " + Int(Gamepad_X0)
		Gamepad_Running = 0
	EndIf
	If Gamepad_Y0 > 550 Or Gamepad_Y0 < 450
		Print "Gamepad Error : Y = " + Int(Gamepad_Y0)
		Gamepad_Running = 0
	EndIf
	' --------------------------------------- Gamepad Execution
	For Gamepad_V1 = 1 To 9e99
		
		' -----------------------------------
		Wait Seconds 0.01
		If Gamepad_Running = 0
			Exit
		EndIf
		' ----------------------------------- Logaritmic speed adjustment
		Gamepad_SpeedMult = (1000 - Slot(Slot_GamepadSpeed)) / 1000 - 0.5
		Gamepad_SpeedMult = Pow(10, Gamepad_SpeedMult)
		' ----------------------------------- Gamepad Axis X
		Gamepad_V2 = Slot(Slot_GamepadX) - Gamepad_X0
		Gamepad_V3 = Abs(Gamepad_V2) - Gamepad_DeadBand
		If Gamepad_V3 > 0
			Gamepad_V3 = Gamepad_V3 * Gamepad_Speed * Gamepad_SpeedMult * 0.00001
			Slot Slot_X = Slot(Slot_X) + Gamepad_V3 * Sign(Gamepad_V2)
			Gosub SoftwareLimits
		EndIf
		' ----------------------------------- Gamepad Axis Y
		Gamepad_V2 = Slot(Slot_GamepadY) - Gamepad_Y0
		Gamepad_V3 = Abs(Gamepad_V2) - Gamepad_DeadBand
		If Gamepad_V3 > 0
			Gamepad_V3 = Gamepad_V3 * Gamepad_Speed * Gamepad_SpeedMult * 0.00001
			Slot Slot_Y = Slot(Slot_Y) + Gamepad_V3 * Sign(Gamepad_V2)
			Gosub SoftwareLimits
		EndIf
		' ----------------------------------- Gamepad Axis Z
		Select Slot(Slot_GamepadZ)
			Case 360
				Slot Slot_Z = Slot(Slot_Z) + Gamepad_Speed * Gamepad_SpeedMult * 0.001
				Gosub SoftwareLimits
			Case 180
				Slot Slot_Z = Slot(Slot_Z) - Gamepad_Speed * Gamepad_SpeedMult * 0.001
				Gosub SoftwareLimits
		EndSelect
		'
		' ----------------------------------- Gamepad Dispenser Button
		Slot(Slot_Dispenser) = Slot(Slot_GamepadDispenser)
		' ----------------------------------- Gamepad White Front button
		If Slot(Slot_GamepadFrontWhite) > 500
			Slot(Slot_WhiteFront) = 1000 - Slot(Slot_WhiteFront)
			Wait Slot(Slot_GamepadFrontWhite) < 500
		EndIf
		' ----------------------------------- Gamepad Open/Close coordinate text box
		Variable Numeric TextBoxIsOpened
		If Slot(Slot_GamepadTexBox) > 500
			If TextBoxIsOpened = 0
				Controls OpenTextBox
				TextBoxIsOpened = 1
			Else
				Controls CloseTextBox
				TextBoxIsOpened = 0
			EndIf
			Wait Slot(Slot_GamepadTexBox) < 500
		EndIf
		' ----------------------------------- Gamepad Save coordinate on a file
		If Slot(Slot_GamepadSaveCoord) > 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_GamepadSaveCoord) < 500
		EndIf
		'
		' ----------------------------------- Gamepad Red button
		Slot(Slot_LedR) = Slot(Slot_GamepadRed)
		' ----------------------------------- Gamepad Green button
		Slot(Slot_LedG) = Slot(Slot_GamepadGreen)
		' ----------------------------------- Gamepad Blu button
		Slot(Slot_LedB) = Slot(Slot_GamepadBlu)
		' ----------------------------------- Gamepad Home button
		If Slot(Slot_GamepadHome6) > 500
			Wait Slot(Slot_GamepadHome6) < 500
			Gosub Home
		EndIf
		' ----------------------------------- Gamepad Dispenser Button
		Slot(Slot_Dispenser) = Slot(Slot_GamepadDispenser)
	Next
	' --------------------------------------- Gamepad Finalizations
	Button 13 Text Gamepad
	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

' ===================================================================
'  SOFTWARE LIMITS
' ===================================================================
Variable Numeric Xmax
Variable Numeric Xmin
Variable Numeric Ymax
Variable Numeric Ymin
Variable Numeric Zmax
Variable Numeric Zmin

Label SoftwareLimits
	If Slot(Slot_X) > Xmax
		Slot(Slot_X) = Xmax
	EndIf
	If Slot(Slot_X) < Xmin
		Slot(Slot_X) = Xmin
	EndIf
	
	If Slot(Slot_Y) > Ymax
		Slot(Slot_Y) = Ymax
	EndIf
	If Slot(Slot_Y) < Ymin
		Slot(Slot_Y) = Ymin
	EndIf
	
	If Slot(Slot_Z) > Zmax
		Slot(Slot_Z) = Zmax
	EndIf
	If Slot(Slot_Z) < Zmin
		Slot(Slot_Z) = Zmin
	EndIf
	Gosub PrintValues
Return


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