' ====================================================================
'     Name: WXM_DoorLockBT_6.txt                     Date: 22 May 2020
'    Autor: Leonardo DE PALO
'    Scope: Test Door lock smart phone Blue Tooth based.
' Hardware: Standard Master.
' ====================================================================

Load BT_finger_emulator.jpg

' ----------------------------------------- STEPPER MOTORS
NumMotors = 4 ' <<< Used by WaitAllMotors

Slot_MotorX       = 1
Slot_MotorY       = 3
Slot_MotorA       = 25
Slot_MotorZ       = 5

Slot_SensorX      = 19
Slot_SensorY      = 20
Slot_SensorA      = 31
Slot_SensorZ      = 21

Slot_EnableMotors = 11

SLOT_PEN          = 13

Slot_LASER        = 30
Slot_LockSensor   = 32

PEN_DOWN          = 232
PEN_UP            = 280


' ----------------------------------------- LOG FILE SET UP
Variable String FileHeader
Variable String LogFileName
LogFileName = "TestLogDoorLockBT.txt"
FileHeader = CRLF
FileHeader = FileHeader + "- DATE - " + " - TIME - " + " CYCLE "
FileHeader = FileHeader + " RESULT " + "    NUM. " + "    RATE IN %"
'FileHeader = FileHeader + "- = - = - = -  START  - = - = - = - "
Save StringToFile FileHeader LogFileName

' ----------------------------------------- LOAD APPS
Load ..\Theremino_HAL\Theremino_HAL.exe
Load ..\Theremino_HAL\Theremino_SlotViewer.exe

Key Right		Gosub MoveRight
Key Left 		Gosub MoveLeft
Key Up 			Gosub MoveFar
Key Down 		Gosub MoveNear
Key Pageup 		Gosub MoveUp
Key Pagedown 	Gosub MoveDown
Key A 			Gosub MoveAfar
Key Z 			Gosub MoveAnear

' ----------------------------------------- START
Gosub PEN UP
Option Speed 9

'Window MinSize

COM Open 1 115200         ' Home

Button  1 Text  Home 
Button  1 Color Green9
Button  2 Text  Home_X 
Button  2 Color GreenYellow
'Button  3 Text  Home_Y 
'Button  3 Color GreenYellow
Button  4 Text  Home_A 
Button  4 Color GreenYellow
Button  5 Text  Home_Z 
Button  5 Color GreenYellow

Button  7 Text  CONTINUE TEST 
Button  7 Color Yellow9
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  re-start test 
Button 11 Color Yellow9



Button 13 Text  ENABLE MOT 
Button 13 Color YellowGreen
Button 14 Text  DISABLE MOT 
Button 14 Color Gold

Button 16 Text  CLOSE ALL 
Button 16 Color OrangeRed

Variable Numeric Slot_MotorX
Variable Numeric Slot_MotorY
Variable Numeric Slot_MotorA
Variable Numeric Slot_MotorZ

Variable Numeric Slot_SensorX
Variable Numeric Slot_SensorY
Variable Numeric Slot_SensorA
Variable Numeric Slot_SensorZ
Variable Numeric Slot_EnableMotors
Variable Numeric Slot_LASER
Variable Numeric Slot_LockSensor

Gosub LoadVars
Wait Seconds 1
Load Hide
'Gosub "MechTest"
Stop


' ===============  Save, Load and Clear VARS  =========================
Label SaveVars
	v1 = NumCycle
	v2 = ErrorNumber
	Save Vars
Return

Label LoadVars
	Load Vars
	NumCycle = v1
	ErrorNumber = v2
Return

Label ClearVars
	NumCycle = 0
	ErrorNumber = 0
Return

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

Label CONTINUE TEST
	Save StringToFile FileHeader FileName
	Gosub Home
	For NumCycle = NumCycle To 60000
		Gosub SaveVars
		
		SequenceFileName = "SEQUENCES\HandleDown.seq"
		Gosub ExecSequence
		
        '------------------ laser on and test
		Slot(Slot_LASER) = 1000    ' LASER ON
		Wait Seconds 0.2
		
		If Slot(Slot_LockSensor) > 500
			Gosub TEST PASSED
		Else
			Gosub TEST FAIL
		EndIf
		
		Slot(Slot_LASER) = 0    ' LASER OFF
		Wait Seconds 1
		
		SequenceFileName = "SEQUENCES\HandleUp.seq"
		Gosub ExecSequence
		
		Print "Cycle number:" + NumCycle
