'╔════════════════════════════════════════════════════════════════════╗
'║     Name: 311_Stitcher_0.992.txt                  Date: 13 Sep 2021║
'║   Author: Leonardo DE PALO Ph.D. & Roberto Fasciani Ms.C.          ║
'║    Scope: Early test of the stitcher using HAL or ArduHAL          ║
'║ Hardware: Master or xxxxx NANO                                     ║
'║     note: https://bulletpointsymbol.com/alt-codes.html             ║
'╚════════════════════════════════════════════════════════════════════╝
Variable Numeric DEBUG = 1  '1=Livio debug 0=Leo

' ====================================================================
'   SWITCH SETTING STEPPER DRIVER - SHENLI 2490 -
' ====================================================================
'   1    2    3    4          5    6    7    8    9    10
' --------------------------------------------------------------------
'  ON   OFF  OFF   ON        ON   OFF  ON   ON   OFF   OFF      800
'
' ====================================================================
'   SWITCH SETTING STEPPER DRIVER - LEADSHINE MA860 -
' ====================================================================
'   1    2    3    4    5    6    7    8
' --------------------------------------------------------------------
'                                            200    ' 10 Sept 2021
'                      ON   ON   ON   ON     400
'                     OFF   ON   ON   ON     800
'                      ON  OFF   ON   ON    1600
' --------------------------------------------------------------------
'                 ON                        Power saving
'                OFF                        No power saving    <<<<<
' --------------------------------------------------------------------
'  ON   ON   ON                             2.00 A
' OFF   ON   ON                             2.57 A
'  ON  OFF   ON                             3.14 A
' OFF  OFF   ON                             3.71 A
'  ON   ON  OFF                             4.28 A
' OFF   ON  OFF                             4.86 A
'  ON  OFF  OFF                             5.43 A
' OFF  OFF  OFF                             6.00 A        <<<<<
' =====================================================================

' =====================================================================
'  BROTHER 311 - HAL MAX SPEED X/Y/Z
' =====================================================================
'   Max speed     Max speed     Max acc    Step
'   mm/min        mm/sec        mm/s/s     per mm
' ---------------------------------------------------------------------
'  X  50000          833          9000        16
'  Y  50000          833          9000        16
'  Z   3000           50 (st/s)    100       400

' =====================================================================
'  SEWING MACHINE PARAMETERS
' =====================================================================
SewMaxPerSec = 10
' ---------------------------------------------------------------------
' This value (max stitches per second) depends from HAL Z-Speed
' Set SewMaxPerSec = HAL Z-Speed(mm/min) / 60
' To get the max precision reduce this value to 70% or 50%
' ---------------------------------------------------------------------

' -------------------------------------- FIRST INITIALIZATIONS
Option Speed 9
Option GlobalKeys Enabled
If DEBUG = 0
	VarsFromFile ".\Theremino_HAL\Theremino_Notepad\ConfigVars.txt"
EndIf

' -------------------------------------- LOAD EXTERNAL PROGRAMS
'Load Theremino_ArduHAL\Theremino_ArduHAL.exe
'Load Theremino_ArduHAL\Theremino_SlotViewer.exe
'Load Theremino_ArduHAL\Theremino_SignalScope.exe
'Load Theremino_ArduHAL\Theremino_DAA\DAA.exe

Load "Theremino_HAL\Theremino_HAL.exe"
Load "Theremino_HAL\Theremino_SlotViewer.exe"
Load "Theremino_HAL\Theremino_SignalScope.exe"
Load "Theremino_HAL\WXM_SewScope.exe"
'Load Theremino_MicroVideo\Theremino_MicroVideo.exe
'Load "Theremino_HAL\Theremino_WaveGenerator.exe"

