' ====================================================================
'     Name:	WXM_CNC_Vslot_longrail_07.txt            Date: 17 Jun 2021
'  Authors: Leonardo DE PALO Ph.D. - Roberto Fasciani MSC.Eng.
'    Scope:
' Hardware: Stepper Driver: SL2490A          3A  1600PPR
'   1   2   3   4   5   6   7   8   9   10
'      OFF OFF     OFF OFF         OFF OFF
'  ON          ON          ON  ON
' ====================================================================
Option Speed 9
Option GlobalKeys Enabled

' -------------------------------------- Slots
Slot_MotorX        =  1
Slot_EnableMotors  = 10
Slot_PEN           =  3
Slot_Emergency     = 11

Slot_SensorX_Left  =  5
Slot_SensorX_Right =  6

Slot_LedR          =  7
Slot_LedG          =  8
Slot_LedB          =  9

' -------------------------------------- X limits
Xmax               = 1790
Xmin               = 3

' -------------------------------------- X values
XLeft               = 2
XMiddle             = 900
XRight              = 1790

' -------------------------------------- Pen Up / Down set up
PenDown_Value      = 907
PenUp_Value        = 770

' -------------------------------------- Key Jog Speed
KeyJog_Speed       = 100

' -------------------------------------- BUTTONS
Button 1 Text HomeLeft
Button HomeLeft Text "HOME LEFT"
Button HomeLeft Color Thistle

Button 2 Text HomeRight
Button HomeRight Text "HOME RIGHT"
Button HomeRight Color Thistle

Button 3 Text Left
Button 4 Text Middle
Button 5 Text Right

Button 7 Text RunSequence
Button RunSequence Text "Run Sequence"
Button RunSequence Color Yellow

Button 8 Text EditSequence
Button EditSequence Text "Edit Sequence"
Button EditSequence Color PaleGreen

Button 9 Text SelectSequence
Button SelectSequence Text "Select Sequence"
Button SelectSequence Color Yellow

Button 11 Text EnableMotors
Button EnableMotors Color Tan

Button 12 Text DisableMotors
Button DisableMotors Color Tan

Button 14 Text  CLOSEALL
Button CLOSEALL Color Red

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

Button 17 Text RedOn
Button RedOn Color Red

Button 18 Text GreenOn
Button GreenOn Color Green

Button 19 Text YellowOn
Button YellowOn Color Yellow

Button 20 Text BlueOn
Button BlueOn Color MediumBlue

Button 21 Text WhiteOn
Button GreenOn Color Green

Button 22 Text RgbOff
Button RgbOff Color Black

Button 25 Text ResetLogCounter
Button ResetLogCounter Text "Reset Log counter"
Button ResetLogCounter Color White

Button 26 Text  OpenLogFile
Button OpenLogFile Text "Open Log file"
Button OpenLogFile Color PaleGreen

Button 27 Text  OpenLogFolder
Button OpenLogFolder Text "Open Log folder"
Button OpenLogFolder Color Yellow

Button 29 Text PenUp
Button PenUp Text "PEN UP"
Button PenUp Color LightSteelBlue

Button 30 Text PenDown
Button PenDown Text "PEN DOWN"
Button PenDown Color LightSteelBlue

Button 31 Text PenSingle
Button PenSingle Text "PEN SINGLE"
Button PenSingle Color LightSteelBlue

Gosub DisableAllButtons

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

' ----------------------------------------- KEYS
Key Left  Gosub ExecKeys
Key Right Gosub ExecKeys

' -------------------------------------- LOG FILE SET UP
Variable String FileHeader
Variable String LogName

' -------------------------------------- LOAD EXTERNAL PROGRAMS
Load "Theremino_HAL\Theremino_HAL.exe"
Load "Theremino_HAL\Theremino_SlotViewer.exe"

Load "Media\Images\LongRail V-slot.jpg"

' -------------------------------------- TEST MASTER PRESENT
'For v1 = 1 To 9e99
'	Window SetFocus
'	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 CRLF + " Invalid master number: " + Slot(0) + CRLF
'	Slot 0 = Recognize
'	Beep
'Next
'Print ""

' -------------------------------------- Ensure motor stopped and enable
Gosub WaitAllMotors
Gosub EnableMotors

If Slot(Slot_Emergency) > 500
	Goto EMERGENCY
EndIf

' ====================================================================
'  STARTING ACTIONS
' ====================================================================

Gosub RedOn

'Gosub HomeLeft

'Gosub Middle

Gosub EnableAllButtons
Gosub GreenOn
Option GlobalKeys Disabled
Load Hide

Stop																'

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_MotorX
Variable Numeric Slot_SensorX_Left
Variable Numeric Slot_SensorX_Right

Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB

Variable Numeric Slot_PEN
Variable Numeric PenDown_Value
Variable Numeric PenUp_Value

Variable Numeric Slot_EnableMotors
Variable Numeric Slot_Emergency


