'     Name: WXM_WXM_DoorLockX1_15.txt                Date: 15 Apr 2021
'    Autor: Leonardo DE PALO
'    Scope: Test DoorLock using BT remote and MOTORS SM29 with RS485
'           and  Protocol1 of FeeTech.
' Hardware: Standard Master.
' note:  SIGNALSCOPE-MOTOR-TORQUE slot 464 = min -150 max 100
' ====================================================================

Option Speed 9

Load "media\images\DoorLock_test_system.jpg"
Gosub BlueOn

'Wait Seconds 1
'Button  sound\gong.wav
' -------------------------------------- Initialize SLOTS, BUTTONS and KEYS
Gosub SetSlots
Slot(Slot_Emergency) = 0
Slot(Slot_Pause) = 0
Gosub SetButtonsAndKeys
'Gosub DisableAllButtons

' -------------------------------------- Torque parameters - DoorLock
' DoorLock limit: 1.5 newton/meter
' DoorLock limit: 15.3 kg/cm
' -------------------------------------- Torque parameters - Motor
' Motor test: 679 current = 352 g/25cm
' Motor test: 679 current = 8.8 kg/cm
' Motor test: 679 current = 0.863 n/m
' --------------------------------------
Coeff_NM = 0.001271  '(0.863 / 679)

'-------------------------------------- Torque limits in Newton Meter
TorqueForStopCW  = 0.7 ' View from motor side (human view)
TorqueForStopCCW = 0.7
TorqueForLogCW   = 0.35
TorqueForLogCCW  = 0.35

' -------------------------------------- Rotation software limits
Rmax           = +30719
Rmin           = -30719

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

PenUp_Value    = 570

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

' -------------------------------------- LOAD APPS
Load "Theremino_RS485\Theremino_RS485.exe"

Load "Theremino_HAL\Theremino_HAL.exe"
Load "Theremino_HAL\Theremino_SlotViewer.exe"
Load "Theremino_HAL\Theremino_SlotViewer1.exe"
Load "Theremino_HAL\Theremino_SlotViewer2.exe"
Load "Theremino_HAL\Theremino_SlotViewer3.exe"
'Load "Theremino_HAL\Theremino_SlotViewer4.exe"
Load "Theremino_HAL\Theremino_SignalScope_MOTOR_TORQUE.exe"
Load "Theremino_HAL\Theremino_SignalScope_BLUETOOTH_SOLENOID.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) + CRLF
'	Slot 0 = Recognize
'	Beep
'Next
'
'' -------------------------------------- TEST MOTORS WORKING
'Slot (Slot_Temperature) = -999
'For v1 = 1 To 9e99
'	For v2 = 1 To 20 ' <<< testing for 2 seconds
'		If Slot (Slot_Temperature) > -999 Or Key("CTRL")
'			Print ""
'			Exit 2
'		EndIf
'		Wait Seconds 0.1
'	Next
'	Print " Modbus not working" + CRLF
'	Beep
'Next

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

' -------------------------------------- Miscellanous start
Gosub PenUp
'Window MinSize
'Wait Seconds 1
Load Hide

Gosub InitMotors

' ====================================================================
'  MAIN LOOP
' ====================================================================
Label MainLoop
	Gosub CheckMotorTemperature
	Gosub UpdateButtons
	' ---------------------------------------------------------
	' 0.02 sec. or more to allow time for keys and other tasks
	Wait Seconds 0.1
	' ---------------------------------------------------------
Goto MainLoop

Label UpdateButtons
	Button m1 Text "M1=" + Format(Slot(Slot_Destination1) * 360 / 4096, "0.0")
	Button m2 Text "M2=" + Format(Slot(Slot_Destination2) * 360 / 4096, "0.0")
	Button m3 Text "M3=" + Format(Slot(Slot_Destination3) * 360 / 4096, "0.0")
Return



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

' ====================================================================
'  MOTORS
' ====================================================================
Variable Numeric Slot_Destination1
Variable Numeric Slot_Destination2
Variable Numeric Slot_Destination3
Variable Numeric Slot_Destination4
Variable Numeric Slot_Position1
Variable Numeric Slot_Position2
Variable Numeric Slot_Position3
Variable Numeric Slot_Position4

