

ffffffffff


'---------------------------------------------------------------------
' File name:  WXM_600E_SAMSUNGXY_14.txt             Date: 01 July 2020
'---------------------------------------------------------------------
'---------------------------------------------------------------------
' Encoder 200 ppr
' HAL recognize 800 ppr
' Stepper 1.8 degree
' Step per full turn = 800 (jumper on B)
'----------------------------------------------------------------------

Load SAMSUNG_filter.jpg
Option Speed 9

NumMotors   = 3
Slot_M0     = 101
Slot_M1     = 103
Slot_M2     = 5

Slot_M0_DTD = 2
Slot_M1_DTD = 4
Slot_M2_DTD = 6

Slot_SensorM0 = 19
Slot_SensorM1 = 20
Slot_SensorM2 = 21

Slot_CounterReaderOutM0 = 25
Slot_CounterReaderResetM0 = 26

Slot_CounterReaderOutM1 = 27
Slot_CounterReaderResetM1 = 28

Slot_CounterReaderOutM2 = 29
Slot_CounterReaderResetM2 = 30

Slot_EnableMotors = 22

Slot_LedR = 16
Slot_LedG = 17
Slot_LedB = 18

Slot_GamepadX = 200
Slot_GamepadY = 201
Slot_GamepadStartStop = 210
Gamepad_Speed = 100
Gamepad_DeadBand = 30


' -------------------------------------- BUTTONS
'Button  1 Text "HOME"
'Button  1 Color SkyBlue
'Button  2 Text "Homing_M0"
'Button  2 Color SkyBlue
'Button  3 Text "Homing_M1"
'Button  3 Color SkyBlue
'Button  4 Text "Homing_M2 Z"
'Button  4 Color SkyBlue

Button  6 Text "SAMSUNG paste"
Button  6 Color Yellow4

Button  8 Text "Stress Test"
Button  8 Color Yellow
Button  9 Text "Cross Test"
Button  9 Color Yellow
Button 10 Text "Square Test"
Button 10 Color Yellow
Button 11 Text "Circle Test"
Button 11 Color Yellow
Button 12 Text "Triangle Test"
Button 12 Color Yellow

Button 13 Text "Enable Motors"
Button 13 Color Tan
Button 14 Text "Disable Motors"
Button 14 Color Tan

Button 15 Text  "GAMEPAD"
Button 15 Color Orange
Button 15 Slot  Slot_GamepadStartStop

Button 16 Text "CLOSE ALL"
Button 16 Color Red

' -------------------------------------- KEYS
Key Right		Gosub MoveRight
Key Left 		Gosub MoveLeft
Key Up 			Gosub MoveUp
Key Down 		Gosub MoveDown
Key Pageup		Gosub MoveUpper
Key Pagedown	Gosub MoveLower
Key M			Gosub "Enable Motors"

' -------------------------------------- LOG FILE
FileName = "Log_WXN600E.txt"
FileHeader = "- DATE - " + "  - TIME - " + "  CYCLE " + " Dest.=0 " + " Dest.=50 " + + " Pulse"
Save StringToFile FileHeader FileName

' -------------------------------------- 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_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 ""

' -------------------------------------- COLORS
Gosub "WHITE"
Wait Seconds 0.2
Gosub "RED"
Wait Seconds 0.2
Gosub "GREEN"
Wait Seconds 0.2
Gosub "BLU"
Wait Seconds 0.2
Load Hide

Slot 110 = 0 		'STOP recording
Gosub "Enable Motors"

Stop


' =====================================================================
'  CLOSING EVENTS
' =====================================================================
Label "CLOSE ALL"
	Load CloseApps
Return

Label "EventStop"
	Save Vars
	Gosub "Disable Motors"
	Window Sizable
End


' =====================================================================
'  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_M0_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M1_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M2_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M3_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M4_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M5_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M6_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M7_DTD ' Slot DistanceToDestination
Variable Numeric Slot_M8_DTD ' Slot DistanceToDestination

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_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB

Variable Numeric NumCycles
Variable Numeric ErrorNumber

Variable String FileName
Variable String FileHeader


' =====================================================================
'  FUNCTIONS
' =====================================================================
' --------------------------------------------------------
' We work with 800 steps per 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 "UPDATE FILE"
	v1 = Slot(Slot_M0)
	v2 = Slot(Slot_CounterReaderOutM0)
	v3 = v1 - v2
	S3 = Now
	s3 = S3 + "  " + Format(NumCycles, "00000")
	s3 = s3 + "   " + Format(v1, "00000")
	s3 = s3 + "   " + Format(v2, "00000")
	s3 = S3 + "   " + Format(v3, "00000")
	Save StringToFile s3 FileName
	'Print s3
Return

Label SaveVars
	v1 = NumCycles
	v2 = ErrorNumber
	Save Vars
Return

Label LoadVars
	Load Vars
	NumCycles = v1
	ErrorNumber = v2
Return

Label "Enable Motors"
	Slot Slot_EnableMotors = 1000 		'Enable Motors
Return

Label "Disable Motors"
	'Slot Slot_EnableMotors = 0 	        'Disable Motors
Return

' ===================================================================
'  HOME
' ===================================================================
Label "Home"
	Gosub "Enable Motors"
	Gosub "Homing_M2 Z"
	Gosub "Homing_M1"
	Gosub "Homing_M0"
Return

' ===================================================================
'  HOMING M0 M1 M2
' ===================================================================
Label "Homing_M0"
	Gosub "Enable Motors"
	SlotMotor = Slot_M0
	SlotSensor = Slot_SensorM0
	SlotCounterReaderReset = Slot_CounterReaderResetM0
	SpeedSearch = -0.1  	' search direction and speed
	SpeedReturn = +0.01	    ' return direction and speed
	DeclaredZero = 83.9	' declared switch angle
	InitialPosition = 180	' move to this angle
	'Gosub HomeMotor
	'Gosub PrintValues
	Button 2 Color SkyBlue
	Slot 101 = 180
	Slot 103 = 180
	Slot 105 = 190
Return

Label "Homing_M1"
	Gosub "Enable Motors"
	SlotMotor = Slot_M1
	SlotSensor = Slot_SensorM1
	SlotCounterReaderReset = Slot_CounterReaderResetM1
	SpeedSearch = -0.06		' 0.06 search direction and speed
	SpeedReturn = +0.002  	' return direction and speed
	DeclaredZero = 74.5		' 33.3 declared switch angle
	InitialPosition = 180	' move to this angle
	'Gosub HomeMotor
	'Gosub PrintValues
	Button 3 Color SkyBlue
	Slot 101 = 180
	Slot 103 = 180
	Slot 105 = 190
Return

Label "Homing_M2 Z"
	Gosub "Enable Motors"
	SlotMotor = Slot_M2
	SlotSensor = Slot_SensorM2
	SlotCounterReaderReset = Slot_CounterReaderResetM2
	SpeedSearch = +0.04		' search direction and speed
	SpeedReturn = -0.02		' return direction and speed
	DeclaredZero = 192		' declared switch angle
	InitialPosition = 190	' move to this angle
	'Gosub HomeMotor
	'Gosub PrintValues
	Button 4 Color SkyBlue
	Slot 101 = 180
	Slot 103 = 180
	Slot 105 = 190
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 DeclaredZero
Variable Numeric InitialPosition

Label HomeMotor
	' ------------------------- 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")
		Gosub WaitMotor
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' ------------------------- detach from the sensor
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
Goto DetachSensor
    ' ------------------------- reset and move to InitialPos
Label AssignZero
	Slot(SlotMotor) = Reset
	Slot(SlotCounterReaderReset) = Reset
	Wait Seconds 0.08
	Slot(SlotMotor) = DeclaredZero
	Slot(SlotCounterReaderReset) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Slot(SlotCounterReaderReset) = InitialPosition
	Gosub WaitMotor
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

