' ====================================================================
'     Name: WXM_DoorLockBTSM29_1.txt                 Date: 29 Oct 2020
'    Autor: Leonardo DE PALO
'    Scope: Test DoorLock using smart phone Blue Tooth and MOTOR SM45
' Hardware: Standard Master. Recycled Cartesian robot.
'
' ====================================================================

'Load "media\images\FeeTech SM29.png"
'Load Cartesian.jpg
'Load BT_finger_emulator.jpg

'Wait Seconds 2
'Button  sound\gong.wav
Option Speed 9

' -------------------------------------- Slots
Slot_R            = 12
Slot_Current      = 11
Slot_Emergency    = 912
Slot_Torque       = 913


Slot_LedR = 907
Slot_LedG = 908
Slot_LedB = 909

' -------------------------------------- Software limits
Rmax =   4095
Rmin =    2

' -------------------------------------- BUTTONS
Button  1 Text  Home R
Button  1 Color SkyBlue


Button  8 Text  PEN UP
Button  8 Color LightSteelBlue
Button  9 Text  PEN DOWN
Button  9 Color LightSteelBlue
Button 10 Text  PEN SINGLE
Button 10 Color LightSteelBlue
'Button 11 Text  free
'Button 11 Color Yellow9

Button 15 Text  CLOSE ALL
Button 15 Color Red

Button 16 Text  EMERGENCY
Button 16 Color Red
Button 16 Slot  Slot_Emergency


Button 17 Text  RedOn
Button 17 Color Red
Button 18 Text  GreenOn
Button 18 Color Green
Button 19 Text  BluON
Button 19 Color Blue
Button 20 Text  WhiteOn
Button 20 Color White
Button 21 Text  RgbOff
Button 21 Color Black

Button 23 Text  set-up TURNKEY
Button 23 Color Aqua
Button 24 Text  TURNKEY
Button 24 Color Aqua

Button 27 Text  R_0
Button 27 Color Tan
Button 28 Text  R_90
Button 28 Color Tan
Button 29 Text  R_180
Button 29 Color Tan
Button 30 Text  R_270
Button 30 Color Tan
Button 31 Text  R_360
Button 31 Color Tan

Button 32 Text  Reset Log counter
Button 32 Color White
'Gosub DisableAllButtons

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

' -------------------------------------- LOG FILE SET UP
Variable String FileHeader
Variable String LogFileName
LogFileName = "TestLogDoorLockBT.txt"
FileHeader = CRLF
FileHeader = FileHeader + "- DATE - " + " - TIME - " + " CYCLE "
FileHeader = FileHeader + " - EXEC TIME - "
'FileHeader = FileHeader + "- = - = - = -  START  - = - = - = - "
Save StringToFile FileHeader LogFileName

' -------------------------------------- LOAD APPS
Load "Theremino_HAL\Theremino_HAL.exe"
Load "Theremino_HAL\Theremino_MODBUS.exe"
Load "Theremino_HAL\Theremino_SlotViewer.exe"
Load "Theremino_HAL\Theremino_SignalScope.exe"


' -------------------------------------- TEST MASTER PRESENT
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)
	Slot 0 = Recognize
	Beep
Next
Print ""

' -------------------------------------- Restore old motor positions and variables
Gosub RestoreVarsAndPositions

' -------------------------------------- Miscellanous start
Gosub PEN UP
'Window MinSize
'Wait Seconds 1
Load Hide
Gosub RedOn
Gosub Home R
'Wait Seconds 1

Gosub WhiteOn


'<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
Stop						 '<<<<<<<<<<<<<< S T O P >>>>>>>>>>>>>>>>>
'<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_R
Variable Numeric SlotMotor

Variable Numeric Slot_Current
Variable Numeric Slot_Torque

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_Emergency

Variable Numeric Rmax
Variable Numeric Rmin

' ====================================================================
'  EMERGENCY
' ====================================================================
Label EMERGENCY
	Gosub DisableAllButtons
	Button 15 Enabled
	Button 15 Color Flashing7 Yellow Red
	KeyJog_Speed = 0
	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
	Button 24 Disabled		'Turnkey
Return

Label DisableAllButtons
	For v1 = 1 To 32
		Button v1 Disabled
	Next
Return

