' ====================================================================
'     Name: WXM_DoorLockBT_11.txt                    Date: 13 Aug 2020
'    Autor: Leonardo DE PALO
'    Scope: Test Door lock smart phone Blue Tooth based.
' Hardware: Standard Master.
' ====================================================================

Load BT_finger_emulator.jpg
Load sound\gong.wav
Option Speed 9

' -------------------------------------- Slots
NumMotors         = 4
Slot_X            = 1
Slot_Y            = 3
Slot_Z            = 5
Slot_A            = 25

Slot_SensorX      = 19
Slot_SensorY      = 20
Slot_SensorA      = 31
Slot_SensorZ      = 21

Slot_EnableMotors = 11

Slot_Dispenser = -1
Slot_Emergency    = -1

Slot_WhiteFront = -1
Slot_LedR = 16
Slot_LedG = 17
Slot_LedB = 18

Slot_JoyPadX = 200
Slot_JoyPadY = 201
Slot_JoyPadZ = 206
Slot_JoyPadA = 201
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

Slot_PEN          = 13

Slot_LASER        = 30
Slot_LockSensor   = 32

' -------------------------------------- OPTIONS
JoyPad_DeadBand  = 5
JoyPad_Speed     = 200
KeyJog_Speed     = 200

PenDown_Value    = 232
PenUp_Value      = 280

' -------------------------------------- Software limits
Xmax =  190
Xmin =    2
Ymax =  500
Ymin =    2
Zmax =  190
Zmin =  100
Amax =  500
Amin =    2

' -------------------------------------- BUTTONS
Button  1 Text  Home
Button  1 Color SkyBlue
Button  2 Text  Home_X
Button  2 Color SkyBlue
Button  3 Text  Home_Y
Button  3 Color SkyBlue
Button  3 Disabled
Button  4 Text  Home_Z
Button  4 Color SkyBlue
Button  5 Text  Home_A
Button  5 Color SkyBlue

Button  6 Text  CONTINUE TEST
Button  6 Color Yellow9
Button  7 Text  PEN UP
Button  7 Color LightSteelBlue
Button  8 Text  PEN DOWN
Button  8 Color LightSteelBlue
Button  9 Text  PEN SINGLE
Button  9 Color LightSteelBlue
Button 10 Text  re-start test
Button 10 Color Yellow9

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

Button 15 Text  EMERGENCY
Button 15 Color Red
Button 15 Slot  Slot_Emergency

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
Key A 			Gosub ExecKeys
Key Z 			Gosub ExecKeys

' -------------------------------------- LOG FILE SET UP
Variable String FileHeader
Variable String LogFileName
LogFileName = "TestLogDoorLockBT.txt"
FileHeader = CRLF
FileHeader = FileHeader + "- DATE - " + " - TIME - " + " CYCLE "
FileHeader = FileHeader + " RESULT " + "    NUM. " + "    RATE IN %"
'FileHeader = FileHeader + "- = - = - = -  START  - = - = - = - "
Save StringToFile FileHeader LogFileName

' -------------------------------------- LOAD APPS
Load "Theremino_HAL\Theremino_HAL.exe"
Load "Theremino_HAL\Theremino_SlotViewer1.exe"
Load "Theremino_HAL\Theremino_SlotViewer2.exe"
Load "Theremino_HAL\Theremino_SignalScope.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 ""

' -------------------------------------- Ensure motor stopped and enable
Gosub WaitAllMotors
Gosub Enable Motors

' -------------------------------------- Restore old motor positions and variables
Gosub RestoreVarsAndPositions

' -------------------------------------- Miscellanous start
Gosub PEN UP
'Window MinSize
COM Open 1 115200
Wait Seconds 1
Load Hide
'Gosub MechTest
Gosub EnableAllButtons
Button  3 Disabled

Stop						 '<<<<<<<<<<<<<< STOP >>>>>>>>>>>>>>


' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_X
Variable Numeric Slot_Y
Variable Numeric Slot_A
Variable Numeric Slot_Z

Variable Numeric Slot_SensorX
Variable Numeric Slot_SensorY
Variable Numeric Slot_SensorZ
Variable Numeric Slot_SensorA

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

