'---------------------------------------------------------------------
' File name:  WXM_600E_SAMSUNGXY_45.txt             Date: 11 Ago 2020
'---------------------------------------------------------------------
' WARNING  this version have the 3 encoders on Master N. 2
' ====================================================================
'  Trial with low number of encoder pulse to increase reliability
'  if the FW on the Masteron the en
' ====================================================================
' Encoder 100 ppr
' HAL set for 400 ppr
' Stepper 1.8 degree
' Step per full turn = 800 (jumper on B)
'---------------------------------------------------------------------

Load SAMSUNG_filter_tip.jpg
Option Speed 9

FileName = "Coordinate.txt"

' -------------------------------------- Slots
NumMotors   = 3
Slot_M0     = 1
Slot_M1     = 3
Slot_M2     = 5

Slot_X = 101
Slot_Y = 103
Slot_Z = 5

Slot_SensorM0 = 7
Slot_SensorM1 = 8
Slot_SensorM2 = 9

Slot_CounterReaderOutM0 = 25
Slot_CounterReaderResetM0 = 26
Slot_CounterReaderOutM1 = 27
Slot_CounterReaderResetM1 = 28
Slot_CounterReaderOutM2 = 29
Slot_CounterReaderResetM2 = 30
Tolerance = 1000 					' tolerance for encoder recovery

Slot_EnableMotors = 10
Slot_Dispenser = -1
Slot_Emergency = 11

Slot_WhiteFront = 15
Slot_LedR = 16
Slot_LedG = 17
Slot_LedB = 18

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

Slot_StartStopVideo = 222
Slot_SaveVideo = 223
Slot_TakeImage = 224

' -------------------------------------- OPTIONS
JoyPad_DeadBand = 30
JoyPad_Speed = 50
KeyJog_Speed = 50

' -------------------------------------- Software limits
Xmax =  190
Xmin = -190
Ymax =  -90
Ymin = -280
Zmax =  190
Zmin =  100

' -------------------------------------- BUTTONS
Button  1 Text  Home
Button  1 Color SkyBlue
Button  2 Text  Homing_M2 Z
Button  2 Color SkyBlue
Button  3 Text  SAMSUNG front
Button  3 Color Yellow4
Button  4 Text  SAMSUNG back
Button  4 Color Yellow4

Button  5 Text  TIP calibration
Button  5 Color LightCyan
Button  6 Text  Stress Test
Button  6 Color LightCyan

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  WhiteOn
Button 11 Color White
'Button 11 Text  Enable Motors
'Button 11 Color Tan
'Button 12 Text  Disable Motors
'Button 12 Color Tan
Button 12 Text  TestEncoder
Button 12 Color Green

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
Button 15 Enabled

' -------------------------------------- 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 M			Gosub Enable Motors

' -------------------------------------- LOAD EXTERNAL PROGRAMS
Load "Theremino_HAL\Theremino_HAL.exe"
Load "RoboDriver\RoboDriver\RoboDriver.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_HAL\Theremino_SignalScope.exe"
Load "Theremino_HAL\Theremino_CounterReader1.exe"
Load "Theremino_HAL\Theremino_CounterReader2.exe"
Load "Theremino_HAL\Theremino_CounterReader3.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 ""

' -------------------------------------- Slot init
Slot Slot_Dispenser = 0
Slot 110 = 0 							'STOP recording

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

' -------------------------------------- Restore old motor positions
Gosub RestorePositions

' -------------------------------------- COLORS
Gosub WhiteFrontOFF
Gosub RgbOff
Wait Seconds 0.3
Gosub RedOn
Wait Seconds 0.3
Gosub GreenOn
Wait Seconds 0.3
Gosub BluON
Wait Seconds 0.3
Gosub RgbOff
Wait Seconds 0.3

Gosub WhiteFrontOn
Wait Seconds 0.3
Gosub WhiteFrontOFF
Gosub EnableAllButtons

' --------------------------------------- START
Gosub StartVideo
Wait Seconds 0.3
Gosub StopVideo