' ====================================================================
'  INITIALIZATIONS
' ====================================================================
Variable Numeric Slot_MotorX		=   1
Variable Numeric Slot_MotorY		=   3
Variable Numeric Slot_MotorZ		=   5
Variable Numeric Slot_LedR			=   7
Variable Numeric Slot_LedG			=   8
Variable Numeric Slot_LedB			=   9
Variable Numeric Slot_EnableMotors	=  10
Variable Numeric Slot_Emergency     =  11
Variable Numeric Slot_WireCut       =  17
Variable Numeric Slot_Clamp         =  18
Variable Numeric Slot_SewScope      =  29

Variable Numeric Slot_SensorX		=  19
Variable Numeric Slot_SensorY		=  20
Variable Numeric Slot_SensorZ		=  21
Variable Numeric Slot_SignalScope   =  25

' -------------------------------------- Motors limits set in CONFIG VARS
Variable Numeric Xmax               = 162
Variable Numeric Xmin               =   1
Variable Numeric Ymax               =  64
Variable Numeric Ymin               =   1

' -------------------------------------- Sew machine inizializations
Variable Numeric MaxPerSec = 20

Variable Numeric StopAngle = 90
Variable Numeric StartAngle = 270 + 20

SewInit Slot_MotorX Slot_MotorY Slot_MotorZ StopAngle StartAngle

SewLimits Xmin Xmax Ymin Ymax MaxPerSec

' -------------------------------------- Key Jog Speed
Variable Numeric KeyJog_Speed

' -------------------------------------- Video & image
Variable Numeric Slot_StartVideo    =  26
Variable Numeric Slot_SaveVideo     =  27
Variable Numeric Slot_TakeImage     =  28

Gosub SetButtonsAndKeys
Gosub DisableAllButtons
Gosub RestoreVars
Gosub ShowSequenceName

' ====================================================================
'    DEBUG
' ====================================================================
If DEBUG = 1
	Gosub EnableAllButtons
	Gosub InitSequencePaths
	SequenceToExecute = SequencePathAndName
	'Gosub ExecSequence
	Stop
EndIf

Load "Brother_BAS_311E.jpg"

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

' ■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■
'  - S T A R T    - ACTIONS EXECUTION START HERE -
' ■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■
Gosub SignalScope_ON
Gosub Home_Y
Gosub Home_X
Gosub EnableMotors
Gosub Home_Z
Gosub SignalScope_OFF
Load Hide

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

Gosub UpdateButtons
Option GlobalKeys Disabled

Stop


' ====================================================================
'  BUTTONS
' ====================================================================
Label SetButtonsAndKeys
	Button 1 Text Home_All
	Button Home_All Text "Home ALL"
	Button Home_All Color LightBlue
	
	Button 2 Text Home_X
	Button Home_X Text "Home X"
	Button Home_X Color LightBlue
	
	Button 3 Text Home_Y
	Button Home_Y Text "Home Y"
	Button Home_Y Color LightBlue
	
	Button 4 Text Home_Z
	Button Home_Z Text "Home Z"
	Button Home_Z Color LightBlue
	
	Button 5 Text EditSequence
	Button EditSequence Text "Edit sequence"
	Button EditSequence Color PaleGreen
	
	Button 6 Text RunSequence
	Button RunSequence Text "RUN SEQUENCE"
	Button RunSequence Color Aqua
	
	Button 7 Text NeedleOneCycle
	Button NeedleOneCycle Text "Needle One Cycle"
	Button NeedleOneCycle Color Purple
	
	Button 8 Text SelectSequence
	Button SelectSequence Text "Select Sequence"
	Button SelectSequence Color PowderBlue
	
	Button 9 Text EnableMotors
	Button EnableMotors Color Tan
	
	Button 10 Text DisableMotors
	Button DisableMotors Color Tan
	
	Button 11 Text "Rotation"
	Button Rotation Color Yellow
	Button Rotation Text "Rotation"
	
	Button 12 Text "BtnCoordX"
	Button BtnCoordX Text "X = ?"
	Button BtnCoordX Color Yellow
	
	Button 13 Text "BtnCoordY"
	Button BtnCoordY Text "Y = ?"
	Button BtnCoordY Color Yellow
	
	Button 14 Text "BtnCoordZ"
	Button BtnCoordZ Text "Z = ?"
	Button BtnCoordZ Color Yellow
	
	Button 15 Text CloseAll
	Button CloseAll Text "CLOSE ALL"
	Button CloseAll Color Red
	
	Button 16 Text Emergency
	Button Emergency Text "EMERGENCY"
	Button Emergency Color Red
	Button Emergency Slot  Slot_Emergency
	
	Button 17 Text RedOn
	Button RedOn Color Red
	
	Button 18 Text MagentaOn
	Button MagentaOn Color Magenta
	
	Button 19 Text YellowOn
	Button YellowOn Color Yellow
	
	Button 20 Text GreenOn
	Button GreenOn Color Green
	
	Button 21 Text BlueOn
	Button BlueOn Color MediumBlue
	
	Button 22 Text WhiteOn
	Button GreenOn Color Green
	
	Button 23 Text RGBOff
	Button RGBOff Color Black
	
	Button 25 Text ClampOpen
	Button ClampOpen Text "CLAMP OPEN"
	Button ClampOpen Color NavajoWhite
	
	Button 26 Text ClampClose
	Button ClampClose Text "CLAMP CLOSE"
	Button ClampClose Color NavajoWhite
	
	Button 28 Text SPECIAL
	Button SPECIAL Text "special stress"
	Button SPECIAL Color Olive
	
	Button 29 Text TURNS
	Button TURNS Text "3600"
	Button TURNS Color LightBlue
	
	
	
	Button 30 Text EditConfigVars
	Button EditConfigVars Text "Edit Config Vars"
	Button EditConfigVars Color Coral