Label InitMotors
	' --------------------------------- Init Destination Slots
	Slot_Destination1 = 101
	Slot_Destination2 = 201
	Slot_Destination3 = 301
	Slot_Destination4 = 401
	' --------------------------------- Init Position Slots
	Slot_Position1 = 102
	Slot_Position2 = 202
	Slot_Position3 = 302
	Slot_Position4 = 402
	' --------------------------------- Torque 0-1000 to motors 1-4
	Slot(120) = 800
	Slot(220) = 800
	Slot(320) = 800
	Slot(420) = 500
	' --------------------------------- Acceleration 0-255 to motors 1-4
	Slot(121) = 50
	Slot(221) = 50
	Slot(321) = 50
	Slot(421) = 50
	' --------------------------------- Speed 0-10000 to motors 1-4
	Slot(122) = 3000
	Slot(222) = 3000
	Slot(322) = 3000
	Slot(422) = 3000
Return

Label EnableMotors
	Slot(123) = 1
	Slot(223) = 1
	Slot(323) = 1
	Slot(423) = 1
Return

Label DisableMotors
	Slot(123) = 0
	Slot(223) = 0
	Slot(323) = 0
	Slot(423) = 0
Return

Label SetCenterPositions
	Slot(123) = 128
	Slot(223) = 128
	Slot(323) = 128
	Slot(423) = 128
	Wait Seconds 0.5
	Gosub EnableMotors
	Gosub CopyPositionsToDestinations
Return

Label CopyPositionsToDestinations
	Slot(Slot_Destination1) = Slot(Slot_Position1)
	Slot(Slot_Destination2) = Slot(Slot_Position2)
	Slot(Slot_Destination3) = Slot(Slot_Position3)
	Slot(Slot_Destination4) = Slot(Slot_Position4)
Return

' ---------------------------------- goto the middle point at 2048
Label GotoCenterPositions
	Slot(Slot_Destination1) = 2048
	Slot(Slot_Destination2) = 2048
	Slot(Slot_Destination3) = 2048
	Slot(Slot_Destination4) = 2048
Return

Variable Numeric MotorsTemperature
Label TestMotorsTemperature
	MotorsTemperature = Max(Slot(106), Slot(107))
	MotorsTemperature = Max(MotorsTemperature, Slot(108))
	MotorsTemperature = Max(MotorsTemperature, Slot(109))
Return

Variable Numeric MotorsMoving
Label TestMotorsMoving
	MotorsMoving = Abs(Slot(103)) + Abs(Slot(203)) + Abs(Slot(303)) + Abs(Slot(403))
Return

Label WaitAllMotors
	Variable Numeric WM1
	Wait Seconds 0.1 ' <<< ensures that motors are really started
	For WM1 = 1 To 9e99
		Gosub TestMotorsMoving
		If MotorsMoving < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
		Gosub TorqueReader
	Next
	Wait Seconds 0.1
	Gosub TorqueReader
Return

Label InitPositions
	Slot(Slot_Destination1) = 125 * 4096 / 360
	Slot(Slot_Destination2) = 100 * 4096 / 360
	Slot(Slot_Destination3) =  90 * 4096 / 360
	Slot(Slot_Destination4) =   1 * 4096 / 360
	Gosub WaitAllMotors
Return


Label CheckMotorTemperature
	Variable Numeric OldTemperature
	Gosub TestMotorsTemperature
	If MotorsTemperature <> OldTemperature
		OldTemperature = MotorsTemperature
		Button Temperature Text "MotorTemp: " + Format(MotorsTemperature, "0")
		If MotorsTemperature > 55
			For v1 = 1 To 10
				Load Sound\Alarm14.wav
				Wait Not MediaPlaying
			Next
			Stop
		EndIf
	EndIf
Return

Label TorqueReader
	Variable Numeric TorqueNM_Mot2
	Variable Numeric MaxTorqueNM
	TorqueNM_Mot2 = Coeff_NM * Slot(204)
	MaxTorqueNM = Max(MaxTorqueNM, TorqueNM_Mot2)
Return

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB

Variable Numeric Slot_SignalScope

Variable Numeric Slot_PEN
Variable Numeric Slot_SolenoidVoltage
Variable Numeric Slot_Emergency
Variable Numeric Slot_Pause
Variable Numeric Slot_Pivot
Variable Numeric Slot_Run_SignalScope_BT


Variable Numeric PenDown_Value
Variable Numeric PenUp_Value

Variable Numeric Rmax
Variable Numeric Rmin
Variable Numeric Coeff_NM

Variable Numeric TorqueForStopCW		' View from motor side (human view)
Variable Numeric TorqueForStopCCW
Variable Numeric TorqueForLogCW
Variable Numeric TorqueForLogCCW

