' ====================================================================
'     Name:	DoorLock_Linear_05.txt                   Date: 29 Apr 2021
'    Autor: Leonardo DE PALO Ph.D. - Roberto Fasciani MSC.Eng.
'    Scope:
' Hardware: Red Small PCB driver. All 3 dip SW positioned ON
'
' ====================================================================


Load "\images\DoorLockLinear.jpg"
'Wait Seconds 1
'Load Sound\gong.wav

Option Speed 9
Gosub RgbOff

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

Slot_LedR        = 7
Slot_LedG        = 8
Slot_LedB        = 9

' -------------------------------------- Pen Up / Down set up
PenDown_Value  = 418
PenUp_Value    = 570

' -------------------------------------- BUTTONS
Button 1 Text HomeMotor
Button 2 Text Up
Button 3 Text Middle
Button 4 Text Down

Button  9 Text TEST
Button  9 Color Yellow

Button 10 Text OpenTestSeq
Button OpenTestSeq Text "Open TestSeq.seq"
Button OpenTestSeq Color PaleGreen

Button 11 Text EnableMotors
Button 11 Color Tan
Button 12 Text DisableMotors
Button 12 Color Tan

Button 14 Text  CLOSE ALL
Button 14 Color Red

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

Gosub DisableAllButtons
Button 15 Enabled

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

' -------------------------------------- LOG FILE SET UP
Variable String FileHeader
Variable String LogFileName
LogFileName = "TestLogDoorLock.txt"

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

' -------------------------------------- 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 " Invalid master number: " + Slot(0)
'	Slot 0 = Recognize
'	Beep
'Next
'Print ""

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

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

If Slot(Slot_Emergency) > 500
	Goto EMERGENCY
EndIf

'Gosub RedOn
'Gosub HomeMotor
'Wait Seconds 1
'Gosub Middle

Gosub EnableAllButtons
'Load Hide
Gosub GreenOn

Controls OpenTextBox
'Load "\images\DoorLockLinear.jpg"

Stop																'

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_Motor

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

'Variable String FileName

' ====================================================================
'  EMERGENCY
' ====================================================================
Label EMERGENCY
	Gosub DisableAllButtons
	Button 15 Enabled
	Button 15 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
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 1000
Return

Label DisableMotors
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 0
Return

' =====================================================================
'  CLOSING EVENTS
' =====================================================================
Label CLOSE ALL
	Gosub SaveVars
	Gosub Middle
	Gosub DisableMotors
	Gosub RedOn
	Load CloseApps
Return

Label EventStop
	Gosub SaveVars
	Gosub Middle
	Gosub DisableMotors
	Gosub RedOn
	Gosub EnableAllButtons
End																	'

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

Label RestoreVars
	Load Vars
	NumCycle = v1
	ErrorNumber = v2
Return

Label ClearVars
	NumCycle = 0
	ErrorNumber = 0
Return

Label ResetLogCounter
	Gosub ClearVars
Return

' ====================================================================
'  FUNCTIONS
' ====================================================================
Label HomeMotor
	Gosub EnableMotors
	Slot(Slot_Motor) = Slot(Slot_Motor) - 160
	Gosub WaitAllMotors
	Slot(Slot_Motor) = Reset
	Wait Seconds 0.01
	Slot(Slot_Motor) = 0
	Wait Seconds 0.01
	Gosub DisableMotors
	Slot(Slot_Motor) = 2
Return

Label Up
	Gosub EnableMotors
	Slot(Slot_Motor) = 2
	Gosub WaitAllMotors
	Gosub DisableMotors
Return

Label Middle
	Gosub EnableMotors
	Slot(Slot_Motor) = 67
	Gosub WaitAllMotors
	Gosub DisableMotors
Return

Label Down
	Gosub EnableMotors
	Slot(Slot_Motor) =  140
	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) = 1000
	Slot(Slot_LedG) = 0
	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 YellowOn
	Slot(Slot_LedR) = 600
	Slot(Slot_LedG) = 400
	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 OpenTestSeq
	Load "Theremino_HAL\Theremino_Editor\Theremino_Editor.exe" ' Sequences\TestSeq.seq"
Return

Label TEST
	Gosub InitFileHeader
	Button 9 Color Flashing8 Yellow Orange
	Gosub EnableMotors
	For v1 = 1 To 9e99
		SequenceFileName = "sequences\TestSeq.seq"
		Gosub ExecSequence
		Gosub UpdateLog
	Next
	Button 9 Color Yellow
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 + CRLF
		'
		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 CLOSE ALL
			' ---------------------------------------------- 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
				' ---------------------------------------------- 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_Motor) = v1
						Gosub WaitAllMotors
						Gosub DisableMotors
						' ---------------------------------------------- Handler positions
					Case "UP"
						Gosub EnableMotors
						v1 =  GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_Motor) = v1
						Gosub WaitAllMotors
						Gosub DisableMotors
					Case "MIDDLE"
						Gosub EnableMotors
						v1 =  GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_Motor) = v1
						Gosub WaitAllMotors
						Gosub DisableMotors
					Case "DOWN"
						Gosub EnableMotors
						v1 =  GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_Motor) = v1
						Gosub WaitAllMotors
						Gosub DisableMotors
					Case "PAUSE"
						Wait Seconds GetSeparatedString(SequenceCmd, 1)
					CaseElse
						If s1 <> ""
							Controls OpenTextBox
							Print CRLF + " Unrecognized command: " + SequenceCmd
							Beep
							Stop								'
						EndIf
				EndSelect
		EndSelect
	Next
	Print SequenceFileName + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds"
Return

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

Label OpenLogfile
	Load "Files\TestLogDoorLock.txt"
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 LogFileName
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 LogFileName
	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_Motor + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return

'=====================================================================
' ============ stepper motor set-up  =================================
'=====================================================================
'
'  3   2   1  step x run
' OFF OFF OFF    200
'  ON OFF OFF    400
' OFF  ON OFF    800
'  ON  ON OFF   1600
' OFF OFF  ON   3200     4800   400    800
'  ON  ON  ON   6400     2400   200   1600




'WATCH SlotMotor
'WATCH Abs(Slot(SlotMotor))
'WATCH v1
'WATCH Load
'WATCH Load
'EXEC Sin2 = Sin(33)