Return

Stop

'' -------------------------------------- Restore variables
'Gosub RestoreVars
'Gosub ShowSequenceName

' -------------------------------------- KEYS
Key Pageup   Gosub ExecKeys
Key Pagedown Gosub ExecKeys

' ====================================================================
'  EMERGENCY
' ====================================================================
Label Emergency
	Gosub DisableAllButtons
	Button CloseAll Enabled
	Button CloseAll Color Flashing7 Yellow Red
	For v1 = 1 To 9e99
		Gosub RedOn
		Wait Seconds 0.2
		Gosub RGBOff
		Wait Seconds 0.2
		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

' ====================================================================
' ENABLE/DISABLE MOTORS
' ====================================================================
Label EnableMotors
	Slot(Slot_EnableMotors) = 0
Return

Label DisableMotors
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 1000
Return

' ====================================================================
'  BUTTON "CLOSE ALL"
' ====================================================================
Label CloseAll
	Gosub SaveVars
	Window Sizable
	Load CloseApps
	Gosub SignalScope_OFF
	Gosub DisableMotors
End

'▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀
'  STOP EVENT (when pressing stop button)
'▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄▀▄
Label EventStop
	Gosub SaveVars
	Gosub EnableAllButtons
	Window Sizable
	Gosub DisableMotors
	Gosub SignalScope_OFF
End

' ====================================================================
'  SAVE LOAD and CLEAR VARS
' ====================================================================
Label SaveVars
'	v1 = NumCycle
'	v2 = ErrorNumber
	S1 = SequenceName
'	S2 = LogName
	Save Vars
Return

Label RestoreVars
	Load Vars
'	NumCycle = v1
'	ErrorNumber = v2
	SequenceName = S1
'	LogName = S2
Return

Label ClearVars
'	NumCycle = 0
'	ErrorNumber = 0
Return

Label ResetLogCounter
	Gosub ClearVars
Return

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

Label MagentaOn
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label YellowOn
	Slot Slot_LedR = 900
	Slot Slot_LedG = 400
	Slot Slot_LedB = 0
Return

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

Label BlueOn
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label WhiteOn
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 1000
Return

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

' =====================================================================
' CLAMP
' =====================================================================
Label ClampOpen
	Slot Slot_Clamp =  321
Return

Label ClampClose
	Slot Slot_Clamp = 700
Return


