' ====================================================================
'     Name: WXM_DoorLockBTSM29_13.txt                Date: 12 Nov 2020
'    Autor: Leonardo DE PALO
'    Scope: Test DoorLock using smart phone Blue Tooth and MOTOR SM29
'           with MODBUS protocol.
' Hardware: Standard Master.
' ====================================================================

Option Speed 9
'Load "media\images\FeeTech SM29.png"
Load "media\images\KnobBTEster.jpg"
'Wait Seconds 1
'Button  sound\gong.wav
' -------------------------------------- Initialize SLOTS, BUTTONS and KEYS
Gosub SetSlots
Gosub SetButtonsAndKeys
'Gosub DisableAllButtons

' -------------------------------------- Set Motor Parameters
Slot(Slot_ModbusBaudRate)    = 2    ' 0-9    (0=256000 1=128000 2=115200)
Slot(Slot_ModbusReturnDelay) = 500  ' 0-500  (500)
Slot(Slot_MinimumAngleLimit) = 0    ' 0-4095 (0)    0=Dest. from -30720 to +30720
Slot(Slot_MaximumAngleLimit) = 4095 ' 0-4095 (4095) 0=Dest. from -30720 to +30720
Slot(Slot_PositionOffset)    = 0    ' -2048 to +2047 (0)
Slot(Slot_WorkMode)          = 0    ' 0-3 (0)
Slot(Slot_Position_P)        = 50   ' 0-255 (32)
Slot(Slot_Position_D)        = 50   ' 0-255 (32)
Slot(Slot_Position_I)        = 50   ' 0-255 (0)
Slot(Slot_Velocity_P)        = 10   ' 0-255 (10)
Slot(Slot_Velocity_I)        = 10   ' 0-255 (10)

Slot(Slot_TorqueEnable) = 1    ' 0-254 (Max=1 or 254 ?)
Slot(Slot_AccLimit)     = 255  ' 1-255 (0 not working)
Slot(Slot_SpeedLimit)   = 255  ' 0-255
Slot(Slot_TorqueLimit)  = 1000 ' 0-1000


' -------------------------------------- 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 =   4095
Rmin =    2

' -------------------------------------- Pen Up / Down set up
PenDown_Value  = 235
PenUp_Value    = 350

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

' -------------------------------------- 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"
Load "Theremino_HAL\Theremino_Filters\Theremino_Filters.exe"
'Load "Theremino_HAL\Theremino_WaveGenerator.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
'Wait Seconds 1
Slot(Slot_Destination) = 4093
Gosub GreenOn


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

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_ModbusServerID    ' 411 40011
Variable Numeric Slot_ModbusBaudRate    ' 412 40012 (0-9)(0=256000 1=128000 2=115200)
Variable Numeric Slot_ModbusReturnDelay ' 413 40013 (0-500) (500)
Variable Numeric Slot_MinimumAngleLimit ' 414 40014 (0=Dest. from -30720 to +30720)
Variable Numeric Slot_MaximumAngleLimit ' 415 40015 (0=Dest. from -30720 to +30720)
Variable Numeric Slot_PositionOffset    ' 416 40016 (-2048 to +2047) (0)
Variable Numeric Slot_WorkMode          ' 417 40017 (0-3) (0)
Variable Numeric Slot_Position_P        ' 418 40018 (0-255) (32) (changed to 50)
Variable Numeric Slot_Position_D        ' 419 40019 (0-255) (32) (changed to 50)
Variable Numeric Slot_Position_I        ' 420 40020 (0-255) (0)  (changed to 50)
Variable Numeric Slot_Velocity_P        ' 421 40021 (0-255) (10)
Variable Numeric Slot_Velocity_I        ' 422 40022 (0-255) (10)

Variable Numeric Slot_Destination       ' 429 40129 (-30720 to +30720)
Variable Numeric Slot_TorqueEnable      ' 430 40130 (0-254) (Max=1 or 254 ?)
Variable Numeric Slot_AccLimit          ' 431 40131 (1-255) (255) (0 not working)
Variable Numeric Slot_SpeedLimit        ' 432 40132 (0-255) (255)
Variable Numeric Slot_TorqueLimit       ' 433 40133 (0-1000)(1000)

Variable Numeric Slot_Location			' 458 40258
Variable Numeric Slot_Speed           	' 459 40259
Variable Numeric Slot_MotorVoltage      ' 460 40260
Variable Numeric slot_Voltage           ' 461 40261
Variable Numeric Slot_Temperature       ' 462 40262
Variable Numeric Slot_ChangeDirPulse    ' 463 40263
Variable Numeric Slot_Current           ' 464 40264

Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB

Variable Numeric Slot_CurrentFiltered
Variable Numeric Slot_SignalScope

Variable Numeric Slot_LocationDegree
Variable Numeric Slot_TorqueNM

Variable Numeric Slot_PEN
Variable Numeric Slot_Emergency

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