Variable Numeric TorqueLimit

' ====================================================================
'  SLOTS
' ====================================================================
Label SetSlots
	Slot_PEN                =   6 ' The ServoMotor that controls the pen
	Slot_SolenoidVoltage    =   2 ' The solenoid voltage 1000 = +4V / -1000 = -4V
	Slot_Pivot              =  10 ' Pivot out level high
	Slot_Run_SignalScope_BT =  13
	
	Slot_LedR               = 907
	Slot_LedG               = 908
	Slot_LedB               = 909
	
	Slot_Emergency          = 911
	Slot_Pause              = 912
	Slot_SignalScope        = 914
Return


' ====================================================================
'  BUTTONS AND KEYS
' ====================================================================
Label SetButtonsAndKeys
	Button 1 Text InitPositions
	Button InitPositions Color SkyBlue
	
	Button 2 Text EnableMotors
	Button  EnableMotors Text "Enable Motors"
	
	Button 3 Text  DisableMotors
	Button DisableMotors Text "Disable Motors"
	
	Button 4 Text ResetLogCounter
	Button ResetLogCounter Text "Reset Log counter"
	Button ResetLogCounter Color White
	
	Button 7 Text OpenUnLockLockSeq
	Button OpenUnLockLockSeq Text "Open UnLock-Lock.seq"
	Button OpenUnLockLockSeq Color PaleGreen
	
	Button 8 Text TEST
	Button TEST Text "TEST"
	Button TEST Color Aqua
	
	Button  9 Text "M1"
	Button M1 Color Yellow
	Button 10 Text "M2"
	Button M2 Color Yellow
	Button 11 Text "M3"
	Button M3 Color Yellow
	
	Button 14 Text "Temperature"
	Button Temperature Color Salmon
	
	Button 15 Text CloseAll
	Button CloseAll Text "CLOSE ALL"
	Button CloseAll Color Red
	
	Button 16 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 GreenOn
	Button GreenOn Color Yellow
	
	Button 19 Text BlueOn
	Button BlueOn Color MediumBlue
	
	Button 20 Text WhiteOn
	Button GreenOn Color Green
	
	Button 21 Text RgbOff
	Button RgbOff Color Black
	
	Button 26 Text SetCenterPositions
	Button SetCenterPositions Text "Set Center Positions"
	
	Button 27 Text GotoCenterPositions
	Button GotoCenterPositions Text "Goto Center Positions"
	
	Button 29 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
Return


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

' ====================================================================
'  CLOSING EVENTS
' ====================================================================
Label CloseAll
	Gosub SaveVarsAndPositions
	Load CloseApps Theremino_Edito
	Load CloseApps
Return

Label EventStop
	Gosub SaveVarsAndPositions
	Gosub EnableAllButtons
	Window Sizable
	Slot(Slot_SignalScope) = 0
End

' ====================================================================
'  SAVE LOAD and CLEAR VARS and POSITIONS
' ====================================================================
Label SaveVarsAndPositions
	'v1 = Slot(Slot_Destination1)
	'v2 = Slot(Slot_Destination2)
	'v3 = Slot(Slot_Destination3)
	'v4 = Slot(Slot_Destination4)
	v5 = NumCycle
	v6 = ErrorNumber
	Save Vars
Return

Label RestoreVarsAndPositions
	Load Vars
	If v1 <> 0 And v2 <> 0 And v3 <> 0
		'Slot(Slot_Destination1) = Reset
		'Slot(Slot_Destination2) = Reset
		'Slot(Slot_Destination3) = Reset
		'Slot(Slot_Destination4) = Reset
		'Wait Seconds 0.01
		'Slot(Slot_Destination1) = v1
		'Slot(Slot_Destination2) = v2
		'Slot(Slot_Destination3) = v3
		'Slot(Slot_Destination4) = v4
		'Wait Seconds 0.01
	EndIf
	'
	NumCycle = v5
	ErrorNumber = v6
	'
	'Gosub PrintValues
Return

Label ClearVars
	NumCycle = 0
	ErrorNumber = 0
Return

Label ResetLogCounter
	Gosub ClearVars
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 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