Variable Numeric Slot_Dispenser
Variable Numeric Slot_EnableMotors
Variable Numeric Slot_Emergency
Variable Numeric Slot_LASER
Variable Numeric Slot_LockSensor

Variable Numeric Slot_PEN
Variable Numeric PenDown_Value
Variable Numeric PenUp_Value

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

' ====================================================================
'  EMERGENCY
' ====================================================================
Label EMERGENCY
	Gosub DisableAllButtons
	Button 15 Enabled
	Button 15 Color Flashing7 Yellow Red
	KeyJog_Speed = 0
	For v1 = 1 To 9e99
		Gosub RedOn
		Wait Seconds 0.4
		Gosub RgbOff
		Wait Seconds 0.4
		Load sound\Alarm10b.wav
	Next
Stop

' ====================================================================
'  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 SaveVarsAndPositions
	Gosub Disable Motors
	Load CloseApps
Return

Label EventStop
	Gosub SaveVarsAndPositions
	Gosub Disable Motors
	Gosub EnableAllButtons
	Button  3 Disabled
	Window Sizable
End

' ====================================================================
'  SAVE LOAD and CLEAR VARS and POSITIONS
' ====================================================================
Label SaveVarsAndPositions
	v1 = Slot(Slot_X)
	v2 = Slot(Slot_Y)
	v3 = Slot(Slot_Z)
	v4 = Slot(Slot_A)
	v5 = NumCycle
	v6 = ErrorNumber
	Save Vars
Return

Label RestoreVarsAndPositions
	Load Vars
	If v1 <> 0 And v2 <> 0 And v3 <> 0
		Slot(Slot_X) = Reset
		Wait Seconds 0.01
		Slot(Slot_X) = v1
		Wait Seconds 0.01
		
		Slot(Slot_Y) = Reset
		Wait Seconds 0.01
		Slot(Slot_Y) = v2
		Wait Seconds 0.01
		
		Slot(Slot_Z) = Reset
		Wait Seconds 0.01
		Slot(Slot_Z) = v3
		Wait Seconds 0.01
		
		Slot(Slot_A) = Reset
		Wait Seconds 0.01
		Slot(Slot_A) = v4
		Wait Seconds 0.01
		
	EndIf
	'
	NumCycle = v5
	ErrorNumber = v6
	'
	Gosub PrintValues
Return

Label ClearVars
	NumCycle = 0
	ErrorNumber = 0
Return

' ====================================================================
'   FUNCTIONS
' ====================================================================
Label Enable Motors
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 1000
Return

Label Disable Motors
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 0
Return

' ====================================================================
'  TEST
' ====================================================================
Variable Numeric NumCycle
Variable Numeric ErrorNumber
Variable Numeric ErrorPercentage
Variable Numeric RedFlash
Variable String  Result

Label CONTINUE TEST
	Save StringToFile FileHeader FileName
	Gosub Home
	For NumCycle = NumCycle To 60000
		Gosub SaveVars
		
		SequenceFileName = "SEQUENCES\HandleDown.seq"
		Gosub ExecSequence
		
        '------------------ laser on and test
		Slot(Slot_LASER) = 1000    ' LASER ON
		Wait Seconds 0.2
		
		If Slot(Slot_LockSensor) > 500
			Gosub TEST PASSED
		Else
			Gosub TEST FAIL
		EndIf
		
		Slot(Slot_LASER) = 0    ' LASER OFF
		Wait Seconds 1
		
		SequenceFileName = "SEQUENCES\HandleUp.seq"
		Gosub ExecSequence
		
		Print "Cycle number:" + NumCycle
'		Wait Seconds 1.5
	Next
Return

Label re-start test
	Gosub ClearVars
	Gosub CONTINUE TEST
	
Return

Label TEST PASSED
	Gosub Green
	Result = " -PASS- "
	Gosub UPDATE_FILE
Return

Label TEST FAIL
	Gosub Red
	Gosub SaveRecord
	Load Sound\DoorBell.wav
	ErrorNumber = ErrorNumber + 1
	ErrorPercentage = (100 * ErrorNumber) / NumCycle
	Result = " -FAIL- " + "Num Err:" + Format(ErrorNumber, "000") + "   %=" + Format(ErrorPercentage, "00.00")
	Gosub UPDATE_FILE
	Gosub RED ALARM