Load Hide
Stop				 '<<<<<<<<<<<<<< STOP >>>>>>>>>>>>>>

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric NumMotors
Variable Numeric Slot_M0
Variable Numeric Slot_M1
Variable Numeric Slot_M2
Variable Numeric Slot_M3
Variable Numeric Slot_M4
Variable Numeric Slot_M5
Variable Numeric Slot_M6
Variable Numeric Slot_M7
Variable Numeric Slot_M8

Variable Numeric Slot_X
Variable Numeric Slot_Y
Variable Numeric Slot_Z

Variable Numeric Slot_SensorM0
Variable Numeric Slot_SensorM1
Variable Numeric Slot_SensorM2
Variable Numeric Slot_CounterReaderOutM0
Variable Numeric Slot_CounterReaderOutM1
Variable Numeric Slot_CounterReaderOutM2
Variable Numeric Slot_CounterReaderOutM3
Variable Numeric Slot_CounterReaderOutM4
Variable Numeric Slot_CounterReaderOutM5
Variable Numeric Slot_CounterReaderOutM6
Variable Numeric Slot_CounterReaderOutM7
Variable Numeric Slot_CounterReaderOutM8
Variable Numeric Slot_CounterReaderResetM0
Variable Numeric Slot_CounterReaderResetM1
Variable Numeric Slot_CounterReaderResetM2
Variable Numeric Slot_CounterReaderResetM3
Variable Numeric Slot_CounterReaderResetM4
Variable Numeric Slot_CounterReaderResetM5
Variable Numeric Slot_CounterReaderResetM6
Variable Numeric Slot_CounterReaderResetM7
Variable Numeric Slot_CounterReaderResetM8
Variable Numeric Slot_EnableMotors
Variable Numeric Slot_Emergency
Variable Numeric Slot_Dispenser
' --------------------------------------------- LEDS
Variable Numeric Slot_WhiteFront
Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB
' --------------------------------------------- MicroVideo
Variable Numeric Slot_StartStopVideo
Variable Numeric Slot_SaveVideo
Variable Numeric Slot_TakeImage

Variable Numeric DispenserTime

Variable String FileName

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

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

Label EventStop
	Gosub SavePositions
	Gosub Disable Motors
	Gosub EnableAllButtons
End

' ====================================================================
'  SAVE and RESTORE POSITIONS
' ====================================================================
Label SavePositions
	v1 = Slot(Slot_M0)
	v2 = Slot(Slot_M1)
	v3 = Slot(Slot_M2)
	v4 = Slot(Slot_X)
	v5 = Slot(Slot_Y)
	v6 = Slot(Slot_Z)
	Save Vars
Return

Label RestorePositions
	Load Vars
	If v1 <> 0 And v2 <> 0 And v3 <> 0
		Slot(Slot_M0) = Reset
		Wait Seconds 0.01
		Slot(Slot_M0) = v1
		Wait Seconds 0.01
		
		Slot(Slot_M1) = Reset
		Wait Seconds 0.01
		Slot(Slot_M1) = v2
		Wait Seconds 0.01
		
		Slot(Slot_M2) = Reset
		Wait Seconds 0.01
		Slot(Slot_M2) = v3
		Wait Seconds 0.01
		
		Slot(Slot_X) = v4
		Slot(Slot_Y) = v5
		Slot(Slot_Z) = v6
	EndIf
	'
	Gosub PrintValues
Return


' ====================================================================
'  FUNCTIONS
' ====================================================================
' --------------------------------------------------------
' We work with 800 steps per turn
' one step = 0.00125 of turn
' --------------------------------------------------------

Variable Numeric Tolerance

Variable Numeric SlotCounterReaderOut

Label ErrorRecovery
	s2 = Slot(SlotMotor) - Slot(SlotCounterReaderOut)
	Print "ERROR:" + s2
	
	v4 = Slot(SlotMotor)
	Gosub Disable Motors
	Slot(SlotMotor) = Slot(SlotCounterReaderOut)
	Gosub WaitMotor
	Gosub Enable Motors
	Slot(SlotMotor) = v4
	Gosub WaitMotor