' =====================================================================
' WIRE CUT
' =====================================================================
Label WireCut
	Slot Slot_WireCut =  1000
	Wait Seconds 1
	Slot Slot_WireCut =  0
Return


' =====================================================================
' UNUSUAL ACTION FOR TESTS
' =====================================================================
Label NeedleOneCycle
	Gosub EnableMotors
	v5 = Slot(Slot_MotorZ)
	Slot(Slot_MotorZ) = v5 + 1
	Gosub DisableMotors
Return

' =====================================================================
' VIDEO and IMAGE
' =====================================================================
Label  StartVideo
	Slot(Slot_StartVideo) = 1000
Return

Label  StopVideo
	Slot(Slot_StartVideo) = 0
Return

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


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

'=====================================================================
'  SIGNAL SCOPE
' ====================================================================
Label SignalScope_ON
	Slot Slot_SignalScope = 1000
Return

Label SignalScope_OFF
	Slot Slot_SignalScope = 0
Return

'=====================================================================
'  SEWSCOPE
' ====================================================================
Label SewScopeStart
	Slot Slot_SewScope = 0
	Wait Seconds 0.2
	Slot Slot_SewScope = 1000
Return

'=====================================================================
'  MOTOR POSITION
' ====================================================================
Label TURNS
	Gosub EnableMotors
	For v1 = 0 To 5000
		Slot Slot_MotorZ = v1
		Wait Seconds 0.1
		Gosub UpdateButtons
	Next
	Gosub WaitAllMotors
	Gosub ResetMotorZ
	Gosub UpdateButtons
    'Gosub DisableMotors
Return

Label ResetMotorZ
	Slot(Slot_MotorZ) = Reset
	Wait Seconds 0.15
	Slot(Slot_MotorZ) = 0
	Wait Seconds 0.15
Return

Label SPECIAL
	Button SPECIAL Color Flashing8 Olive Yellow
	For v1 = 0 To 1000
		Gosub WireCut
		Wait Seconds 1
		Print v1 + CRLF
	Next
	Button SPECIAL Color Olive
Return
'' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	Gosub EnableMotors
	Variable Numeric JogSpeed					 'Breakpoint
	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("Left")
		Slot(Slot_MotorX) = Limit(Slot(Slot_MotorX) + JogSpeed, Xmin, Xmax)
	EndIf
	If Key("Right")
		Slot(Slot_MotorX) = Limit(Slot(Slot_MotorX) - JogSpeed, Xmin, Xmax)
	EndIf
	If Key("Up")
		Slot(Slot_MotorY) = Limit(Slot(Slot_MotorY) + JogSpeed, Ymin, Ymax)
	EndIf
	If Key("Down")
		Slot(Slot_MotorY) = Limit(Slot(Slot_MotorY) - JogSpeed, Ymin, Ymax)
	EndIf
	If Key("PageUp")
		Slot(Slot_MotorZ) = Slot(Slot_MotorZ) + JogSpeed
	EndIf
	If Key("PageDown")
		Slot(Slot_MotorZ) = Slot(Slot_MotorZ) - JogSpeed
	EndIf
	
	If Key("Space")
		Gosub DisableMotors
	EndIf
	Gosub PrintValues
	Gosub UpdateButtons
Return

' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	Variable String PV1
	Controls OpenTextBox
	PV1 = " DestX=" + Format(Slot(Slot_MotorX), "0.000")
	PV1 = PV1 + " DestY=" + Format(Slot(Slot_MotorY), "0.000")
	PV1 = PV1 + " DestZ=" + Format(Slot(Slot_MotorZ), "0.000")
	Print CRLF + PV1
Return

Label UpdateButtons
	Button BtnCoordX Text "X =" + Format(Slot(Slot_MotorX), "0.000")
	Button BtnCoordY Text "Y =" + Format(Slot(Slot_MotorY), "0.000")
	Button BtnCoordZ Text "Z =" + Format(Slot(Slot_MotorZ), "0.000")
Return