' ====================================================================
'  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)
		'
		If SequenceCmd <> ""
			Print " Executing command: " + SequenceCmd + CRLF
			'
			Select SequenceCmd
            	' ---------------------------------------------- End sequence
				Case "EndSeq"
					SequenceIndex = SequenceLength-1
		    	' ---------------------------------------------- ACTIONS
				Case "EnableMotors"     ' enable movement motors
					Gosub EnableMotors
				Case "DisableMotors"    ' disable movement motors
					Gosub DisableMotors
				Case "WaitAllMotors"
					Gosub WaitAllMotors
				Case "CloseAll"
					Gosub CloseAll
				' ---------------------------------------------- PISTONS
				Case "WaitPistonIn"
					Gosub WaitPistonIn
				Case "WaitPistonOut"
					Gosub WaitPistonOut
				' ---------------------------------------------- LED COLORS
				Case "RedON"
					Gosub RedOn
				Case "GreenON"
					Gosub GreenOn
				Case "BlueOn"
					Gosub BlueOn
				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
				' ---------------------------------------------- With parameters
				CaseElse
					s1 = GetSeparatedString(SequenceCmd, 0)
					Select s1
						Case "MoveMot1"
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_Destination1) = v1 * 4096 / 360
						Case "MoveMot2"
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_Destination2) = v1 * 4096 / 360
						Case "MoveMot3"
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_Destination3) = v1 * 4096 / 360
						Case "MoveMot4"
							v1 =  GetSeparatedString(SequenceCmd, 1)
							Slot(Slot_Destination4) = v1 * 4096 / 360
						'
						Case "PenSingleAndWaitSolenoidON"
							TestVolt_TimeSec = GetSeparatedString(SequenceCmd, 1)
							Gosub PenSingleAndWaitSolenoidON
						Case "WaitSolenoidOFF"
							TestVolt_TimeSec = GetSeparatedString(SequenceCmd, 1)
							Gosub WaitSolenoidOFF
						Case "WaitSecurityON"
							TestVolt_TimeSec = GetSeparatedString(SequenceCmd, 1)
							Gosub WaitSecurityON
						Case "WaitSecurityOFF"
							TestVolt_TimeSec = GetSeparatedString(SequenceCmd, 1)
							Gosub WaitSecurityOFF
						'
						Case "Pause"
							Wait Seconds GetSeparatedString(SequenceCmd, 1)
						Case "SignalScope"
							Slot(Slot_SignalScope) = GetSeparatedString(SequenceCmd, 1)
						Case "TorqueForStopCW"
							TorqueForStopCW = GetSeparatedString(SequenceCmd, 1)
						Case "TorqueForStopCCW"
							TorqueForStopCCW = GetSeparatedString(SequenceCmd, 1)
						Case "TorqueForLogCW"
							TorqueForLogCW = GetSeparatedString(SequenceCmd, 1)
						Case "TorqueForLogCCW"
							TorqueForLogCCW = GetSeparatedString(SequenceCmd, 1)
						Case "TorqueLimit"
							TorqueLimit = GetSeparatedString(SequenceCmd, 1)
						CaseElse
							If s1 <> ""
								Controls OpenTextBox
								Print CRLF + " Unrecognized command: " + SequenceCmd + CRLF
								Beep
								Stop
							EndIf
					EndSelect
			EndSelect
			Gosub UpdateButtons
		EndIf
	Next
	Print s1 + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds" + CRLF
Return



' ====================================================================
'   STOP MOTOR
' ====================================================================
Label StopMotor
	' -------------------------------------------
	'  DO NOT CHANGE DELAYS AND INSTRUCTIONS
	' -------------------------------------------
	'Slot(Slot_TorqueLimit) = 1
	'Slot(Slot_Destination) = Slot(Slot_Location)
	'Wait Seconds 1
	'Slot(Slot_Destination) = Slot(Slot_Location)
	'Slot(Slot_TorqueLimit) = 10
	'Wait Seconds 0.1
	'Slot(Slot_TorqueLimit) = 1000
	'' -------------------------------------------
	'Slot(Slot_SignalScope)  = 0
Return


' ====================================================================
'   PRINT LOG
'=====================================================================
Label OpenLogfile
	Load "Files\TestLogDoorLockBT.txt"
Return

Label InitFileHeader
	FileHeader = CRLF
	FileHeader = FileHeader + "                                                        N.m    N.m                 " + CRLF
	FileHeader = FileHeader + "                              Num.                     Trigg.  Max                 " + CRLF
	FileHeader = FileHeader + "   DATE     HH/MM/SS   TIME   Test  Coil -- Angles --  TORQUE TORQUE  Test  Result " + CRLF
	FileHeader = FileHeader + "-----------------------------------------------------------------------------------"
	Print FileHeader + CRLF
	Save StringToFile FileHeader LogFileName
Return