'		Wait Seconds 1.5
	Next
Return


Label re-start test
	Gosub ClearVars
	Gosub CONTINUE TEST
	
Return


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

Label TEST FAIL
	Gosub RED
	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

Label UPDATE_FILE
	s1 = ""
	s1 = Now
	s1 = s1 + " " + Format(NumCycle, "000") + " " + Result
	s2 = "Test: " + Format(NumCycle, "000")
	Print S2
	Save StringToFile s1 LogFileName
Return

Label RED ALARM
	Load Sound\Alarm_2_times.wav		'<<<<<<<<<<<<<<<<
	For RedFlash = 1 To 5
		Gosub BLACK
		Wait Seconds 0.2
		Gosub RED
		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
' =====================================================================
Variable Numeric SLOT_PEN

Label PEN UP
	Move_Slot = SLOT_PEN
	Move_Dest = PEN_UP
	Slot(Move_Slot) = Move_Dest
	Wait Seconds 0.1
Return

Label PEN DOWN
	Move_Slot = SLOT_PEN
	Move_Dest = PEN_DOWN
	Slot(Move_Slot) = Move_Dest
	Wait Seconds 0.1
Return

Label PEN SINGLE
	Gosub BLU
	Gosub PEN DOWN
	Wait Seconds 0.35
	Gosub PEN UP
	Gosub GREEN
Return

' =====================================================================
'  LED COLORS
' =====================================================================
Label RED
	Slot 16 = 800		'RED
	Slot 17 =    0		'GRE
	Slot 18 =    0		'BLU
Return

Label GREEN
	Slot 16 =    0		'RED
	Slot 17 =  800		'GRE
	Slot 18 =    0		'BLU
Return

Label BLU
	Slot 16 =    0		'RED
	Slot 17 =    0		'GRE
	Slot 18 =  800		'BLU
Return

Label YELLOW
	Slot 16 =  750		'RED
	Slot 17 =  600		'GRE
	Slot 18 =  600		'BLU
Return

Label WHITE
	Slot 16 =  750		'RED
	Slot 17 =  750		'GRE
	Slot 18 =  750		'BLU
Return

Label BLACK
	Slot 16 =    0		'RED
	Slot 17 =    0		'GRE
	Slot 18 =    0		'BLU
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)
            ' ------------------- exit from the sequence -------------
			Case "ENDSEQ"
				SequenceIndex = SequenceLength-1
		    ' --------------------- single actions -------------------
			Case "ENABLE MOTORS"     ' enable movement motors
				Gosub ENABLE MOT
			Case "DISABLE MOTORS"    ' disable movement motors
				Gosub DISABLE MOT
			Case "CLOSE ALL"
				Gosub CLOSE ALL
			' --------------------- 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
			' --------------------- WAIT MOTORS -----------------------
			Case "WAIT_MOTORS"
				Gosub WaitAllMotors
			' --------------------- LIGHT COLORS ----------------------
			Case "RED"
				Gosub RED
			Case "GREEN"
				Gosub GREEN
			Case "BLU"
				Gosub BLU
			Case "WHITE"
				Gosub WHITE
			Case "YELLOW"
				Gosub YELLOW
			Case "BLACK"
				Gosub BLACK
			' --------------------- HOMES -----------------------------
			Case "HOME"
				Gosub Home
			Case "HOMEX"
				Gosub Home_X
			Case "HOMEY"
				Gosub Home_Y
