' ====================================================================
'     Name:	DoorLock_Linear_02.txt                   Date: 27 Apr 2021
'    Autor: Leonardo DE PALO Ph.D.
'    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

If True
	
EndIf


' -------------------------------------- Slots
Slot_Motor = 1

Slot_EnableMotors = 3

'Slot_Emergency = 11

'Slot_LedR = 19
'Slot_LedG = 20
'Slot_LedB = 21

' -------------------------------------- BUTTONS
Button 1 Text HomeMotor
Button 2 Text 0mm
Button 3 Text 50mm
Button 4 Text 100mm
Button 5 Text 150mm
Button 6 Text 200mm
Button 7 Text Shoeshine
'Button 8 Text -----

'Button  6 Text Stress Test
'Button  6 Color Gray
'Button  7 Text Paper Test
'Button  7 Color Yellow

''Button  8 Text Square Test
''Button  8 Color Yellow
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

' -------------------------------------- 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 old motor positions
'Gosub RestorePositions

If Slot(Slot_Emergency) > 500
	Goto EMERGENCY
EndIf

Gosub EnableAllButtons
Load Hide

Stop																'

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_Motor
'Variable Numeric Slot_SensorX
'Variable Numeric Slot_SensorY
'Variable Numeric Slot_SensorZ

Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB

'Variable Numeric Slot_Dispenser
'Variable Numeric DispenserTime
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 SavePositions
	Gosub DisableMotors
	Load CloseApps
Return

Label EventStop
'	Gosub SavePositions
	Gosub DisableMotors
	Gosub EnableAllButtons
End																	'

' ====================================================================
'  SAVE and RESTORE POSITIONS
' ====================================================================
'Label SavePositions
'	v1 = Slot(SlotMotor)
'	'v2 = Slot(Slot_Y)
'	'v3 = Slot(Slot_Z)
'	Save Vars
'Return
'
'Label RestorePositions
'	Load Vars
'	If v1 <> 0 And v2 <> 0 And v3 <> 0
'		Slot(SlotMotor) = Reset
'		Wait Seconds 0.01
'		Slot(SlotMotor) = v1
'		Wait Seconds 0.01
'
'		Slot(Slot_Y) = Reset
'		Wait Seconds 0.01
''	Slot(Slot_Y) = v2
'		Wait Seconds 0.01
'
'		Slot(Slot_Z) = Reset
'		Wait Seconds 0.01
''		Slot(Slot_Z) = v3
'		Wait Seconds 0.01
'	EndIf
'	Gosub PrintValues
'Return

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

Label 0mm
	Gosub EnableMotors
	Slot 1 = 0
	Gosub WaitAllMotors
	Gosub DisableMotors
Return

Label 50mm
	Gosub EnableMotors
	Slot 1 = 50
	Gosub WaitAllMotors
	Gosub DisableMotors
Return

Label 100mm
	Gosub EnableMotors
	Slot 1 = 100
	Gosub WaitAllMotors
	Gosub DisableMotors
Return

Label 150mm
	Gosub EnableMotors
	Slot 1 = 150
	Gosub WaitAllMotors
	Gosub DisableMotors
Return

Label 200mm
	Gosub EnableMotors
	Slot 1 = 200
	Gosub WaitAllMotors
	Gosub DisableMotors
Return

Label Shoeshine
	Gosub EnableMotors
	v1 = 0
	Label LoopInc
		If v1 > 200
			Goto LoopDec
		Else
			v1 = v1+1
			Slot 1 = v1
		EndIf
		'Wait Seconds 0.02		' do not alter, loop time 0.02 Sec
		Goto LoopInc
Label LoopDec
	If v1 > 0
		v1 = v1 - 1
		Slot 1 = v1
	Else
'		Gosub DisableMotor
'		Return
		Goto Shoeshine
	EndIf
	'Wait Seconds 0.02		' do not alter, loop time 0.02 Sec
Goto LoopDec
Return

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

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

Label BluON
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 800
Return

Label YellowOn
	Slot Slot_LedR = 800
	Slot Slot_LedG = 600
	Slot Slot_LedB = 0
Return

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

' ====================================================================
'  SEQUENCES
' ====================================================================
Label OpenTestSeq
	Load "Sequences\TestSeq.seq"
Return

Label TEST
	Button 9 Color Flashing8 Yellow Orange
	Gosub EnableMotors
	For v1 = 1 To 9e99
		SequenceFileName = "sequences\TestSeq.seq"
		Gosub ExecSequence
	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 "BLUON"
				Gosub BluON
			Case "YELLOWON"
				Gosub YellowOn
			Case "RGBOFF"
				Gosub RgbOff
			' ---------------------------------------------- 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"
						v1 =  GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_Motor) = v1
						Gosub WaitAllMotors
					Case "PAUSE"
						Gosub RedOn
						Wait Seconds GetSeparatedString(SequenceCmd, 1)
						Gosub GreenOn
					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

' ====================================================================
'  UPDATE FILE
' ====================================================================
Label UPDATE_FILE
	s1 = ""
	s1 = Now
	's1 = s1 + " " + Format(NumCycles, "000") + " " + Result
	Print s1
	Save StringToFile s1 s2
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

' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	s1 =       "X=" + Format(Slot(Slot_Motor), "0.000")
'	s1 = s1 + " Y=" + Format(Slot(Slot_Y), "0.000")
'	s1 = s1 + " Z=" + Format(Slot(Slot_Z), "0.000")
	Print CRLF + s1
Return




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