Label PrintLogParameters
	Controls OpenTextBox
	LogLine = PadLeft(Format(NumCycle, "00000"), 7)
	LogLine = LogLine + "  " + Coil
	LogLine = LogLine + PadLeft(Format(Slot(Slot_Position1)), 4)
	LogLine = LogLine + PadLeft(Format(Slot(Slot_Position2)), 4)
	LogLine = LogLine + PadLeft(Format(Slot(Slot_Position3)), 4)
	LogLine = LogLine + PadLeft(Format(TorqueLimit, "0.000"), 8)
	LogLine = LogLine + PadLeft(Format(MaxTorqueNM, "0.000"), 7)
	LogLine = LogLine + "  " + Result
	Gosub UPDATE_FILE
Return

Variable String LogLine
Variable Numeric StartTime
Label UPDATE_FILE
	s1 = ""
	s1 = Format(Now, "dd/MMM/yyyy HH:mm:ss")
	s1 = s1 + " " + PadLeft(Format(ElapsedTime - StartTime, "0.00"), 6)
	s1 = s1 + LogLine
	Print s1 + CRLF
	Save StringToFile s1 LogFileName
	StartTime = ElapsedTime
Return


' ====================================================================
'  UNLOCK-LOCK sequence
' ====================================================================
Variable Numeric NumCycle
Variable Numeric ErrorNumber
Variable String  Coil
Variable String  Result

Label OpenUnLockLockSeq
	Load "Sequences\UnLock-Lock.seq"
Return

Label TEST
	Slot(Slot_Run_SignalScope_BT) = 1000
	Controls OpenTextBox
	StartTime = ElapsedTime
	SequenceFileName = "SEQUENCES\UnLock-Lock.seq"
	Variable Numeric nn
	nn = Input "How many cycles?"
	If nn <> 0
		For NumCycle = 1 To nn
			Slot(Slot_SignalScope) = 1000
			Gosub ExecSequence
			Slot(Slot_SignalScope) = 0
			Wait Seconds 0.5
		Next
	EndIf
	Slot(Slot_Run_SignalScope_BT) = 0
Return

' ====================================================================
'  PISTON
' ====================================================================
Label WaitPistonIn
	If Slot(Slot_Pivot) > 500
		Gosub ALARM
		End
	EndIf
Return

Label WaitPistonOut
	If Slot(Slot_Pivot) < 500
		Gosub ALARM
		End
	EndIf
Return

Label ALARM
	Variable Numeric AL
	For AL = 1 To 1
		Gosub RedOn
		Load "sound/Alarm10b.wav"
		Wait Not MediaPlaying
		Wait Seconds 0.5
	Next
Return

Variable Numeric TestVolt_TimeSec

' ====================================================================
'  SOLENOID
' ====================================================================
Variable Numeric WS1
Variable Numeric WS2

Label PenSingleAndWaitSolenoidON
	For WS2 = 1 To 9e99
		Gosub PenSingle
		For WS1 = 1 To 9e99
			If WS1 > TestVolt_TimeSec * 10
				If WS2 < 3
					Exit 1
				Else
					Gosub ALARM
					End
				EndIf
			EndIf
			If Slot(Slot_SolenoidVoltage) > 700
				Exit 2
			EndIf
			Wait Seconds 0.1
			Gosub TorqueReader
		Next
	Next
Return

Label WaitSolenoidOFF
	For WS1 = 1 To 9e99
		If WS1 > TestVolt_TimeSec * 10
			Gosub ALARM
			End
		EndIf
		If Slot(Slot_SolenoidVoltage) > 400 And Slot(Slot_SolenoidVoltage) < 600
			Exit
		EndIf
		Wait Seconds 0.1
		Gosub TorqueReader
	Next
Return

Label WaitSecurityON
	For WS1 = 1 To 9e99
		If WS1 > TestVolt_TimeSec * 10
			Gosub ALARM
			End
		EndIf
		If Slot(Slot_SolenoidVoltage) < 300
			Exit
		EndIf
		Wait Seconds 0.1
		Gosub TorqueReader
	Next
Return

Label WaitSecurityOFF
	For WS1 = 1 To 9e99
		If WS1 > TestVolt_TimeSec * 10
			Gosub ALARM
			End
		EndIf
		If Slot(Slot_SolenoidVoltage) > 400 And Slot(Slot_SolenoidVoltage) < 600
			Exit
		EndIf
		Wait Seconds 0.1
		Gosub TorqueReader
	Next
Return


'EXEC DestX = 12