' ====================================================================
'  SEQUENCES
' =====================================================================
Label InitSequencePaths
	Variable String EditorFolder
	Variable String EditorApp
	Variable String SequencePathAndName
	Variable String SequenceName
	'
	EditorFolder = ".\Theremino_HAL\Theremino_Editor\"
	'
	EditorApp = EditorFolder + "Theremino_Editor.exe"
	'
	SequencePathAndName = EditorFolder + "Sequences\" + SequenceName
Return

Label RunSequence
	SewScopeStart
	Gosub DisableAllButtons
	Button EditSequence Enabled
	Button CloseAll Enabled
	Gosub InitSequencePaths
	Button RunSequence Color Flashing8 Yellow Orange
	Variable Numeric ExitFromSequenceLoop
	SequenceToExecute = SequencePathAndName
	Gosub SignalScope_ON
	Gosub Home_Z
	'For v1 = 1 To 1     '9e99
	For v1 = 1 To 9e99
		Gosub ExecSequence
		If ExitFromSequenceLoop > 0
			Exit
		EndIf
	Next
	Gosub SignalScope_OFF
	Button RunSequence Color Aqua
Return

Label EditSequence
	Gosub InitSequencePaths
	If FileExists(SequencePathAndName)
		Load EditorApp + " " + SequencePathAndName
	EndIf
Return