'			Case "HOMEA"
'				Gosub "HOME_A"
			Case "HOMEZ"
				Gosub Home_Z
			' --------------------- Istruction with parameters --------
			CaseElse
				s1 = GetSeparatedString(SequenceCmd, 0)
				Select UCase(s1)
					Case "MOVE_X"
						Slot(Slot_MotorX) = GetSeparatedString(SequenceCmd, 1)
						Gosub WaitAllMotors
					Case "MOVE_Y"
						Slot(Slot_MotorY) = GetSeparatedString(SequenceCmd, 1)
						Gosub WaitAllMotors
					Case "MOVE_Z"
						Slot(Slot_MotorZ) = GetSeparatedString(SequenceCmd, 1)
						Gosub WaitAllMotors
					Case "MOVE_XY"
						Slot(Slot_MotorX) = GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_MotorY) = GetSeparatedString(SequenceCmd, 2)
						Gosub WaitAllMotors
					Case "MOVE_XYZ"
						Slot(Slot_MotorX) = GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_MotorY) = GetSeparatedString(SequenceCmd, 2)
						Slot(Slot_MotorZ) = GetSeparatedString(SequenceCmd, 3)
					Case "MOVE_XAZ"
						Slot(Slot_MotorX) = GetSeparatedString(SequenceCmd, 1)
						Slot(Slot_MotorA) = GetSeparatedString(SequenceCmd, 2)
						Slot(Slot_MotorZ) = GetSeparatedString(SequenceCmd, 3)
						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 ==============================================
Label Home
	Gosub ENABLE MOT
	Gosub Home_Z
	Gosub Home_X
	'Gosub "Home_Y"
	Gosub Home_A
Return

' ===================================================================
'  HOMING M0 M1 M2
' ===================================================================
Label Home_X
	Gosub ENABLE MOT
	SlotMotor = Slot_MotorX
	SlotSensor = Slot_SensorX
	SpeedSearch = -0.1  	' search direction and speed
	SpeedReturn = +0.01	    ' return direction and speed
	DeclaredZero = 0		' declared switch position
	InitialPosition = 2 	' move to this position
	Gosub HomeMotor
	'Gosub PrintValues
	Button 2 Color SkyBlue
Return

Label Home_Y
	Gosub ENABLE MOT
	SlotMotor = Slot_MotorY
	SlotSensor = Slot_SensorY
	SpeedSearch = -0.1		' search direction and speed
	SpeedReturn = 0.015	' return direction and speed
	DeclaredZero = 0		' declared switch position
	InitialPosition = 2 	' move to this position
	Gosub HomeMotor
	'Gosub PrintValues
	Button 3 Color SkyBlue
Return

Label Home_A
	Gosub ENABLE MOT
	SlotMotor = Slot_MotorA
	SlotSensor = Slot_SensorA
	SpeedSearch = -0.1		' search direction and speed
	SpeedReturn = 0.015		' return direction and speed
	DeclaredZero = 0		' declared switch angle
	InitialPosition = 2	' move to this angle
	Gosub HomeMotor
	'Gosub PrintValues
	Button 4 Color SkyBlue
Return

Label Home_Z
	Gosub ENABLE MOT
	SlotMotor = Slot_MotorZ
	SlotSensor = Slot_SensorZ
	SpeedSearch = 0.1		' search direction and speed
	SpeedReturn = -0.02		' return direction and speed
	DeclaredZero = 192		' declared switch angle
	InitialPosition = 190	' move to this angle
	Gosub HomeMotor
	'Gosub PrintValues
	Button 5 Color SkyBlue
Return

' ==========================================================
'  Home the Motor(SlotMotor) to the Sensor(SlotSensor)
' ==========================================================
Variable Numeric SlotMotor
Variable Numeric SlotSensor
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredZero
Variable Numeric InitialPosition

Label HomeMotor
	' ------------------------- 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")
		Gosub WaitMotor
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' ------------------------- detach from the sensor
Label DetachSensor
	Slot(SlotMotor) = Slot(SlotMotor) + SpeedReturn
	Wait Seconds 0.001
	If Slot (SlotSensor) < 500
		Gosub WaitMotor
		Goto AssignZero
	EndIf
Goto DetachSensor
    ' ------------------------- reset and move to InitialPos
Label AssignZero
	Slot(SlotMotor) = Reset
	Wait Seconds 0.08
	Slot(SlotMotor) = DeclaredZero
	Gosub WaitMotor
	Slot(SlotMotor) = InitialPosition
	Gosub WaitMotor
Return


' =====================================================================
'  SERVO-MOTORS MOVEMENTS
' =====================================================================
Variable Numeric PEN_DOWN
Variable Numeric PEN_UP

' ==========================================================
'   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 MotorNumber = 0 To NumMotors - 1
		Gosub MotorNumberToSlot
		If Abs(Slot(SlotFromMotorNumber + 1)) > 0.01
			MotorNumber = -1
		EndIf
		Wait Seconds 0.01
	Next