Return

Label Enable Motors
	Slot Slot_EnableMotors = 1000 		'Enable Motors
Return

Label Disable Motors
	Slot Slot_EnableMotors = 0 	        'Disable Motors
Return

Label TestEncoder
	'For v9 = 1 To 10
	'	MoveSlowXY_DestX = Slot(Slot_X) + 200
	'	MoveSlowXY_DestY = Slot(Slot_Y) - 100
	'	MoveSlowXY_Speed = 60
	'	Gosub MoveSlowXY
	'	Wait Seconds 1
	'	MoveSlowXY_DestX = Slot(Slot_X) - 200
	'	MoveSlowXY_DestY = Slot(Slot_Y) + 100
	'	MoveSlowXY_Speed = 60
	'	Gosub MoveSlowXY
	'	Wait Seconds 1
	'Next
	
	'MoveSlowXY_DestX = Slot(Slot_X) + 500
	'MoveSlowXY_DestY = Slot(Slot_Y)
	'MoveSlowXY_Speed = 60
	'Gosub MoveSlowXY
	
	'' ---------------------------------- NO work correctly
	'If Slot(1) < 200
	'	MoveSlowXY_DestX = 59.900
	'Else
	'	MoveSlowXY_DestX = -248.840
	'EndIf
	'MoveSlowXY_DestY = -150
	'MoveSlowXY_Speed = 1000
	'Gosub MoveSlowXY
	
	' ---------------------------------- NO work correctly
	'If Slot(1) < 200
	'	Slot(Slot_X) = 59.868
	'Else
	'	Slot(Slot_X) = -248.842
	'EndIf
	'Slot(Slot_Y) = -150
	'Gosub waitallmotors
	
	' ---------------------------------- OK
	If Slot(1) < 200
		Slot(Slot_M0) = 270
		Slot(Slot_M1) = 60
	Else
		Slot(Slot_M0) = 150
		Slot(Slot_M1) = 125
	EndIf
	Gosub WaitAllMotors
	
	Gosub PrintValues
Return

Label StartVideo
	Slot(Slot_StartStopVideo) = 1000
Return

Label StopVideo
	Slot(Slot_StartStopVideo) = 0
Return

Label SaveVideo
	Slot(Slot_SaveVideo) = 1000
	Wait Seconds 0.5
	Slot(Slot_SaveVideo) = 0
Return

Label TakeImage
	Wait Seconds 1.5
	Slot(Slot_TakeImage) = 1000
	Wait Seconds 0.2
	Slot(Slot_TakeImage) = 0
Return

Label Dispenser
	Gosub BluON
	Slot Slot_Dispenser = 1000
	Wait Seconds DispenserTime
	Slot Slot_Dispenser = 0
	Gosub GreenOn
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

' ====================================================================
'  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 v1 = 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 v1 = 1 To 50		'9e99
		SequenceFileName = "SEQUENCES\StressTest.seq"
		Gosub ExecSequence
	Next
	
	Gosub PARK
	Button 6 Color Yellow
Return

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

Label Square Test
	Button 8 Color Flashing8 Black White
	Gosub Enable Motors
	Gosub PARK
	For v1 = 1 To 2			'9e99
		SequenceFileName = "SEQUENCES\SquareTest.seq"
		Gosub ExecSequence
	Next
	Gosub PARK
	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
	Gosub PARK
	Button 9 Color Yellow
Return

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