Label SelectSequence
	Gosub InitSequencePaths
	s1 = SelectFile(EditorFolder + "Sequences\")
	If s1 <> ""
		SequenceName = s1
		Gosub ShowSequenceName
	EndIf
Return

Label ShowSequenceName
	Button SelectSequence Text "Sequence" + CRLF + SequenceName
Return

' ████████████████████████████████████████████████████████████████████
'  SEQUENCE LOADING AND EXECUTION
' ████████████████████████████████████████████████████████████████████
Variable String  SequenceToExecute
Variable String  Sequence
Variable String  SequenceCmd
Variable Numeric SequenceIndex
Variable Numeric SequenceLength
Variable Numeric N1

Label ExecSequence
	Load Sequence SequenceToExecute
	If Len(Sequence) = 0
		Controls OpenTextBox
		Print CRLF + " Sequence file not found: " + SequenceToExecute
		Beep
		Stop
	EndIf
	'
	Print CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)
	Print " started at " + Format(Now, "HH:mm:ss") + CRLF + Chars("-", 65) + CRLF
	' ----------------------------------------------------------
	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
		' ------------------------------------------------------ Do not print lines containing ''
		If GetSeparatedStringCount(SequenceCmd, "''") < 2
			Print " Exec: " + SequenceCmd + CRLF
		EndIf
		' ------------------------------------------------------ Trim and remove comments
		SequenceCmd = Trim(RemoveComments(SequenceCmd))
		' ------------------------------------------------------
		If SequenceCmd <> ""
			Select GetSeparatedString(SequenceCmd, 0)
            	' ---------------------------------------------- End sequence
				Case "EndSeq"
					Exit
				' ---------------------------------------------- STOP
				Case "Stop"
					ExitFromSequenceLoop = 1
					Exit
		    	' ---------------------------------------------- ACTIONS
				Case "WaitAllMotors"
					Gosub WaitAllMotors
				Case "CLOSEALL"
					Gosub CloseAll
			   ' ---------------------------------------------- Video-Image
				Case "StartVideo"
					Gosub StartVideo
				Case "StopVideo"
					Gosub StopVideo
				Case "SaveVideo"
					Gosub SaveVideo
				Case "TakeImage"
					Gosub TakeImage
				' ---------------------------------------------- LED COLORS
				Case "RedOn"
					Gosub RedOn
				Case "MagentaON"
					Gosub MagentaOn
				Case "GreenOn"
					Gosub GreenOn
				Case "BlueOn"
					Gosub BlueOn
				Case "YellowOn"
					Gosub YellowOn
				Case "WhiteOn"
					Gosub WhiteOn
				Case "RGBOFF"
					Gosub RGBOff
				' ------------------------------------------- Motor fixed angles
				Case "ClampOpen"
					Gosub ClampOpen
				Case "ClampClose"
					Gosub ClampClose
				' ------------------------------------------- Motor fixed angles
				Case "Home_Z"
					Gosub Home_Z
	            ' -------------------------------------------- With parameters
				Case "WriteSlot"
					N1 =  GetSeparatedString(SequenceCmd, 1)
					Slot(N1) = GetSeparatedString(SequenceCmd, 2)
				Case "ResetStepper"
					N1 =  GetSeparatedString(SequenceCmd, 1)
					Slot(N1) = Reset
					Wait Seconds 0.05
					Slot(N1) = GetSeparatedString(SequenceCmd, 2)
					Wait Seconds 0.05
					Slot(N1) = GetSeparatedString(SequenceCmd, 3)
				Case "Move_X"
					Slot(Slot_MotorX) = GetSeparatedString(SequenceCmd, 1)
					Gosub WaitAllMotors
				Case "Move_Y"
					Slot(Slot_MotorY) = GetSeparatedString(SequenceCmd, 1)
					Gosub WaitAllMotors
				Case "Move_Z"
					Slot(Slot_MotorZ) = GetSeparatedString(SequenceCmd, 1)
					Gosub WaitAllMotors
				Case "MOVE_XY"
					Slot(Slot_MotorX) = GetSeparatedString(SequenceCmd, 1)
					Slot(Slot_MotorY) = GetSeparatedString(SequenceCmd, 2)
					Gosub WaitAllMotors
				Case "MOVE_XYZ"
					Slot(Slot_MotorX) = GetSeparatedString(SequenceCmd, 1)
					Slot(Slot_MotorY) = GetSeparatedString(SequenceCmd, 2)
					'Wait Seconds 0.005
					Slot(Slot_MotorZ) = GetSeparatedString(SequenceCmd, 3)
					Gosub WaitAllMotors
				    ' ---------------------------------------------- Stitching commands
				Case "SewTo"
					SewTo_DestX = GetSeparatedString(SequenceCmd, 1)
					SewTo_DestY = GetSeparatedString(SequenceCmd, 2)
					SewTo_Speed = GetSeparatedString(SequenceCmd, 3)
					SewTo_Pitch = GetSeparatedString(SequenceCmd, 4)
					Gosub SewTo
				Case "SewCircle"
					SewCircle_X  = GetSeparatedString(SequenceCmd, 1)
					SewCircle_Y  = GetSeparatedString(SequenceCmd, 2)
					SewCircle_Speed  = GetSeparatedString(SequenceCmd, 3)
					SewCircle_Pitch  = GetSeparatedString(SequenceCmd, 4)
					SewCircle_Radius = GetSeparatedString(SequenceCmd, 5)
					Gosub SewCircle
				Case "SewUp"
					Gosub SewUp
				Case "SewDown"
					Gosub SewDown
				Case "Pause"
					Wait Seconds GetSeparatedString(SequenceCmd, 1)
				CaseElse
					Controls OpenTextBox
					Print CRLF + " Unrecognized command: " + SequenceCmd + CRLF
					Beep
					Stop								'
			EndSelect
		EndIf
	Next
	Print Chars("-", 63) + CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)
	Print " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " sec" + CRLF
Return


' ====================================================================
'  HOME  X Y Z
' ====================================================================
Label Home_All
	Gosub EnableMotors
	Gosub Home_Z
	Gosub Home_X
	Gosub Home_Y
Return

Label Home_X
	Gosub EnableMotors
	SlotMotor = Slot_MotorX
	SlotSensor = Slot_SensorX
	SpeedSearch = -0.3
	SpeedReturn = +0.05
	DeclaredSwitchPos = 0
	InitialPosition = 1
	Gosub HomeMotor
	Gosub PrintValues
Return