' ====================================================================
'  CLOSING EVENTS
' ====================================================================
Label CLOSE ALL
	Gosub Home R
	Gosub SaveVarsAndPositions
	Load CloseApps
Return

Label EventStop
	Gosub Home R
	Gosub SaveVarsAndPositions
	Gosub EnableAllButtons
	Button  5 Disabled
	Window Sizable
End

Label EventTimer
	Variable Numeric TorqueNow
	TorqueNow = Slot(Slot_Torque)
	TorqueNow = TorqueNow + (Slot(Slot_Current) - TorqueNow) * 0.2
	Slot(Slot_Torque) = TorqueNow
Return

ppp

' ====================================================================
'  SAVE LOAD and CLEAR VARS and POSITIONS
' ====================================================================
Label SaveVarsAndPositions
	v1 = Slot(Slot_R)
	v2 = NumCycle
	v3 = ErrorNumber
	Save Vars
Return

Label RestoreVarsAndPositions
	Load Vars
	If v1 <> 0 And v2 <> 0 And v3 <> 0
		Slot(Slot_R) = Reset
		Wait Seconds 0.01
		Slot(Slot_R) = v1
		Wait Seconds 0.01
	EndIf
	'
	NumCycle = v2
	ErrorNumber = v3
	'
	Gosub PrintValues
Return

Label ClearVars
	NumCycle = 0
	ErrorNumber = 0
Return

' ====================================================================
'   FUNCTIONS
' ====================================================================

Label R_0
	SlotMotor = Slot_R
	Slot (SlotMotor) = 0
Return

Label R_90
	SlotMotor = Slot_R
	Slot (SlotMotor) = 1023
Return

Label R_180
	SlotMotor = Slot_R
	Slot (SlotMotor) = 2047
Return

Label R_270
	SlotMotor = Slot_R
	Slot (SlotMotor) = 3071
Return

Label R_360
	SlotMotor = Slot_R
	Slot (SlotMotor) = 4095
Return

' ====================================================================
'  TEST
' ====================================================================
Variable Numeric NumCycle
Variable Numeric ErrorNumber
Variable Numeric ErrorPercentage
Variable Numeric RedFlash
Variable String  Result

Label set-up turnkey
	'For NumCycle = NumCycle To 9e99
	SequenceFileName = "SEQUENCES\SetUpTurnKey.seq"
	Gosub ExecSequence
	'Next
	Button 23 Disabled		' Set-Up Turnkey
	Button 24 Enabled		' Turnkey
Return

Label TURNKEY
	'Gosub Home_Z
	'Gosub Home_X
	'Gosub Home_R
	StartTime = ElapsedTime
	For NumCycle = NumCycle To 9e99
		SequenceFileName = "SEQUENCES\TestTurnKey.seq"
		Gosub ExecSequence
		Print "Cycle number:" + NumCycle
		Gosub UPDATE_FILE
	Next
Return

Label Reset Log counter
	Gosub ClearVars
Return

Label TEST PASSED
	Gosub GreenOn
	Result = " -PASS- "
	Gosub UPDATE_FILE
Return

Label TEST FAIL
	Gosub RedOn
	Gosub SaveRecord
	Load Sound\DoorBell.wav
	ErrorNumber = ErrorNumber + 1
	ErrorPercentage = (100 * ErrorNumber) / NumCycle
	Result = " -FAIL- " + "Num Err:" + Format(ErrorNumber, "000") + "   %=" + Format(ErrorPercentage, "00.00")
	Gosub UPDATE_FILE
	Gosub RED ALARM
Return

Variable Numeric StartTime
Label UPDATE_FILE
	s1 = ""
	s1 = Format(Now, "dd/MMM/yyyy HH:mm:ss")
	s1 = s1 + " " + Format(NumCycle, "000") + " " + Result
	s1 = s1 + " " + Format(ElapsedTime - StartTime, "0.00")
	StartTime = ElapsedTime
	Save StringToFile s1 LogFileName
	' ----------------------------------------------------
	s2 = "Test: " + Format(NumCycle, "000")
	Print S2
Return

Label RED ALARM
	Load Sound\Alarm_2_times.wav		'<<<<<<<<<<<<<<<<
	For RedFlash = 1 To 5
		Gosub RgbOff
		Wait Seconds 0.2
		Gosub RedOn
		Wait Seconds 0.15
	Next