Return

Label UPDATE_FILE
	s1 = ""
	s1 = Now
	s1 = s1 + " " + Format(NumCycle, "000") + " " + Result
	s2 = "Test: " + Format(NumCycle, "000")
	Print S2
	Save StringToFile s1 LogFileName
Return

Label RED ALARM
	Load Sound\Alarm_2_times.wav		'<<<<<<<<<<<<<<<<
	For RedFlash = 1 To 5
		Gosub RgbOff
		Wait Seconds 0.2
		Gosub Red
		Wait Seconds 0.15
	Next
Return

' ====================================================================
'  V I D E O
' ====================================================================
Label StartRecord
	Slot(49) = 1000			'	Start recording
Return

Label StopRecord
	Slot(49) = 0
Return

Label SaveRecord
	Slot(50) = 1000			' save to file
	Wait Seconds 0.1
	Slot(50) =    0
	Slot(49) =    0			' Stop recording and save to file
Return


' ====================================================================
'  PEN UP/DOWN
' ====================================================================
Label PEN UP
	Slot(Slot_PEN) = PenUp_Value
	Wait Seconds 0.1
Return

Label PEN DOWN
	Slot(Slot_PEN) = PenDown_Value
	Wait Seconds 0.1
Return

Label PEN SINGLE
	Gosub Blu
	Gosub PEN DOWN
	Wait Seconds 0.35
	Gosub PEN UP
	Gosub Green
Return

' ====================================================================
'  LED COLORS
' ====================================================================
Label RedOn
	Slot Slot_LedR = 800
	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 BluON
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 800
Return

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

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

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

Label WhiteFrontOn
	Slot Slot_WhiteFront = 1000
Return

Label WhiteFrontOFF
	Slot Slot_WhiteFront = 0
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), " ") 'TABS to SPACES
		SequenceCmd = RemoveComments(SequenceCmd)
		SequenceCmd = Trim(SequenceCmd)
		'
		Print " Executing command: " + SequenceCmd
		'
		Select UCase(SequenceCmd)
            ' ---------------------------------------------- End sequence
			Case "ENDSEQ"
				SequenceIndex = SequenceLength-1
		    ' ---------------------------------------------- ACTIONS
			Case "ENABLE MOTORS"     ' enable movement motors
				Gosub Enable Motors
			Case "DISABLE MOTORS"    ' disable movement motors
				Gosub Disable Motors
			Case "WAIT_MOTORS"
				Gosub WaitAllMotors
			Case "CLOSE ALL"
				Gosub CLOSE ALL
			' ---------------------------------------------- HOMING
			Case "HOME"
				Gosub Home
			Case "HOMEX"
				Gosub Home_X
			Case "HOMEY"
				Gosub Home_Y
'			Case "HOMEZ"
'				Gosub HOME_Z
			Case "HOMEA"
				Gosub Home_A
			' ---------------------------------------------- LED COLORS
			Case "REDON"
				Gosub RedOn
			Case "GREENON"
				Gosub GreenOn
			Case "BLUON"
				Gosub BluON
			Case "YELLOWON"
				Gosub YellowOn
			Case "WHITEON"
				Gosub WhiteOn
			Case "RGBOFF"
				Gosub RgbOff
			Case "WHITEFRONTON"
				Gosub WhiteFrontOn
			Case "WHITEFRONTOFF"
				Gosub WhiteFrontOFF
			' ---------------------------------------------- Pen actions
			Case "PEN UP"            ' move pen up
				Gosub PEN UP
			Case "PEN DOWN"          ' move pen down
				Gosub PEN DOWN
			Case "PEN SINGLE"        ' single click
				Gosub PEN SINGLE
			' ---------------------------------------------- With parameters
			CaseElse
				s1 = GetSeparatedString(SequenceCmd, 0)
				Select UCase(s1)
					Case "MOVE_X"
						Slot(Slot_X) = GetSeparatedString(SequenceCmd, 1)
						Gosub WaitAllMotors
					Case "MOVE_Y"
						Slot(Slot_Y) = GetSeparatedString(SequenceCmd, 1)
						Gosub WaitAllMotors
					Case "MOVE_Z"
						Slot(Slot_Z) = GetSeparatedString(SequenceCmd, 1)
						Gosub WaitAllMotors
					Case "MOVE_XY"
						Slot(Slot_X) = GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_Y) = GetSeparatedString(SequenceCmd, 2)
						Gosub WaitAllMotors
					Case "MOVE_XYZ"
						Slot(Slot_X) = GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_Y) = GetSeparatedString(SequenceCmd, 2)
						Slot(Slot_Z) = GetSeparatedString(SequenceCmd, 3)
					Case "MOVE_XAZ"
						Slot(Slot_X) = GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_A) = GetSeparatedString(SequenceCmd, 2)
						Slot(Slot_Z) = GetSeparatedString(SequenceCmd, 3)
						Gosub WaitAllMotors
					Case "PAUSE"
						Wait Seconds GetSeparatedString(SequenceCmd, 1)
					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

