' ===================================================================
'     Name:	WXM_DoorLock_V-Slot_1_34.txt            Date: 02 Ago 2021
'  Authors: Leonardo de Palo Ph.D. - Roberto Fasciani MSC.Eng.
'    Scope:
' Hardware: Stepper Driver: TB6600
'   1   2   3   4
'  ON  OFF ON  ON    1600 Step per revolution    step per mm = 26.72
'
' ====================================================================
Option Speed 9
Option GlobalKeys Enabled
Load "Media\Images\DooLock_Vslot_One.jpg"
VarsFromFile ".\Theremino_HAL\Theremino_Notepad\ConfigVars.txt"

' -------------------------------------- Slots
Variable Numeric Slot_MotorZ        =   1
Variable Numeric Slot_PEN           =   3
Variable Numeric Slot_Emergency     =  11

Variable Numeric Slot_PistonSensor  =   5
Variable Numeric Slot_SensorZ_DOWN  =   6

Variable Numeric Slot_LedR          =   7
Variable Numeric Slot_LedG          =   8
Variable Numeric Slot_LedB          =   9

'-------------------------------------- Z zeroing
Variable Numeric Zaxis_SwitchPos
Variable Numeric Zaxis_InitialPos

' -------------------------------------- Z limits
Variable Numeric Zmax
Variable Numeric Zmin

' -------------------------------------- Z values
Variable Numeric ZUp
Variable Numeric ZMiddle
Variable Numeric ZDown

' -------------------------------------- Pen Up / Down set up
Variable Numeric PenUp_Value
Variable Numeric PenDown_Value

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

Button 1 Text HomeDown
Button HomeDown Text "HOME DOWN"
Button HomeDown Color Thistle

Button 2 Text Up
Button 3 Text Middle
Button 4 Text Down

Button 6 Text EditSequence
Button EditSequence Text "Edit sequence"
Button EditSequence Color  Coral

Button 7 Text RunSequence
Button RunSequence Text "RUN SEQUENCE"
Button RunSequence Color Aqua

Button 8 Text SelectSequence
Button SelectSequence Text "Select Sequence"
Button SelectSequence Color PowderBlue

Button 11 Text "BtnDestZ"
Button BtnDestZ Text "Z = ?"
Button BtnDestZ Color Yellow

Button 14 Text CloseAll
Button CloseAll Text "CLOSEALL"
Button CloseAll Color Red

Button 15 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 ResetLogCounter
Button ResetLogCounter Text "Reset Log counter"
Button ResetLogCounter Color Coral

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

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

Button 28 Text EditConfigVars
Button EditConfigVars Text "Edit Config Vars"
Button EditConfigVars Color Coral

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

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

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

Gosub DisableAllButtons

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

' -------------------------------------- KEYS
Key Pageup   Gosub ExecKeys
Key Pagedown 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 "Theremino_HAL\Theremino_SignalScope.exe"

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

' -------------------------------------- TEST EMERGENCY
If Slot(Slot_Emergency) > 500
	Gosub Emergency
EndIf

Load Hide
Print ""

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

' ====================================================================
'  STARTING ACTIONS
' ====================================================================
Gosub PenUp
Gosub YellowOn
Controls ClearTextBox
'Gosub HomeDown
'Gosub Middle

Gosub EnableAllButtons
Gosub GreenOn
Option GlobalKeys Disabled

Stop																'


' ====================================================================
'  EMERGENCY
' ====================================================================
Label Emergency
	Gosub DisableAllButtons
	Button Emergency Enabled
	Button Emergency Color Flashing7 Yellow Red
	Gosub RedOn
	For v1 = 1 To 9e99
		Gosub RedOn
		Wait Seconds 0.4
		Gosub RgbOff
		Wait Seconds 0.4
		Load sound\Alarm10b.wav
		Wait Not MediaPlaying
		Load sound\Error.m4a
		Wait Not MediaPlaying
	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 CloseAll
	Gosub SaveVars
	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
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 HomeDown
	Gosub DisableAllButtons
	Gosub HomeDown_Core
	Gosub EnableAllButtons
Return

Label HomeDown_Core
	SlotMotor = Slot_MotorZ
	SlotSensor = Slot_SensorZ_DOWN
	SpeedSearch = -0.15
	SpeedReturn = +0.02
	DeclaredSwitchPos = Zaxis_SwitchPos
	InitialPosition = Zaxis_InitialPos
	Gosub HomeMotor
	Gosub PrintValues
	Gosub UpdateButtonZ
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
	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 (0) = 0			      'to me removed after DC-DC converter installed
		Slot 0 = Recognize        'to me removed after DC-DC converter installed
		Load sound\Error.m4a      'to me removed after DC-DC converter installed
		Wait Not MediaPlaying	  'to me removed after DC-DC converter installed
	EndIf                         'to me removed after DC-DC converter installed
	
	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