'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_DTD
		If Abs(Slot(MotorSlotFromNumber_DTD)) > 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_DTD
		If Abs(Slot(MotorSlotFromNumber_DTD)) > 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

' ===================================================================
'  LED COLORS
' ===================================================================
Label "Red"
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

Label "Green"
	Slot Slot_LedR = 0
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 0
Return

Label "Blu"
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label "White"
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 1000
Return

Label "Yellow"
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 900
	Slot Slot_LedB = 0
Return

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

' ===================================================================
'  SEQUENCES
' ===================================================================
Label "SAMSUNG paste"
	SequenceFileName = "SEQUENCES\SAMSUNG_paste.seq"
	Gosub ExecSequence
Return

Label "Stress Test"
	Button 8 Color Flashing8 Black White
	Gosub "Enable Motors"
	'For v1 = 1 To 2		'9e99
	Slot 110 = 1000		'START recording
	SequenceFileName = "SEQUENCES\SquareTest.seq"
	Gosub ExecSequence
	SequenceFileName = "SEQUENCES\TriangleTest.seq"
	Gosub ExecSequence
	SequenceFileName = "SEQUENCES\CircleTest.seq"
	Gosub ExecSequence
	SequenceFileName = "SEQUENCES\CircleTest.seq"
	Gosub ExecSequence
	'Next
	Slot 110 = 0 		'STOP recording
	Gosub "PARK"
	Button 8 Color Yellow
Return

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

Label "Square Test"
	Button 10 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 10 Color Yellow
Return

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

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

Label "PARK"
	Slot(Slot_M2) = 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"     'enable movement motors
				Gosub "Enable Motors"
			Case "DISABLEMOTORS"    'disable movement motors
				Gosub "Disable Motors"
			'Case "STARTVACUUM"       'Start Vacuum
			'	Gosub "Start Vacuum"
			'Case "STOPVACUUM"        'Stop Vacuum
			'	Gosub "Stop Vacuum"
			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)"
			Case "CLOSEALL"
				Gosub "CLOSE ALL"
			' -------------------------------------------------- COLORS
			Case "RED"
				Gosub "RED"
			Case "GREEN"
				Gosub "GREEN"
			Case "BLU"
				Gosub "BLU"
			Case "WHITE"
				Gosub "WHITE"
			' -------------------------------------------------- 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 "RED"
						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

' =====================================================================
'  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 SLOT STD (Steps To Destination) CONVERSION
' =====================================================================
Variable Numeric MotorSlotFromNumber_DTD
Label MotorNumberToSlot_DTD
	Select MotorNumber
		Case 0
			MotorSlotFromNumber_DTD = Slot_M0_DTD
		Case 1
			MotorSlotFromNumber_DTD = Slot_M1_DTD
		Case 2
			MotorSlotFromNumber_DTD = Slot_M2_DTD
		Case 3
			MotorSlotFromNumber_DTD = Slot_M3_DTD
		Case 4
			MotorSlotFromNumber_DTD = Slot_M4_DTD
		Case 5
			MotorSlotFromNumber_DTD = Slot_M5_DTD
		Case 6
			MotorSlotFromNumber_DTD = Slot_M6_DTD
		Case 7
			MotorSlotFromNumber_DTD = Slot_M7_DTD
		Case 8
			MotorSlotFromNumber_DTD = Slot_M8_DTD
	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 MoveSlow_Slot 		' motor slot