Label SetSlots
	Slot_PEN               =   1 ' Slot pen
	
	Slot_Location          = 458 ' Get Location
	Slot_Speed             = 459 ' Get Speed
	Slot_MotorVoltage      = 460 ' Get Motor Voltage
	slot_Voltage           = 461 ' Get Voltage
	Slot_Temperature       = 462 ' Get Temperature
	Slot_ChangeDirPulse    = 463 ' Get Change dir pulse
	Slot_Current           = 464 ' Get Current
	
	Slot_Destination       = 429  ' Set Destination         0-4095
	Slot_TorqueEnable      = 430  ' Set Torque Enable       0-254   (Max=1 or 254 ?)
	Slot_AccLimit          = 431  ' Set Acc. Limit          0-255   (0 not working ?)
	Slot_SpeedLimit        = 432  ' Set Speed Limit         0-255
	Slot_TorqueLimit       = 433  ' Set Torque Limit        0-1000
	
	Slot_ModbusServerID    = 411  ' Set Modbus Server ID    1-247
	Slot_ModbusBaudRate    = 412  ' Set Modbus BaudRate     0-9
	Slot_ModbusReturnDelay = 413  ' Set Modbus Return Delay 0-500
	Slot_MinimumAngleLimit = 414  ' Set Minimum Angle Limit 0-4095
	Slot_MaximumAngleLimit = 415  ' Set Maximum Angle Limit 0-4095
	Slot_PositionOffset    = 416  ' Set Position Offset     -2048 +2047
	Slot_WorkMode          = 417  ' Set Work Mode           0-3
	Slot_Position_P        = 418  ' Set Position P          0-255 (32)
	Slot_Position_D        = 419  ' Set Position D          0-255 (32)
	Slot_Position_I        = 420  ' Set Position I          0-255 (32)
	Slot_Velocity_P        = 421  ' Set Velocity P          0-255 (10)
	Slot_Velocity_I        = 422  ' Set Velocity I          0-255 (10)
	
	Slot_LedR              = 907
	Slot_LedG              = 908
	Slot_LedB              = 909
	
	Slot_Emergency         = 911
	Slot_CurrentFiltered   = 913
	Slot_SignalScope       = 914
	
	Slot_LocationDegree    = 950
	Slot_TorqueNM          = 951
Return


' ====================================================================
'  BUTTONS AND KEYS
' ====================================================================
Label SetButtonsAndKeys
	Button  1 Text  Home R
	Button  1 Color SkyBlue
	
	Button  3 Text Enable Motors
	Button  4 Text Disable Motors
	
	Button  6 Text Open Log file
	
	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 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  YellowOn
	Button 18 Color Yellow
	Button 19 Text  GreenOn
	Button 19 Color Green
	Button 20 Text  BluON
	Button 20 Color Blue
	Button 21 Text  WhiteOn
	Button 21 Color White
	Button 22 Text  RgbOff
	Button 22 Color Black
	
	'Button 23 Text  set-up TURNKNOB
	'Button 23 Color Aqua
	Button 24 Text  TURNKNOB
	Button 24 Color Aqua
	Button 25 Text  "Temperature"
	Button 25 Color Salmon
	
	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
	
	' -------------------------------------- KEYS
	Key Right		Gosub ExecKeys
	Key Left 		Gosub ExecKeys
Return


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

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

' ====================================================================
'  CLOSING EVENTS
' ====================================================================
Label CLOSE ALL
	Gosub SaveVarsAndPositions
	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_Destination)
	v2 = NumCycle
	v3 = ErrorNumber
	Save Vars
Return

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

Label ClearVars
	NumCycle = 0
	ErrorNumber = 0
Return

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

Label R_0
	Slot (Slot_Destination) = 0
Return

Label R_90
	Slot (Slot_Destination) = 1023
Return

Label R_180
	Slot (Slot_Destination) = 2047
Return

Label R_270
	Slot (Slot_Destination) = 3071
Return

Label R_360
	Slot (Slot_Destination) = 4095
Return

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

Label set-up TURNKNOB
	'For NumCycle = NumCycle To 9e99
	SequenceFileName = "SEQUENCES\SetUpTurnKnob.seq"
	Gosub ExecSequence
	'Next
Return

Label TURNKNOB
	StartTime = ElapsedTime
	Controls OpenTextBox
	For NumCycle = NumCycle To 9e99
		Slot(Slot_SignalScope) = 1000
		SequenceFileName = "SEQUENCES\TestTurnKnob.seq"
		Gosub ExecSequence
		'Print "Cycle number:" + NumCycle + CRLF
		Gosub UPDATE_FILE
		' ----------------------------------------- Max temperature test
		Button 25 Text "MotorTemp: " + Format(Slot(Slot_Temperature), "0")
		If Slot(Slot_Temperature) > 55    '55
			For v1 = 1 To 3
				Load Sound\Alarm10b.wav
				Wait Not MediaPlaying
			Next
			Stop
		EndIf
	Next
Return

Label Enable Motors
	Slot Slot_TorqueEnable = 1
Return

Label Disable Motors
	Slot Slot_TorqueEnable = 0
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
	Print Result
	Gosub RED ALARM
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