Return

' ====================================================================
'  V I D E O
' ====================================================================
Label StartRecord
	Slot(49) = 1000			'	Start recording
Return

Label StopRecord
	Slot(49) = 0
Return

Label SaveRecord
	Slot(50) = 1000			' save to file
	Wait Seconds 0.1
	Slot(50) =    0
	Slot(49) =    0			' Stop recording and save to file
Return


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

Label PEN DOWN
	Slot(Slot_PEN) = PenDown_Value
	Wait Seconds 0.1
Return

Label PEN SINGLE
	Gosub BluON
	Gosub PEN DOWN
	Wait Seconds 0.35
	Gosub PEN UP
	Gosub GreenOn
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 = 400
	Slot Slot_LedB = 0
Return

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

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

Label WhiteFrontOn
	'Slot Slot_WhiteFront = 1000
	Gosub WhiteOn
Return

Label WhiteFrontOFF
	'Slot Slot_WhiteFront = 0
	Gosub RgbOff
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
		'
		Select UCase(SequenceCmd)
            ' ---------------------------------------------- End sequence
			Case "ENDSEQ"
				SequenceIndex = SequenceLength-1
		    ' ---------------------------------------------- ACTIONS
			'Case "ENABLE MOTORS"     ' enable movement motors
			'	Gosub Enable Motors
			'Case "DISABLE MOTORS"    ' disable movement motors
			'	Gosub Disable Motors
			Case "WAIT_MOTORS"
				Gosub WaitAllMotors
			Case "CLOSE ALL"
				Gosub CLOSE ALL
			' ---------------------------------------------- HOMING
			Case "HOME_R"
				Gosub Home R
			' ---------------------------------------------- LED COLORS
			Case "REDON"
				Gosub RedOn
			Case "GREENON"
				Gosub GreenOn
			Case "BLUON"
				Gosub BluON
			Case "YELLOWON"
				Gosub YellowOn
			Case "WHITEON"
				Gosub WhiteOn
			Case "RGBOFF"
				Gosub RgbOff
			Case "WHITEFRONTON"
				Gosub WhiteFrontOn
			Case "WHITEFRONTOFF"
				Gosub WhiteFrontOFF
			' ---------------------------------------------- Pen actions
			Case "PEN UP"            ' move pen up
				Gosub PEN UP
			Case "PEN DOWN"          ' move pen down
				Gosub PEN DOWN
			Case "PEN SINGLE"        ' single click
				Gosub PEN SINGLE
			' ---------------------------------------------- With parameters
			CaseElse
				s1 = GetSeparatedString(SequenceCmd, 0)
				Select UCase(s1)
					Case "MOVE_R"
						Slot(Slot_R) = GetSeparatedString(SequenceCmd, 1)
						Gosub WaitAllMotors
					Case "PAUSE"
						Wait Seconds GetSeparatedString(SequenceCmd, 1)
					CaseElse
						If s1 <> ""
							Controls OpenTextBox
							Print CRLF + " Unrecognized command: " + SequenceCmd
							Beep
							Stop
						EndIf
				EndSelect
		EndSelect
	Next
	SequenceFileName = Replace(SequenceFileName,".seq","")
	Print SequenceFileName + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds"
Return

' ====================================================================
'  HOME  R
' ====================================================================
Label Home R
	Slot(Slot_R) = 0
	Gosub PrintValues
	Button 6 Color SkyBlue
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 the Motors are stopped
' ====================================================================
Label WaitAllMotors
	Wait Seconds 0.1 ' <<< ensures that motors are really started
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(Slot_R + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return

' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	Variable Numeric KeyJog_Speed
	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("Right")
		Slot Slot_R = Limit(Slot(Slot_R) + JogSpeed, Rmin, Rmax)
	EndIf
	If Key("Left")
		Slot Slot_R = Limit(Slot(Slot_R) - JogSpeed, Rmin, Rmax)
	EndIf
	
	Gosub PrintValues
Return


' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	s1 =       "R=" + Format(Slot(Slot_R), "0.000")
	Print CRLF + s1
Return


'WATCH s1
'EXEC DestX = 12