Label PARK
	Slot(Slot_Z) = 150
	MoveSlowXY_DestX = -50	 	' destination in mm
	MoveSlowXY_DestY = -190 	' destination in mm
	MoveSlowXY_Speed = 80
	Gosub MoveSlowXY
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"
				Exit
		    ' ---------------------------------------------- ACTIONS
			Case "ENABLEMOTORS"
				Gosub Enable Motors
			Case "DISABLEMOTORS"
				Gosub Disable Motors
			Case "WAITALLMOTORS"
				Gosub WaitAllMotors
			Case "CLOSE ALL"
				Gosub CLOSE ALL
			' ---------------------------------------------- HOMING
			Case "HOME"
				Gosub Home
			Case "HOMING_M0"
				Gosub Homing_M0
			Case "HOMING_M1"
				Gosub Homing_M1
			Case "HOMING_M2"
				Gosub Homing_M2 Z
			'Case "HOMING_M3(R)"
			'	Gosub Homing_M3(R)
			' ---------------------------------------------- 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
			' ---------------------------------------------- VIDEO
			Case "STARTVIDEO"
				Gosub StartVideo
			Case "STOPVIDEO"
				Gosub StopVideo
			Case "SAVEVIDEO"
				Gosub SaveVideo
			Case "TAKEIMAGE"
				Gosub TakeImage
			' ---------------------------------------------- WAIT MOTORS
				
            ' ---------------------------------------------- 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


' ====================================================================
'  HOME
' ====================================================================
Label Home
	Gosub Enable Motors
	Gosub YellowOn
	Gosub Homing_M2 Z
	Gosub Homing_M1
	Gosub Homing_M0
	Slot Slot_X = -175			' angles M0=180 and M1=90
	Slot Slot_Y = -150			' both values
	Slot Slot_Z = 190
	Gosub PrintValues
Return

' ====================================================================
'  HOMING M0 M1 M2
' ====================================================================
Label Homing_M0
	Gosub Enable Motors
	SlotMotor = Slot_M0
	SlotSensor = Slot_SensorM0
	SlotCounterReaderReset = Slot_CounterReaderResetM0
	SpeedSearch = -1     		' search direction and speed (1 = max speed)
	SpeedReturn = +0.01	    	' return direction and speed
	DeclaredSwitchPos = 81.9	' declared switch angle
	InitialPosition = 180		' move to this angle
	Gosub HomeMotor
	Gosub PrintValues
Return

Label Homing_M1
	Gosub Enable Motors
	SlotMotor = Slot_M1
	SlotSensor = Slot_SensorM1
	SlotCounterReaderReset = Slot_CounterReaderResetM1
	SpeedSearch = -1			' search direction and speed (1 = max speed)
	SpeedReturn = +0.01  		' return direction and speed
	DeclaredSwitchPos = 32.1	' declared switch angle
	InitialPosition =  90		' move to this angle
	Gosub HomeMotor
	Gosub PrintValues
Return

Label Homing_M2 Z
	Gosub Enable Motors
	SlotMotor = Slot_M2
	SlotSensor = Slot_SensorM2
	SlotCounterReaderReset = Slot_CounterReaderResetM2
	SpeedSearch = +1      	  	' search direction and speed (1 = max speed)
	SpeedReturn = -0.02			' return direction and speed
	DeclaredSwitchPos = 192		' declared switch angle
	InitialPosition = 190		' move to this angle
	Gosub HomeMotor
	Gosub PrintValues
	Slot Slot_Z = 190
	Gosub PrintValues
Return


' ====================================================================
'  Home the Motor(SlotMotor) to the Sensor(SlotSensor)
' ====================================================================
Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SlotCounterReaderReset
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) = DeclaredSwitchPos
		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
    ' ------------------------- move to InitialPos and reset counter reader
Label SetFinalValues
	Slot(SlotMotor) = InitialPosition
	Gosub WaitMotor
	Slot(SlotCounterReaderReset) = Reset
	Wait Seconds 0.1
	Slot(SlotCounterReaderReset) = InitialPosition
	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

'Label WaitMotorAndRecovery
'	Wait Seconds 0.1
'	For WaitMot1 = 1 To 9e99
'		If Abs(Slot(SlotMotor + 1)) < 0.01
'			Exit
'		EndIf
'		Wait Seconds 0.005
'	Next
'	Wait Seconds 0.2
'	If Abs(Slot(SlotMotor) - Slot(SlotCounterReaderOut)) >= Tolerance
'		Gosub ErrorRecovery
'	EndIf
'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