' ====================================================================
'  HOME  X Y Z A
' ====================================================================
Label Home
	Gosub Enable Motors
	Gosub Home_Z
	Gosub Home_X
	Gosub Home_Y
	Gosub Home_A
Return

Label Home_X
	Gosub Enable Motors
	SlotMotor = Slot_X
	SlotSensor = Slot_SensorX
	SpeedSearch = -0.1
	SpeedReturn = +0.01
	DeclaredSwitchPos = 0
	InitialPosition = 2
	Gosub HomeMotor
	Gosub PrintValues
	Button 2 Color SkyBlue
Return

Label Home_Y
	Gosub Enable Motors
	SlotMotor = Slot_Y
	SlotSensor = Slot_SensorY
	SpeedSearch = -0.1
	SpeedReturn = 0.015
	DeclaredSwitchPos = 0
	InitialPosition = 2
	Gosub HomeMotor
	Gosub PrintValues
	Button 3 Color SkyBlue
Return

Label Home_Z
	Gosub Enable Motors
	SlotMotor = Slot_Z
	SlotSensor = Slot_SensorZ
	SpeedSearch = 0.1
	SpeedReturn = -0.02
	DeclaredSwitchPos = 192
	InitialPosition = 190
	Gosub HomeMotor
	Gosub PrintValues
	Button 5 Color SkyBlue
Return

Label Home_A
	Gosub Enable Motors
	SlotMotor = Slot_A
	SlotSensor = Slot_SensorA
	SpeedSearch = -0.1
	SpeedReturn = 0.015
	DeclaredSwitchPos = 0
	InitialPosition = 2
	Gosub HomeMotor
	Gosub PrintValues
	Button 4 Color SkyBlue
Return

' ====================================================================
'  Home the Motor(SlotMotor) to the Sensor(SlotSensor)
' ====================================================================
Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredSwitchPos
Variable Numeric InitialPosition

Label HomeMotor
	Gosub DisableAllButtons
	Button 15 Enabled
	' ------------------------- detach if sensor is active
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	EndIf
    ' ------------------------- search the sensor
Label SearchHome
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedSearch
	Wait Seconds 0.001
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		Slot(SlotMotor) = Reset
		Wait seconds 0.1
		Slot(SlotMotor) = 0
		Wait seconds 0.1
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' ------------------------- detach from the sensor
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Slot(SlotMotor) = Reset
		Wait Seconds 0.1
		Slot(SlotMotor) = DeclaredSwitchPos
		Wait Seconds 0.1
		Goto SetFinalValues
	EndIf
Goto DetachSensor
    ' ------------------------- reset and move to InitialPos
Label SetFinalValues
	Slot(SlotMotor) = InitialPosition
	Gosub WaitMotor
	Gosub EnableAllButtons
	Button  3 Disabled
Return