Return


' =====================================================================
'  SERVO-MOTORS MOVE SLOW
' =====================================================================
Variable Numeric Move_Slot
Variable Numeric Move_Speed
Variable Numeric Move_Dest
Variable Numeric Move_Pos

Label MoveSlow
	Move_Pos = Slot(Move_Slot)
	If Move_Dest > Move_Pos
		Move_Pos = Move_Pos + Move_Speed
		If Move_Pos > Move_Dest
			Move_Pos = Move_Dest
		EndIf
	Else
		Move_Pos = Move_Pos - Move_Speed
		If Move_Pos < Move_Dest
			Move_Pos = Move_Dest
		EndIf
	EndIf
	Slot(Move_Slot) = Move_Pos
	Wait Seconds 0.01
	If Move_Pos = Move_Dest
		Return
	EndIf
Goto MoveSlow

' =====================================================================
'  MOTOR NUMBER TO SLOT CONVERSION
' =====================================================================
Variable Numeric NumMotors
Variable Numeric MotorNumber
Variable Numeric SlotFromMotorNumber

Label MotorNumberToSlot
	Select MotorNumber
		Case 0
			SlotFromMotorNumber = Slot_MotorX
		Case 1
			SlotFromMotorNumber = Slot_MotorY
		Case 2
			SlotFromMotorNumber = Slot_MotorA
		Case 3
			SlotFromMotorNumber = Slot_MotorZ
	EndSelect
Return

' ==========================================================
'   ENABLE DISABLE MOTORS
' ==========================================================
Label ENABLE MOT
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 1000
Return

Label DISABLE MOT
	Gosub WaitAllMotors
	Slot(Slot_EnableMotors) = 0
Return


' ================================================
'  JOG WITH keyboard
'  (SHIFT = x100 / CTRL = x10 / ALT = x0.1)
' ================================================
Label MoveRight
	Gosub SpeedModifiers
	Slot(Slot_MotorX) = Slot(Slot_MotorX) + JogSpeed
Return

Label MoveLeft
	Gosub SpeedModifiers
	Slot(Slot_MotorX) = Slot(Slot_MotorX) - JogSpeed
Return

Label MoveFar
	Gosub SpeedModifiers
	Slot(Slot_MotorA) = Slot(Slot_MotorA) + JogSpeed
Return

Label MoveNear
	Gosub SpeedModifiers
	Slot(Slot_MotorA) = Slot(Slot_MotorA) - JogSpeed
Return

Label MoveUp
	Gosub SpeedModifiers
	Slot(Slot_MotorZ) = Slot(Slot_MotorZ) + JogSpeed
Return

Label MoveDown
	Gosub SpeedModifiers
	Slot(Slot_MotorZ) = Slot(Slot_MotorZ) - JogSpeed
Return

Label MoveAfar
	Gosub SpeedModifiers
	Slot(Slot_MotorA) = Slot(Slot_MotorA) + JogSpeed
Return

Label MoveAnear
	Gosub SpeedModifiers
	Slot(Slot_MotorA) = Slot(Slot_MotorA) - JogSpeed
Return

Variable Numeric JogSpeed
Label SpeedModifiers
	JogSpeed = 0.1
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 100
		Case Key("CTRL")
			JogSpeed = JogSpeed * 10
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.1
	EndSelect
Return

' ================================================
'  ReceiveFileFromCOM
' ================================================
Label ReceiveFileFromCOM
	Variable String FileName
	Variable String Str1
	'
	COM DiscardInBuffer
	'
	Str1 = ""
	For v1 = 1 To 9e99
		s1 = COM_Received
		If s1 <> ""
			s1 = Replace(s1, "{{{CRLF}}}", CRLF)
			Str1 = Str1 + s1
			If s1 = "{{{FileEndMarker}}}"
				Str1 = Replace(Str1, "{{{FileEndMarker}}}", "")
				Save DeleteFile FileName
				Save StringToFile Str1 FileName
				Exit
			EndIf
		EndIf
		Wait Seconds 0.01
	Next
Return

' ================================================
'  CLOSE ALL and EVENT STOP
' ================================================
Label CLOSE ALL
	Gosub DISABLE MOT
	Load CloseApps
Return

Label EventStop
	Window Sizable
End


'WATCH s1
'EXEC DestX = 12