' ====================================================================
'  EMERGENCY
' ====================================================================
Label EMERGENCY
	'Gosub DisableAllButtons
	'Button EMERGENCY Enabled
	'Button EMERGENCY Color Flashing7 Yellow Red
	'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


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

Label DisableMotors
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 0
Return

' =====================================================================
'  CLOSING EVENTS
' =====================================================================
Label CLOSEALL
	Gosub SaveVars
	Gosub DisableMotors
	Gosub RedOn
	Load CloseApps
Return

Label EventStop
	Gosub SaveVars
	'Gosub HomeDown
	'Wait Seconds 0.2
	'Gosub Middle
	'Wait Seconds 0.2
	'Gosub EnableMotors
	Gosub RedOn
	Gosub EnableAllButtons
	Gosub DisableMotors
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

' ====================================================================
'  HOME Z
' ====================================================================
Label HomeLeft
	Gosub DisableAllButtons
	Gosub HomeLeft_Core
	Gosub EnableAllButtons
Return

Label HomeRight
	Gosub DisableAllButtons
	Gosub HomeRight_Core
	Gosub EnableAllButtons
Return

Label HomeDown
	Gosub DisableAllButtons
	Gosub HomeLeft_Core
	Gosub EnableAllButtons
Return

Label HomeLeft_Core
	Gosub EnableMotors
	SlotMotor = Slot_MotorX
	SlotSensor = Slot_SensorX_Left
	SpeedSearch = -0.25			'0.5
	SpeedReturn = +0.02
	DeclaredSwitchPos = 0
	InitialPosition = 2
	Gosub HomeMotor
	Gosub PrintValues
Return

Label HomeRight_Core
	Gosub EnableMotors
	SlotMotor = Slot_MotorX
	SlotSensor = Slot_SensorX_Right
	SpeedSearch = +0.25           '0.5
	SpeedReturn = -0.02
	DeclaredSwitchPos = 1800
	InitialPosition = 1798
	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 EnableMotors
	Button EMERGENCY 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 WaitAllMotors
Return

' ====================================================================
'  FUNCTIONS
' ====================================================================
Variable Numeric XLeft
Variable Numeric XMiddle
Variable Numeric XRight

Label Left
	Gosub EnableMotors
	Slot(Slot_MotorX) = XLeft
	Gosub WaitAllMotors
	'Gosub DisableMotors
Return

Label Middle
	Gosub EnableMotors
	Slot(Slot_MotorX) = XMiddle
	Gosub WaitAllMotors
	'Gosub DisableMotors
Return

Label Right
	Gosub EnableMotors
	Slot(Slot_MotorX) = XRight
	Gosub WaitAllMotors
	'Gosub DisableMotors
Return

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

Label PenDown
	Slot(Slot_PEN) = PenDown_Value
	Wait Seconds 0.1
Return

Label PenSingle
	Gosub WhiteOn
	Gosub PenDown
	'Load sound\Click3.wav
	'Load "media\sound\laser.wav"
	Wait Seconds 0.5
	Gosub PenUp
	Gosub GreenOn
Return

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

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

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

Label YellowOn
	Slot(Slot_LedR) = 400
	Slot(Slot_LedG) = 600
	Slot(Slot_LedB) = 1000
Return

Label WhiteOn
	Slot(Slot_LedR) = 0
	Slot(Slot_LedG) = 0
	Slot(Slot_LedB) = 0
Return

Label RgbOff
	Slot(Slot_LedR) = 1000
	Slot(Slot_LedG) = 1000
	Slot(Slot_LedB) = 1000
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
	Gosub DisableAllButtons
	Button EditSequence Enabled
	Button OpenLogFile Enabled
	Button OpenLogFolder Enabled
	Button CLOSEALL Enabled
	Gosub InitSequencePaths
	Gosub InitFileHeader
	Button RunSequence Color Flashing8 Yellow Orange
	Gosub EnableMotors
	SequenceToExecute = SequencePathAndName
	For v1 = 1 To 9e99
		Gosub ExecSequence
		Gosub UpdateLog
	Next
	Button RunSequence Color Yellow
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
		LogName = "Files\Logs\" + Replace(SequenceName, ".seq", ".txt")
		Gosub ShowSequenceName
		Gosub EditSequence
	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