' ====================================================================
'   Wait until the motor(SlotMotor) is stopped
' ====================================================================
Variable Numeric WaitMot1
Label WaitMotor
	Wait Seconds 0.1
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(SlotMotor + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return

' ====================================================================
'   Wait until all the 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(SlotFromMotorNumber + 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 SlotFromMotorNumber

Label MotorNumberToSlot
	Select MotorNumber
		Case 0
			SlotFromMotorNumber = Slot_X
		Case 1
			SlotFromMotorNumber = Slot_Y
		Case 2
			SlotFromMotorNumber = Slot_Z
		Case 3
			SlotFromMotorNumber = Slot_A
	EndSelect
Return

' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	Variable Numeric KeyJog_Speed
	Variable Numeric JogSpeed
	JogSpeed = KeyJog_Speed / 100
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 0.1
		Case Key("CTRL")
			JogSpeed = JogSpeed * 0.01
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.001
	EndSelect
	'
	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
	'
	If Key("A")
		Slot Slot_A = Limit(Slot(Slot_A) + JogSpeed, Amin, Amax)
	EndIf
	If Key("Z")
		Slot Slot_A = Limit(Slot(Slot_A) - JogSpeed, Amin, Amax)
	EndIf
	'
	Gosub PrintValues
Return

' ====================================================================
'  JoyPad
' ====================================================================
Variable Numeric Slot_JoyPadX
Variable Numeric Slot_JoyPadY
Variable Numeric Slot_JoyPadZ
Variable Numeric Slot_JoyPadA
Variable Numeric Slot_JoyPadStartStop
Variable Numeric Slot_JoyPadSpeed
Variable Numeric Slot_JoyPadTexBox
Variable Numeric Slot_JoyPadSaveCoord
Variable Numeric Slot_JoyPadDispenser
Variable Numeric Slot_JoyPadFrontWhite
Variable Numeric Slot_JoyPadRed
Variable Numeric Slot_JoyPadGreen
Variable Numeric Slot_JoyPadBlu
Variable Numeric Slot_JoyPadHome6

Variable Numeric JoyPad_DeadBand
Variable Numeric JoyPad_Speed
Variable Numeric JoyPad_SpeedMult

Variable Numeric JoyPad_X0
Variable Numeric JoyPad_Y0
Variable Numeric JoyPad_Z0
Variable Numeric JoyPad_A0
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 RgbOff
	EndIf
	' --------------------------------------- JoyPad Initializations
	Button 13 Color Green9
	Button 13 Text JoyPad RUNNING
	JoyPad_X0 = Slot(Slot_JoyPadX)
	JoyPad_Y0 = Slot(Slot_JoyPadY)
	JoyPad_Z0 = Slot(Slot_JoyPadZ)
	JoyPad_A0 = Slot(Slot_JoyPadA)
	' --------------------------------------- JoyPad Zero-Test
	If JoyPad_X0 > 550 Or JoyPad_X0 < 450
		Print "JoyPad Error not center : X = " + Int(JoyPad_X0)
		JoyPad_Running = 0
	EndIf
	If JoyPad_Y0 > 550 Or JoyPad_Y0 < 450
		Print "JoyPad Error not center : 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 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 Axis A
		JoyPad_V2 = Slot(Slot_JoyPadA) - JoyPad_A0
		JoyPad_V3 = Abs(JoyPad_V2) - JoyPad_DeadBand
		If JoyPad_V3 > 0
			JoyPad_V3 = JoyPad_V3 * JoyPad_Speed * JoyPad_SpeedMult * 0.00001
			Slot Slot_A = Limit(Slot(Slot_A) + JoyPad_V3 * Sign(JoyPad_V2), Amin, Amax)
			Gosub PrintValues
		EndIf
		'
		' ----------------------------------- 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 RgbOff
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")
	s1 = s1 + " A=" + Format(Slot(Slot_A), "0.000")
	Print CRLF + s1
Return

' ====================================================================
'  ReceiveFileFromCOM
' ====================================================================
Label ReceiveFileFromCOM
	Variable String FileName
	Variable String Str1
	'
	COM DiscardInBuffer
	'
	Str1 = ""
	For v1 = 1 To 9e99
		s1 = COM_Received
		If s1 <> ""
			s1 = Replace(s1, "{{{CRLF}}}", CRLF)
			Str1 = Str1 + s1
			If s1 = "{{{FileEndMarker}}}"
				Str1 = Replace(Str1, "{{{FileEndMarker}}}", "")
				Save DeleteFile FileName
				Save StringToFile Str1 FileName
				Exit
			EndIf
		EndIf
		Wait Seconds 0.01
	Next
Return


'WATCH s1
'EXEC DestX = 12