Label Home_Y
	Gosub EnableMotors
	SlotMotor = Slot_MotorY
	SlotSensor = Slot_SensorY
	SpeedSearch = -0.1
	SpeedReturn = +0.05
	DeclaredSwitchPos = 0
	InitialPosition = 3
	Gosub HomeMotor
	Gosub PrintValues
Return

Label Home_Z
	Gosub EnableMotors
	SlotMotor = Slot_MotorZ
	SlotSensor = Slot_SensorZ
	SpeedSearch = +0.005
	SpeedReturn = -0.0001
	DeclaredSwitchPos = 0
	InitialPosition = 0
	Gosub HomeMotor
	Gosub PrintValues
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 CloseAll 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 RunSequence 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 motors are stopped
' ====================================================================
Label WaitAllMotors
	Wait Seconds 0.05
	Variable Numeric WM1
	For WM1 = 1 To 9e99
		If Abs(Slot(Slot_MotorX + 1)) < 0.01
			If Abs(Slot(Slot_MotorY + 1)) < 0.01
				If Abs(Slot(Slot_MotorZ + 1)) < 0.01
					Exit
				EndIf
			EndIf
		EndIf
		Wait Seconds 0.001
		Gosub UpdateButtons
	Next
	Gosub UpdateButtons
Return




' ====================================================================
'   CONFIGURATION FILE
' ====================================================================
Label EditConfigVars
	Variable String VarsFilePathAndName
	'
	EditorFolder = ".\Theremino_HAL\Theremino_Notepad\"
	'
	EditorApp = EditorFolder + "Theremino_Notepad.exe"
	'
	VarsFilePathAndName = EditorFolder + "ConfigVars.txt"
	'
	Load EditorApp + " " + VarsFilePathAndName
Return


' ====================================================================
'  SewTo X Y Speed Pitch
' ====================================================================
Variable Numeric SewMaxPerSec = 10

Variable Numeric SewTo_DestX 	' Destination X in mm
Variable Numeric SewTo_DestY 	' Destination Y in mm
Variable Numeric SewTo_Speed 	' Speed in mm/sec
Variable Numeric SewTo_Pitch 	' Pitch in mm

Label SewTo
	Gosub SewUp
	SewTo SewTo_DestX SewTo_DestY SewTo_Speed SewTo_Pitch ""
	Gosub WaitSewProgress
Return

' ====================================================================
'  SewCircle X Y Radius Speed Pitch
' ====================================================================
Variable Numeric SewCircle_X 	   ' Center X in mm
Variable Numeric SewCircle_Y 	   ' Center Y in mm
Variable Numeric SewCircle_Radius  ' Radius in mm
Variable Numeric SewCircle_Speed   ' Speed in mm/sec
Variable Numeric SewCircle_Pitch   ' Pitch in mm

Label SewCircle
	Variable String Radius = "Radius=" + SewCircle_Radius
	SewTo SewCircle_X SewCircle_Y SewCircle_Speed SewCircle_Pitch Radius
	Gosub WaitSewProgress
Return

' ====================================================================
'  Sew Functions
' ====================================================================
Variable Numeric NeedleIsUp = 1   'needle up

Label SewUp
	If NeedleIsUp = 0
		NeedleIsUp = 1
		Slot(Slot_MotorZ) = Slot(Slot_MotorZ) + 0.5
		Gosub WaitAllMotors
		Gosub ResetMotorZ
	EndIf
Return

Label SewDown
	If NeedleIsUp = 1
		NeedleIsUp = 0
		Gosub ResetMotorZ
		Slot(Slot_MotorZ) = 0.5
		Gosub WaitAllMotors
	EndIf
Return

Label WaitSewProgress
	Variable Numeric wsp1
	For wsp1 = 1 To Infinity
		If SewProgress < 0
			wsp1 = SewProgress
			Exit
		EndIf
		'Print Int(Abs(SewProgress) * 100) + "%"
		'Print Mod(Slot(Slot_MotorZ),1)
	Next
	Gosub WaitAllMotors
Return


'WATCH SewTo_Speed