Label ExecSequence
	Load Sequence SequenceToExecute
	If Len(Sequence) = 0
		Controls OpenTextBox
		Print CRLF + " Sequence file not found: " + SequenceToExecute
		Beep
		Stop														'
	EndIf
		'
	Print "Sequence " + GetFileNameWithoutPath(SequenceToExecute) + " started at " + Format(Now, "HH:mm:ss") + CRLF
	Print Chars("-", 50) + 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
		'
		SequenceCmd = GetSeparatedString(Sequence, SequenceIndex, CRLF)
		SequenceCmd = Replace(SequenceCmd, Chr(9), " ") 'TABS to SPACES
		'
		Print " Exec: " + SequenceCmd + CRLF
		'
		SequenceCmd = RemoveComments(SequenceCmd)
		SequenceCmd = Trim(SequenceCmd)
		'
		If SequenceCmd <> ""
			'
			Select UCase(SequenceCmd)
            	' ---------------------------------------------- End sequence
				Case "ENDSEQ"
					Exit
		    	' ---------------------------------------------- ACTIONS
				Case "ENABLEMOTORS"
					Gosub EnableMotors
				Case "DISABLEMOTORS"
					Gosub DisableMotors
				Case "WAITALLMOTORS"
					Gosub WaitAllMotors
				Case "CLOSE ALL"
					Gosub CLOSEALL
				' ---------------------------------------------- LED COLORS
				Case "REDON"
					Gosub RedOn
				Case "GREENON"
					Gosub GreenOn
				Case "BLUEON"
					Gosub BlueOn
				Case "YELLOWON"
					Gosub YellowOn
				Case "WHITEON"
					Gosub WhiteOn
				Case "RGBOFF"
					Gosub RgbOff
				' ---------------------------------------------- HOME LEFT & RIGHT
				Case "HomeLeft"
					Gosub HomeLeft_Core
				Case "HomeRight"
					Gosub HomeRight_Core
				' ------------------------------------------ Pen actions
				Case "PenUp"            ' move pen up
					Gosub PenUp
				Case "PenDown"          ' move pen down
					Gosub PenDown
				Case "PenSingle"        ' single click
					Gosub PenSingle
				' ---------------------------------------------- VIDEO
				'Case "STARTVIDEO"
				'	Gosub StartVideo
				'Case "STOPVIDEO"
				'	Gosub StopVideo
				'Case "SAVEVIDEO"
				'	Gosub SaveVideo
				'Case "TAKEIMAGE"
				'	Gosub TakeImage
            	' ---------------------------------------------- With parameters
				CaseElse
					s1 = GetSeparatedString(SequenceCmd, 0)
					Select UCase(s1)
						Case "MoveMotor"
							Gosub EnableMotors
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_MotorX) = v1
							Gosub WaitAllMotors
						' ---------------------------------------------- Handler positions
						Case "Left"
							Gosub EnableMotors
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_MotorX) = v1
							Gosub WaitAllMotors
						Case "Middle"
							Gosub EnableMotors
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_MotorX) = v1
							Gosub WaitAllMotors
						Case "Right"
							Gosub EnableMotors
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_MotorX) = v1
							Gosub WaitAllMotors
						Case "Pause"
							Wait Seconds GetSeparatedString(SequenceCmd, 1)
						CaseElse
							If s1 <> ""
								Controls OpenTextBox
								Print CRLF + " Unrecognized command: " + SequenceCmd
								Beep
								Stop								'
							EndIf
					EndSelect
			EndSelect
		EndIf
	Next
	Print Chars("-", 50) + CRLF
	Print "Sequence " + GetFileNameWithoutPath(SequenceToExecute) + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds" + CRLF
Return

' ====================================================================
'   PRINT LOG
'=====================================================================
Variable Numeric NumCycle
Variable Numeric ErrorNumber

Label OpenLogFile
	Load LogName
Return

Label OpenLogFolder
	Load "Files\Logs\"
Return

Label InitFileHeader
	FileHeader = CRLF
	FileHeader = FileHeader + "    TEST ROBOT W1     Num   Durat." + CRLF
	FileHeader = FileHeader + "     Date   Time     Cycle   sec. " + CRLF
	FileHeader = FileHeader + "-----------------------------------------"
	Save StringToFile FileHeader LogName
Return

Variable String LG1
Variable Numeric StartTime

Label UpdateLog
	NumCycle = NumCycle + 1
	Controls OpenTextBox
	LG1 = ""
	LG1 = LG1 + Format(Now, "yyyy/MM/dd HH:mm:ss")
	LG1 = LG1 + PadLeft(Format(NumCycle, "00000"), 7) + " "
	LG1 = LG1 + PadLeft(Format(ElapsedTime - StartTime, "0.00"), 6)
	Save StringToFile LG1 LogName
	Print CRLF + LG1 + CRLF + CRLF
	StartTime = ElapsedTime
Return

' ====================================================================
'   Wait until all motors are stopped
' ====================================================================
Label WaitAllMotors
	Wait Seconds 0.1
	Variable Numeric WM1
	For WM1 = 1 To 9e99
		If Abs(Slot(Slot_MotorX + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return

' ====================================================================
'  JOG with keyboard
' ====================================================================
Variable Numeric Xmin
Variable Numeric Xmax
Variable Numeric KeyJog_Speed

Label ExecKeys
	Gosub EnableMotors
	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("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
	
	Gosub PrintValues
Return

' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	Controls OpenTextBox
	s1 =       "DestZ=" + Format(Slot(Slot_MotorX), "0.000")
	Print CRLF + s1
Return


'EXEC Sin2 = Sin(33)