'Variable Numeric MoveSlow_Dest 		' destination in mm or degree
'Variable Numeric MoveSlow_Speed 	' speed in mm/sec

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_M0)
	MSV2 = MoveSlowXY_DestY - Slot(Slot_M1)
	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_M0) = MoveSlowXY_DestX
		Slot(Slot_M1) = MoveSlowXY_DestY
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlowXY_DestX - Slot(Slot_M0)) / MSV4
	MSV2 = (MoveSlowXY_DestY - Slot(Slot_M1)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_M0) = Slot(Slot_M0) + MSV1
		Slot(Slot_M1) = Slot(Slot_M1) + MSV2
		Wait Seconds 0.01 '<<< loop time
	Next
	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_M2)
	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_M2) = MoveSlowZ_Dest
		Return
	EndIf
	' ----------------------------------- prepare the increment
	MSV1 = (MoveSlowZ_Dest - Slot(Slot_M2)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_M2) = Slot(Slot_M2) + MSV1
		Wait Seconds 0.01 '<<< loop time
	Next
	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_M0) = DrawCircle_X + DrawCircle_Radius * Sin(DCV2)
		Slot(Slot_M1) = DrawCircle_Y + DrawCircle_Radius * Cos(DCV2)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return


' ===================================================================
'  JOG
' ===================================================================
Label MoveRight
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M0 = Slot(Slot_M0) + JogSpeed
	Gosub PrintValues
Return

Label MoveLeft
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M0 = Slot(Slot_M0) - JogSpeed
	Gosub PrintValues
Return

Label MoveUp
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M1 = Slot(Slot_M1) + JogSpeed
	Gosub PrintValues
Return

Label MoveDown
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M1 = Slot(Slot_M1) - JogSpeed
	Gosub PrintValues
Return

Label MoveUpper
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M2 = Slot(Slot_M2) + JogSpeed
	Gosub PrintValues
Return

Label MoveLower
	Gosub "Enable Motors"
	Gosub SpeedModifiers
	Slot Slot_M2 = Slot(Slot_M2) - JogSpeed
	Gosub PrintValues
Return

Variable Numeric JogSpeed
Label SpeedModifiers
	JogSpeed = 10
	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

' ===================================================================
'  PRINT MOTOR POSITIONS
' ===================================================================

Label PrintValues
	s1 =        "X  = " + Format(Slot(Slot_M0), "0.00")
	s1 = s1 + "  Y  = " + Format(Slot(Slot_M1), "0.00")
	s1 = s1 + "  Z  = " + Format(Slot(Slot_M2), "0.00")
	s1 = s1 + "  M3 = " + Format(Slot(Slot_M3), "0.00")
	s1 = s1 + "  M4 = " + Format(Slot(Slot_M4), "0.00")
	s1 = s1 + "  M5 = " + Format(Slot(Slot_M5), "0.00")
	Print s1
Return


' ===================================================================
'  GAMEPAD
' ===================================================================
Variable Numeric Slot_GamepadX
Variable Numeric Slot_GamepadY
Variable Numeric Slot_GamepadZ
Variable Numeric Slot_GamepadStartStop
Variable Numeric Gamepad_Speed
Variable Numeric Gamepad_DeadBand

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
		Return
	Else
		Gamepad_Running = 1
	EndIf
	' --------------------------------------- Gamepad Initializations
	Button 15 Color Green9
	Button 15 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
		' ----------------------------------- 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 * 0.00001
			Slot Slot_M0 = Slot(Slot_M0) + Gamepad_V3 * Sign(Gamepad_V2)
		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 * 0.00001
			Slot Slot_M1 = Slot(Slot_M1) + Gamepad_V3 * Sign(Gamepad_V2)
		EndIf
		' ----------------------------------- Gamepad Axis Z
		Select Slot(Slot_GamepadZ)
			Case 360
				Slot Slot_M1 = Slot(Slot_M1) + Gamepad_V3 * Sign(Gamepad_V2)
				
			Case 180
				If v1 = v2
					
					
				EndIf
				
		EndSelect
		
	Next
	' --------------------------------------- Gamepad Finalizations
	Button 15 Text Gamepad
	Button 15 Color Orange
Return


'WATCH SequenceCmd
'WATCH s1
'WATCH UCase(SequenceCmd)