Label WaitAllMotorsAndRecovery
	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
	
	For MotorNumber = 0 To NumMotors - 1
		Gosub MotorNumberToSlot
		Gosub MotorNumberToSlotCounterReaderOut
		If Abs(Slot(MotorSlotFromNumber) - Slot(SlotCounterReaderOut)) >= Tolerance
			Gosub ErrorRecovery
		EndIf
	Next
Return

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

Label MotorNumberToSlot
	Select MotorNumber
		Case 0
			MotorSlotFromNumber = Slot_M0
		Case 1
			MotorSlotFromNumber = Slot_M1
		Case 2
			MotorSlotFromNumber = Slot_M2
		Case 3
			MotorSlotFromNumber = Slot_M3
		Case 4
			MotorSlotFromNumber = Slot_M4
		Case 5
			MotorSlotFromNumber = Slot_M5
		Case 6
			MotorSlotFromNumber = Slot_M6
		Case 7
			MotorSlotFromNumber = Slot_M7
		Case 8
			MotorSlotFromNumber = Slot_M8
	EndSelect
Return

' ====================================================================
'  MOTOR NUMBER TO Counter Reader Slots CONVERSION
' ====================================================================
Label MotorNumberToSlotCounterReaderOut
	Select MotorNumber
		Case 0
			SlotCounterReaderOut = Slot_CounterReaderOutM0
		Case 1
			SlotCounterReaderOut = Slot_CounterReaderOutM1
		Case 2
			SlotCounterReaderOut = Slot_CounterReaderOutM2
		Case 3
			SlotCounterReaderOut = Slot_CounterReaderOutM3
		Case 4
			SlotCounterReaderOut = Slot_CounterReaderOutM4
		Case 5
			SlotCounterReaderOut = Slot_CounterReaderOutM5
		Case 6
			SlotCounterReaderOut = Slot_CounterReaderOutM6
		Case 7
			SlotCounterReaderOut = Slot_CounterReaderOutM7
		Case 8
			SlotCounterReaderOut = Slot_CounterReaderOutM8
	EndSelect
Return

Label MotorNumberToSlotCounterReaderReset
	Select MotorNumber
		Case 0
			SlotCounterReaderReset = Slot_CounterReaderResetM0
		Case 1
			SlotCounterReaderReset = Slot_CounterReaderResetM1
		Case 2
			SlotCounterReaderReset = Slot_CounterReaderResetM2
		Case 3
			SlotCounterReaderReset = Slot_CounterReaderResetM3
		Case 4
			SlotCounterReaderReset = Slot_CounterReaderResetM4
		Case 5
			SlotCounterReaderReset = Slot_CounterReaderResetM5
		Case 6
			SlotCounterReaderReset = Slot_CounterReaderResetM6
		Case 7
			SlotCounterReaderReset = Slot_CounterReaderResetM7
		Case 8
			SlotCounterReaderReset = Slot_CounterReaderResetM8
	EndSelect
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 out of limit
	Slot(Slot_X) = Limit(Slot(Slot_X), Xmin, Xmax)
	Slot(Slot_Y) = Limit(Slot(Slot_Y), Ymin, Ymax)
	' ----------------------------------- 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 out of limit
	Slot(Slot_Z) = Limit(Slot(Slot_Z), Zmin, Zmax)
	' ----------------------------------- 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
	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
	'
	Gosub PrintValues
Return


' ====================================================================
'  JoyPad
' ====================================================================
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 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_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 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 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 + "   "
	s1 = s1 + " M0err=" + Format(Slot(Slot_CounterReaderOutM0) - Slot(Slot_M0), "0.000")
	s1 = s1 + " M1err=" + Format(Slot(Slot_CounterReaderOutM1) - Slot(Slot_M1), "0.000")
	s1 = s1 + " M2err=" + Format(Slot(Slot_CounterReaderOutM2) - Slot(Slot_M2), "0.000")
	Print CRLF + s1
Return


'WATCH SequenceCmd
'WATCH Slot_Dispenser