' ====================================================================
Label Up
	Slot(Slot_MotorZ) = ZUp
	Gosub WaitAllMotors
Return

Label Middle
	Slot(Slot_MotorZ) = ZMiddle
	Gosub WaitAllMotors
Return

Label Down
	Slot(Slot_MotorZ) = ZDown
	Gosub WaitAllMotors
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

' ====================================================================
'  PISTON
' ====================================================================
Label PistonIn
	Variable Numeric PistonInErrCounter
	Variable Numeric AlarmInCounter
	If Slot(Slot_PistonSensor) < 500
		PistonInErrCounter = PistonInErrCounter + 1
		LG1 = Chars("*", 25) + " Piston In Error num: " + PistonInErrCounter
		Save StringToFile LG1 LogName
		Print CRLF + LG1 + CRLF + CRLF
		If PistonInErrCounter >= 3
			For AlarmInCounter = 1 To 5
				Gosub RedOn
				Load Sound\Alarm10b.wav
				Wait Not MediaPlaying
				Load sound\Error.m4a
				Wait Not MediaPlaying
			Next
			Stop
		EndIf
	Else
		PistonInErrCounter = 0
	EndIf
Return

Label PistonOut
	Variable Numeric PistonOutErrCounter
	Variable Numeric AlarmOutCounter
	If Slot(Slot_PistonSensor) > 500
		PistonOutErrCounter = PistonOutErrCounter + 1
		LG1 = Chars("*", 25) + " Piston Out Error num: " + PistonOutErrCounter
		Save StringToFile LG1 LogName
		Print CRLF + LG1 + CRLF + CRLF
		If PistonOutErrCounter >= 3
			For AlarmOutCounter = 1 To 5
				Gosub RedOn
				Load Sound\Alarm10b.wav
				Wait Not MediaPlaying
				Load sound\Error.m4a
				Wait Not MediaPlaying
			Next
			Stop
		EndIf
	Else
		PistonOutErrCounter = 0
	EndIf
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 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 YellowOn
	Slot(Slot_LedR) = 800
	Slot(Slot_LedG) = 800
	Slot(Slot_LedB) = 0
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

' =====================================================================
'  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
	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
	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 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
		    ' ---------------------------------------------- ACTIONS
				Case "WaitAllMotors"
					Gosub WaitAllMotors
				Case "CLOSEALL"
					Gosub CloseAll
			' ---------------------------------------------- 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
			' ---------------------------------------------- HOME UP & DOWN
				Case "HomeDown"
					Gosub HomeDown_Core
				' ------------------------------------------ Pen actions
				Case "PenUp"            ' move pen up
					Gosub PenUp
				Case "PenDown"          ' move pen down
					Gosub PenDown
				Case "PenSingle"        ' single click
					Gosub PenSingle
		        ' ---------------------------------------------- PISTON
				Case "PistonOut"
					Gosub PistonOut
				Case "PistonIn"
					Gosub PistonIn
	            ' ---------------------------------------------- With parameters
				Case "MoveMotor"
					v1 =  GetSeparatedString(SequenceCmd, 1)
					Slot(Slot_MotorZ) = v1
					Gosub WaitAllMotors
						' ---------------------------------------------- Handler positions
				Case "Up"
					v1 =  GetSeparatedString(SequenceCmd, 1)
					Slot(Slot_MotorZ) = v1
					Gosub WaitAllMotors
				Case "Middle"
					v1 =  GetSeparatedString(SequenceCmd, 1)
					Slot(Slot_MotorZ) = v1
					Gosub WaitAllMotors
				Case "Down"
					v1 =  GetSeparatedString(SequenceCmd, 1)
					Slot(Slot_MotorZ) = v1
					Gosub WaitAllMotors
				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

' ====================================================================
'   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        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

' ====================================================================
'   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


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

' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	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("PageUp")
		Slot(Slot_MotorZ) = Limit(Slot(Slot_MotorZ) + JogSpeed, Zmin, Zmax)
	EndIf
	If Key("PageDown")
		Slot(Slot_MotorZ) = Limit(Slot(Slot_MotorZ) - JogSpeed, Zmin, Zmax)
	EndIf
	
	Gosub PrintValues
	Gosub UpdateButtonZ
Return

' ====================================================================
'  PRINT COORDINATES AND UPDATE COORDINATE BUTTONS
' ====================================================================
Label PrintValues
	Controls OpenTextBox
	s1 =       " DestZ=" + Format(Slot(Slot_MotorZ), "0.000") + CRLF
	Print CRLF + s1
Return

Label UpdateButtonZ
	Button BtnDestZ Text "Z=" + Format(Slot(Slot_MotorZ), "0.000")
Return


'WATCH PistonInErrCounter
'WATCH PistonOutErrCounter
'EXEC Sin2 = Sin(33)