' ====================================================================
'  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 PEN DOWN
	Wait Seconds 0.35
	Gosub PEN UP
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
	' Select
	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 LCase(SequenceCmd)
            ' ---------------------------------------------- End sequence
				Case "endseq"
					SequenceIndex = SequenceLength-1
		    ' ---------------------------------------------- ACTIONS
				Case "enablemotors"     ' enable movement motors
					Gosub Enable Motors
				Case "disablemotors"    ' disable movement motors
					Gosub Disable Motors
				Case "waitallmotors"
					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 "penup"            ' move pen up
					Gosub PEN UP
				Case "pendown"          ' move pen down
					Gosub PEN DOWN
				Case "pensingle"        ' single click
					Gosub PEN SINGLE
			' ---------------------------------------------- With parameters
				CaseElse
					s1 = GetSeparatedString(SequenceCmd, 0)
					Select LCase(s1)
						Case "move_r"
							Slot(Slot_Destination) = GetSeparatedString(SequenceCmd, 1)
							Gosub WaitAllMotors
						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)
						CaseElse
							If s1 <> ""
								Controls OpenTextBox
								Print CRLF + " Unrecognized command: " + SequenceCmd + CRLF
								Beep
								Stop
							EndIf
					EndSelect
			EndSelect
		EndIf
	Next
	SequenceFileName = Replace(SequenceFileName,".seq","")
	'Print SequenceFileName + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds" + CRLF
Return

' ====================================================================
'  HOME  R
' ====================================================================
Label Home R
	Slot(Slot_Destination) = 4095
	'Gosub PrintValues
	Button 6 Color SkyBlue
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_Destination = Limit(Slot(Slot_Destination) + JogSpeed, Rmin, Rmax)
	EndIf
	If Key("Left")
		Slot Slot_Destination = Limit(Slot(Slot_Destination) - JogSpeed, Rmin, Rmax)
	EndIf
	
	Gosub PrintValues
Return


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



' ====================================================================
'   Wait until all the Motors are stopped
'=====================================================================
Variable Numeric WaitMot1
Label WaitAllMotors
	Wait Seconds 0.1 ' <<< ensures that motors are really started
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(Slot_Location) - Slot(Slot_Destination)) < 15
			Exit
		EndIf
		Wait Seconds 0.005
		Gosub TorqueTest
	Next
Return

' ====================================================================
'   Torque Test
'=====================================================================
Variable Numeric TrigLevelTorque
Variable Numeric TrigStopTorque
Variable Numeric TorqueNM
Variable String Direction
Label TorqueTest
	TorqueNM = Coeff_NM * Slot(Slot_CurrentFiltered)
	'
	Slot (Slot_TorqueNM) = TorqueNM
	Slot (Slot_LocationDegree) = Slot(Slot_Location) * 360 / 4096
	'
	If Slot(Slot_Speed) > 0
		Direction = " CW: "
		TrigLevelTorque = TorqueForLogCW
		TrigStopTorque = TorqueForStopCW
		
	Else
		Direction = "CCW: "
		TrigLevelTorque = TorqueForLogCCW
		TrigStopTorque = TorqueForStopCCW
	EndIf
	'
	If TorqueNM > TrigLevelTorque
		Gosub PrintLogParameters
	EndIf
	If TorqueNM > TrigStopTorque
		Gosub PrintStopParameters
		Gosub StopMotor
		Gosub RedOn
		Stop
	EndIf
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 PrintStopParameters
	Controls OpenTextBox
	Print " ----------------------------- STOP LIMIT REACHED -- " + CRLF
	Gosub PrintLogParameters
Return

Label PrintLogParameters
	Controls OpenTextBox
	LogLine = PadLeft(Format(NumCycle), 7)
	LogLine = LogLine + "  " + Direction + Format(Slot(Slot_LocationDegree), "000")
	LogLine = LogLine + "   " + Format(TrigLevelTorque, "0.000")
	LogLine = LogLine + "  " + Format(TorqueNM, "0.000")
	LogLine = LogLine + "   " + Format(TrigStopTorque, "0.000")
	LogLine = LogLine + "  " + Format(TorqueNM, "0.000")
	Print LogLine +  CRLF
	Gosub UPDATE_FILE
Return

Label InitFileHeader
	FileHeader = CRLF
	FileHeader = FileHeader + "                                              N.m    N.m    N.m.   N.m.  " + CRLF
	FileHeader = FileHeader + "                                             TRIGG.        TRIGG.        " + CRLF
	FileHeader = FileHeader + "                     EXEC         DIRECTION  LEVEL  ACTUAL  STOP  ACTUAL " + CRLF
	FileHeader = FileHeader + "   DATE     HH/MM/SS TIME  CYCLE    ANGLE    TORQUE TORQUE TORQUE TORQUE " + CRLF
	FileHeader = FileHeader + "-------------------------------------------------------------------------" + CRLF
	Save StringToFile FileHeader LogFileName
Return

Label Open Log file
	Load "Files\" + LogFileName
Return

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


'EXEC DestX = 12
