' ====================================================================
'     Name: WXM_CNC_Vslot_Med_16_8.txt               Date: 15 Jul 2021
'   Author: Leonardo DE PALO Ph.D. & Roberto Fasciani Ms.C.
'    Scope: Two gripper PWM, rotating with Protocol1 of FeeTech.
' Hardware: Standard Master e SM29.
' note: Cable color right side:
'        Shield: Ground
'         White: Step
'           Red: Dir
'         Black: Right limit
' ====================================================================
'
' ATTENZIONE: C'e' una illogicita' nel colore del cavo Stp/Dir.
' Deve essere Bianco e rosso.
' Rosso va al "+" e il bianco al "-"
'
Option Speed 9
Option GlobalKeys Enabled
'Window MinSize
Load "media\images\CNC_Vslot_Med.jpg"

' ====================================================================
'  INITIALIZATIONS
' ====================================================================
' -------------------------------------- Set slots
Slot_LedR            = 907
Slot_LedG            = 908
Slot_LedB            = 909
Slot_Air             = 911
Slot_WhiteLight      = 912

Slot_SignalScope     = 914

Slot_ClampM1         = 504
Slot_ClampM2         = 604

Gosub InitCoreXY
Gosub SetButtonsAndKeys
Gosub RestoreVarsAndPositions
Gosub ShowSequenceName

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

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

' -------------------------------------- Torque limit
TorqueLimitRotation = 9e99

' -------------------------------------- Positions
'A_PickVib  = 1   * 4096 / 360
'B_PickVib  = 90  * 4096 / 360
'A_PlaceCNC = 180 * 4096 / 360
'B_PlaceCNC = 235 * 4096 / 360
'A_PickCNC  = 270 * 4096 / 360
'A_Release  = 359 * 4096 / 360

 '====================================================================
'  LOAD APPS
' ====================================================================
Load Theremino_RS485\Theremino_RS485.exe
Load Theremino_HAL\Theremino_HAL.exe
'Load DigiClampM1\DigiClampM1.exe
'Load DigiClampM2\DigiClampM2.exe
Load Theremino_HAL\Theremino_SlotViewer.exe
'Load Theremino_HAL\Theremino_Signalscope_DigiClamp_1.exe
'Load Theremino_HAL\Theremino_SignalScope_DigiClamp_2.exe
'Load Theremino_HAL\Theremino_SignalScope_MOTOR_TORQUE.exe
'Load Theremino_HAL\Theremino_WaveGenerator.exe

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

' ====================================================================
'  START
' ====================================================================
Slot(Slot_Speed) = 1000
Controls ClearTextBox

Button RunSequence Disabled
Gosub InitMotors

'Gosub CheckMotorTemperature

'Gosub ReleaseM1
'Gosub ReleaseM2
Wait Seconds 0.5
'Gosub HomeRotation

Load Hide                        'need to found the best position

'Gosub HomeXY
Button RunSequence Enabled

Option GlobalKeys Disabled
Gosub BlueOn
Gosub MainLoop

Stop

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

Label UpdateButtons
	Button Rotation Text "Rot.=" + Format(Slot(Slot_Destination1) * 360 / 4096, "0.0")
Return

					 										'

' ====================================================================
'  BUTTONS
' ====================================================================
Label SetButtonsAndKeys
	
	'button 1 is in CoreXY section
	
	Button 2 Text HomeRotation
	Button HomeRotation Text "Home Rotation"
	Button HomeRotation Color SkyBlue
	
	Button 3 Text EnableRotation
	Button EnableRotation Text "Enable Rotation"
	
	Button 4 Text DisableRotation
	Button DisableRotation Text "Disable Rotation"
	
	Button 5 Text EditSequence
	Button EditSequence Text "Edit sequence"
	Button EditSequence Color PaleGreen
	
	Button 6 Text RunSequence
	Button RunSequence Text "RUN SEQUENCE"
	Button RunSequence Color Aqua
	
	Button 7 Text SelectSequence
	Button SelectSequence Text "Select Sequence"
	Button SelectSequence Color PowderBlue
	
	Button 10 Text "Rotation"
	Button Rotation Color Yellow
	Button Rotation Text "Rotation"
	
	Button 11 Text "BtnDestX"
	Button BtnDestX Text "X = ?"
	Button BtnDestX Color Yellow
	
	Button 12 Text "BtnDestY"
	Button BtnDestY Text "Y = ?"
	Button BtnDestY Color Yellow
	
	Button 13 Text ResetLogCounter
	Button ResetLogCounter Text "Reset Log counter"
	Button ResetLogCounter Color White
	
	Button 14 Text "MotorTemp"
	Button MotorTemp 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 MagentaOn
	Button MagentaOn Color Magenta
	
	Button 19 Text YellowOn
	Button YellowOn Color Yellow
	
	Button 20 Text GreenOn
	Button GreenOn Color Green
	
	Button 21 Text BlueOn
	Button BlueOn Color MediumBlue
	
	Button 22 Text WhiteOn
	Button GreenOn Color Green
	
	Button 23 Text RgbOff
	Button RgbOff Color Black
	
	Button 25 Text ClampM1Toggle
	Button ClampM1Toggle Text "Clamp M1"
	Button ClampM1Toggle Color LightBlue
	
	Button 26 Text ClampM2Toggle
	Button ClampM2Toggle Text "Clamp M2"
	Button ClampM2Toggle Color LightBlue
	
	Button 27 Text AirToggle
	Button AirToggle Text "Air ON"
	Button AirToggle Color LightCyan
	
	Button 28 Text WhiteLightToggle
	Button WhiteLightToggle Text "WhiteLight ON"
	Button WhiteLightToggle Color LightCyan
	
	Button 29 Text  OpenLogfile
	Button OpenLogfile Text "Open Log file"
	Button OpenLogfile Color PaleGreen
	
	Button 31 Text SetCenterPositions
	Button SetCenterPositions Text " WARNING" + CRLF + "Set Center Positions"
	Button SetCenterPositions Color DarkOrange
	
	Button 32 Text GotoCenterPositions
	Button GotoCenterPositions Text "Goto Center Positions"
Return

' ====================================================================
'  MOTORS
' ====================================================================
Variable Numeric Slot_Destination1
Variable Numeric Slot_Position1

Label InitMotors
	' --------------------------------- Init Destination Slots
	Slot_Destination1 = 101
	' --------------------------------- Init Position Slots
	Slot_Position1 = 102
	' --------------------------------- All to 0 to ensure resending values
	Slot(101) = Slot(101) - 1
	Slot(120) = 0
	Slot(121) = 0
	Slot(122) = 0
	Wait Seconds 0.1
	Slot(101) = Slot(101) + 1
	' --------------------------------- Torque 0-1000 to motors 1-4
	Slot(120) = 800
	' --------------------------------- Acceleration 0-255 to motors 1-4
	Slot(121) = 50
	' --------------------------------- Speed 0-10000 to motors 1-4
	Slot(122) = 3000
Return

Label EnableRotation
	Slot(123) = 1
	Wait Seconds 0.1
Return

Label DisableRotation
	Slot(123) = 0
Return

Label SetCenterPositions
	'Slot(123) = 180
	'Wait Seconds 0.5
	'Gosub EnableRotation
	'Gosub CopyPositionsToDestinations
Return

Label CopyPositionsToDestinations
	Slot(Slot_Destination1) = Slot(Slot_Position1)
Return

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

Variable Numeric MotorsTemperature
Label TestMotorsTemperature
	MotorsTemperature = Slot(106)
Return

Label TorqueReader
	' ------------------------------------------------- read the torque
	Variable Numeric TorqueNM_Rotation
	TorqueNM_Rotation = Coeff_NM * Slot(104)
	If TorqueNM_Rotation >= 1024
		TorqueNM_Rotation = TorqueNM_Rotation - 1024
	EndIf
Return

Label TorqueTester
	Gosub TorqueReader
	' ------------------------------------------------- read the torque
	Variable Numeric MaxTorqueRotation
	TorqueNM_Rotation = Coeff_NM * Slot(104)
	MaxTorqueRotation = Max(MaxTorqueRotation, TorqueNM_Rotation)
	' ------------------------------------------------- test limits
	If MaxTorqueRotation > TorqueLimitRotation
		Gosub ALARM
		Result = Result + CRLF + " Torque limit Rotation    "
		Gosub UpdateResultTorque
		Result = Result + CRLF
		Gosub UpdateResult
		Result = Result + CRLF
	EndIf
Return

Label WaitMotor1
	Variable Numeric WM1
	Wait Seconds 0.2 ' <<< ensures that motors are really started
	For WM1 = 1 To Infinity
		If Abs(Slot(103)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
		Gosub TorqueTester
	Next
	Wait Seconds 0.1
	Gosub TorqueTester
Return

Label TestRaw
	Variable Numeric TR1
	For TR1 = 1 To Infinity
		If Slot(Slot_RawSensor) < 500
			Exit
		EndIf
		Gosub YellowOn
		Load sound\beep.wav
		Wait Seconds 0.5
		Load sound\PleaseInsertMushroom.m4a
		Wait Not MediaPlaying
	Next
	Gosub GreenOn
Return


Label CheckMotor1AndReinit
	If Abs(Slot(103)) > 1
		Gosub InitMotors
		Load sound\beep.wav
	EndIf
Return

Label HomeRotation
	Gosub EnableRotation
	Slot(Slot_Destination1) = 183 * 4096 / 360
	Gosub WaitMotor1
Return

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

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

Variable Numeric Slot_SignalScope

Variable Numeric Slot_Emergency

Variable Numeric Rmax
Variable Numeric Rmin
Variable Numeric Coeff_NM

Variable Numeric TorqueLimitRotation

Variable Numeric Slot_ClampM1
Variable Numeric Slot_ClampM2

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

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

' ====================================================================
'  SAVE LOAD and CLEAR VARS and POSITIONS
' ====================================================================
Label SaveVarsAndPositions
	v1 = NumCycle
	v2 = ErrorNumber
	v3 = DestX
	v4 = DestY
	S1 = SequenceName
	S2 = LogName
	Save Vars
	'Print CRLF + "SAVED SEQUENCENAME = " + s1 + CRLF
Return

Label RestoreVarsAndPositions
	Load Vars
	NumCycle = v1
	ErrorNumber = v2
	DestX = v3
	DestY = v4
	SequenceName = S1
	LogName = S2
Return

Label ClearVars
	NumCycle = 0
	ErrorNumber = 0
Return

Label ResetLogCounter
	Gosub ClearVars
Return

' ====================================================================
'  LED COLORS
' ====================================================================
Label RedOn
	Slot(Slot_LedR) = 1000
	Slot(Slot_LedG) = 0
	Slot(Slot_LedB) = 0
Return

Label MagentaOn
	Slot(Slot_LedR) = 1000
	Slot(Slot_LedG) = 0
	Slot(Slot_LedB) = 1000
Return

Label YellowOn
	Slot(Slot_LedR) = 1000
	Slot(Slot_LedG) = 400
	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

' =====================================================================
'  COMPRESSED AIR MANAGEMENT and WHITE LIGHT
' =====================================================================
Label AirToggle
	If Slot(Slot_Air) = 0
		Gosub AirON
	Else
		Gosub AirOFF
	EndIf
Return

Label AirON
	Slot(Slot_Air) = 1000
	Button AirToggle Text "Air OFF"
	Button AirToggle Color PowderBlue
Return

Label AirOFF
	Slot(Slot_Air) = 0
	Button AirToggle Text "Air ON"
	Button AirToggle Color LightCyan
Return

Label WhiteLightToggle
	If Slot(Slot_WhiteLight) = 0
		Gosub WhiteLightON
	Else
		Gosub WhiteLightOFF
	EndIf
Return

Label WhiteLightON
	Slot Slot_WhiteLight = 1000
	Button WhiteLightToggle Text "WhiteLight OFF"
	Button WhiteLightToggle Color PowderBlue
Return

Label WhiteLightOFF
	Slot Slot_WhiteLight = 0
	Button WhiteLightToggle Text "WhiteLight ON"
	Button WhiteLightToggle Color LightCyan
Return

' =====================================================================
'  DigiClamp ACTIONS
' =====================================================================

Label ClampM1Toggle
	If Slot(Slot_ClampM1) = 0
		Gosub ClampM1
	Else
		Gosub ReleaseM1
	EndIf
Return

Label ClampM1
	Slot(Slot_ClampM1) = 1000
	Button ClampM1Toggle Text "Release M1"
	Button ClampM1Toggle Color PowderBlue
Return

Label ReleaseM1
	Slot(Slot_ClampM1) = 0
	Button ClampM1Toggle Text "Clamp M1"
	Button ClampM1Toggle Color Cyan
Return

Label ClampM2Toggle
	If Slot(Slot_ClampM2) = 0
		Gosub ClampM2
	Else
		Gosub ReleaseM2
	EndIf
Return

Label ClampM2
	Slot(Slot_ClampM2) = 1000
	Button ClampM2Toggle Text "Release M2"
	Button ClampM2Toggle Color PowderBlue
Return

Label ReleaseM2
	Slot(Slot_ClampM2) = 0
	Button ClampM2Toggle Text "Clamp M2"
	Button ClampM2Toggle Color Cyan
Return

' =====================================================================
'  SEQUENCES
' =====================================================================
Label InitSequencePaths
	Variable String EditorFolder
	Variable String EditorApp
	Variable String SequencePathAndName
	Variable String SequenceName
	'
	EditorFolder = ".\Theremino_HAL\Theremino_Editor\"
	'
	EditorApp = EditorFolder + "Theremino_Editor.exe"
	'
	SequencePathAndName = EditorFolder + "Sequences\" + SequenceName
Return

Label EditSequence
	Gosub InitSequencePaths
	If FileExists(SequencePathAndName)
		Load EditorApp + " " + SequencePathAndName
	EndIf
Return

Label SelectSequence
	Gosub InitSequencePaths
	s1 = SelectFile(EditorFolder + "Sequences\")
	If s1 <> ""
		SequenceName = s1
		LogName = "Files\Logs\" + Replace(SequenceName, ".seq", ".txt")
		Gosub ShowSequenceName
		Gosub EditSequence
	EndIf
Return

Label ShowSequenceName
	Button SelectSequence Text "Sequence" + CRLF + SequenceName
Return

' ====================================================================
'  RUN sequence
' ====================================================================
Label RunSequence
	Gosub InitMotors
	Gosub DisableAllButtons
	Gosub ReleaseM1
	Gosub ReleaseM2
	Gosub HomeXY
	Button EditSequence Enabled
	Button OpenLogfile Enabled
	Button CloseAll Enabled
	Button Emergency Enabled
	Button Rotation Enabled
	Button MotorTemp Enabled
	Button BtnDestX Enabled
	Button BtnDestY Enabled
	Variable Numeric ExitFromSequenceLoop
	'
	Slot(Slot_SignalScope) = 1000
	Controls OpenTextBox
	StartTime = ElapsedTime
	Gosub InitSequencePaths
	Gosub InitFileHeader
	'
	ExitFromSequenceLoop = 0
	SequenceToExecute = SequencePathAndName
	For NumCycle = NumCycle To Infinity
		Slot(Slot_SignalScope) = 1000
		Result = ""
		MaxTorqueRotation = 0
		Gosub ExecSequence
		Gosub UpdateLog
		Slot(Slot_SignalScope) = 0
		Wait Seconds 0.5
		If ExitFromSequenceLoop > 0
			Exit
		EndIf
	Next
	Slot(Slot_SignalScope) = 0
	Gosub EnableAllButtons
Return

' ====================================================================
'  SEQUENCE LOADING AND EXECUTION
' ====================================================================
Variable String SequenceToExecute
Variable String Sequence
Variable String SequenceCmd
Variable Numeric SequenceIndex
Variable Numeric SequenceLength

Label ExecSequence
	Load Sequence SequenceToExecute
	If Len(Sequence) = 0
		Controls OpenTextBox
		Print CRLF + " Sequence file not found: " + SequenceToExecute
		Beep
		Stop
	EndIf
	'
	Print CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)
	Print " started at " + Format(Now, "HH:mm:ss") + CRLF + Chars("-", 50) + CRLF
	' ----------------------------------------------------------
	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
		' ------------------------------------------------------ Do not print lines containing ''
		If GetSeparatedStringCount(SequenceCmd, "''") < 2
			Print " Exec: " + SequenceCmd + CRLF
		EndIf
		' ------------------------------------------------------ Trim and remove comments
		SequenceCmd = Trim(RemoveComments(SequenceCmd))
		' ------------------------------------------------------
		If SequenceCmd <> ""
			Select GetSeparatedString(SequenceCmd, 0)
            	' ---------------------------------------------- End sequence
				Case "EndSeq"
					SequenceIndex = SequenceLength-1
				' ---------------------------------------------- Event STOPd sequence
				Case "EventStop"
					Gosub EventStop
				' ---------------------------------------------- STOP
				Case "Stop"
					ExitFromSequenceLoop = 1
					Exit
		    	' ---------------------------------------------- ACTIONS
				Case "TestRaw"
					Gosub TestRaw
				Case "EnableRotation"
					Gosub EnableRotation
				Case "DisableRotation"
					Gosub DisableRotation
				Case "WaitMotor1"
					Gosub WaitMotor1
				Case "CloseAll"
					Gosub CloseAll
				Case "HomeXY"
					Gosub HomeXY
				' ---------------------------------------------- DigiClamp commands
				Case "ClampM1"
					Gosub ClampM1
				Case "ClampM2"
					Gosub ClampM2
				Case "ReleaseM1"
					Gosub ReleaseM1
				Case "ReleaseM2"
					Gosub ReleaseM2
				' ---------------------------------------------- Air compressed IR
				Case "AirON"
					Gosub AirON
				Case "AirOFF"
					Gosub AirOFF
				' ---------------------------------------------- White light
				Case "WhiteLightON"
					Gosub WhiteLightON
				Case "WhiteLightOFF"
					Gosub WhiteLightOFF
				' ---------------------------------------------- LED COLORS
				Case "RedON"
					Gosub RedOn
				Case "MagentaON"
					Gosub MagentaOn
				Case "YellowON"
					Gosub YellowOn
				Case "GreenON"
					Gosub GreenOn
				Case "BlueOn"
					Gosub BlueOn
				Case "WhiteOn"
					Gosub WhiteOn
				Case "RgbOFF"
					Gosub RgbOff
				' ---------------------------------------------- With parameters
				Case "MoveRotation"
					v1 =  GetSeparatedString(SequenceCmd, 1)
					Slot(Slot_Destination1) = v1 * 4096 / 360
					Gosub WaitMotor1
				Case "MoveSlowXY"
					MoveSlowXY_DestX = GetSeparatedString(SequenceCmd, 1)
					MoveSlowXY_DestY = GetSeparatedString(SequenceCmd, 2)
					MoveSlowXY_Speed = GetSeparatedString(SequenceCmd, 3)
					Gosub MoveSlowXY
				Case "Pause"
					Wait Seconds GetSeparatedString(SequenceCmd, 1)
				Case "SignalScope"
					Slot(Slot_SignalScope) = GetSeparatedString(SequenceCmd, 1)
				Case "TorqueLimitRotation"
					TorqueLimitRotation = GetSeparatedString(SequenceCmd, 1)
				CaseElse
					Controls OpenTextBox
					Print CRLF + " Unrecognized command: " + SequenceCmd + CRLF
					Beep
					Stop
			EndSelect
			Gosub UpdateButtons
		EndIf
	Next
	Print Chars("-", 50) + CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)
	Print " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " sec" + CRLF
Return



' ====================================================================
'   PRINT LOG
'=====================================================================
Variable String LogName
Variable String FileHeader
Variable Numeric NumCycle
Variable Numeric ErrorNumber
Variable String  Result

Label OpenLogfile
	Load LogName
Return

Label InitFileHeader
	FileHeader = CRLF
	FileHeader = FileHeader + "    TEST ROBOT W1     Num  Durat.         " + CRLF
	FileHeader = FileHeader + "    Date     Time    Cycle  sec.   RESULT " + CRLF
	FileHeader = FileHeader + "-----------------------------------------"
	Save StringToFile FileHeader LogName
Return

Variable String LG1
Variable Numeric StartTime

Label UpdateLog
	Controls OpenTextBox
	LG1 = ""
	LG1 = LG1 + Format(Now, "yyyy/MM/dd HH:mm:ss")
	LG1 = LG1 + PadLeft(Format(NumCycle, "00000"), 7)
	LG1 = LG1 + PadLeft(Format(ElapsedTime - StartTime, "0.00"), 6)
	If Result <> ""
		ErrorNumber = ErrorNumber + 1
		LG1 = LG1 + "   FAIL " + ErrorNumber
		LG1 = LG1 + " (" + Format(100 * ErrorNumber / NumCycle, "0.0") + "%)"
		LG1 = CRLF + LG1 + Result + CRLF
	Else
		LG1 = LG1 + "   PASS"
	EndIf
	Save StringToFile LG1 LogName
	Print CRLF + LG1 + CRLF + CRLF
	StartTime = ElapsedTime
Return

Label UpdateResult
	Result = Result + "Rot.=" + Format(Slot(Slot_Position1) * 360 / 4096, "0.0") + "° "
Return

Label UpdateResultTorque
	Result = Result + "TorqueMax=" + Format(MaxTorqueRotation, "0.0")
	Result = Result + "(" + Format(TorqueLimitRotation, "0.0") + ")"
Return


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

Stop                                                                   '
Stop                                                                   '
Stop                                                                   '
Stop                                                                   '
'    CORE XY SECTION
Stop                                                                   '
Stop                                                                   '
Stop                                                                   '
Stop                                                                   '

Label InitCoreXY
	' -------------------------------------- Options
	KeyJog_Speed = 500
	
	' -------------------------------------- Software limits
	DestxMax =  1720
	DestxMin =     1
	DestyMax =   690
	DestyMin =    1
	
	' ----------------------------------------- STEPPER SLOTS
	Slot_MotorA        =  1			'1_TabTesterXY pin  1 STEPPER A
	Slot_MotorB        =  3			'1_TabTesterXY pin  3 STEPPER B
	Slot_SensorX       =  7			'1_TabTesterXY pin  7 DIG INPUT Limit X
	Slot_SensorY       =  8			'1_TabTesterXY pin  8 DIG INPUT Limit Y
	
	Slot_RawSensor     =  9
	
	Slot_EnableMotorsCoreXY  = 10			'1_TabTesterXY pin 10 DIG OUT
	
	' ----------------------------------------- EMERGENCY and SPEED
	Slot_Speed         =  6
	
	' ----------------------------------------- KEYS
	Key Right		Gosub ExecKeys
	Key Left 		Gosub ExecKeys
	Key Up 			Gosub ExecKeys
	Key Down 		Gosub ExecKeys
	
	' ----------------------------------------- START
	DestX = v1
	DestY = v2
	
	' -----------------------------------------------------------------
	'  BUTTONS - COREXY
	' -----------------------------------------------------------------
	Button 1 Text HomeXY
	Button HomeXY Text "Home XY"
	Button HomeXY Color Yellow
	
	'Button 34 Text EnableMotorsCoreXY
	'Button EnableMotorsCoreXY Text "ENABLE XY"
	'Button EnableMotorsCoreXY Color PaleGreen
	'
	'Button 35 Text DisableMotorsCoreXY
	'Button DisableMotorsCoreXY Text "DISABLE XY"
	'Button DisableMotorsCoreXY Color PaleGreen
Return

Stop												'

' ------------------------------------ COREXY
Variable Numeric DestxMax
Variable Numeric DestxMin
Variable Numeric DestyMax
Variable Numeric DestyMin

' ------------------------------------ COREXY
Label ClearButtons
	For v1 = 1 To 32
		Button v1 Text ""
		Button v1 Color Moccasin
	Next
Return

' =====================================================================
'  CORE XY
' =====================================================================
Variable Numeric DestX				' destination in mm coords
Variable Numeric DestY				' destination in mm coords

Variable Numeric Slot_MotorA
Variable Numeric Slot_MotorB
Variable Numeric Slot_EnableMotorsCoreXY
Variable Numeric Slot_SensorX
Variable Numeric Slot_SensorY

Variable Numeric Slot_RawSensor

Label CoreXY_MoveToDestXY_AndWait   ' move to destX & destY and wait stop motors
	Gosub CoreXY_MoveToDestXY
	Gosub WaitMotorsCoreXY
Return

Label CoreXY_MoveToDestXY			' move to destX & destY (with limits)
	DestX = Limit(DestX, DestxMin, DestxMax)
	DestY = Limit(DestY, DestyMin, DestyMax)
	If LimitReached
		Load sound\GeigerClick_16.wav
	EndIf
	Gosub CoreXY_PrepareCoreAB
	Slot(Slot_MotorA) = CoreA
	Slot(Slot_MotorB) = CoreB
Return

Label CoreXY_MoveToDestXY_NoLimits	' move to destX & destY (without using limits)
	Gosub CoreXY_PrepareCoreAB
	Slot(Slot_MotorA) = CoreA
	Slot(Slot_MotorB) = CoreB
Return

' ----------------------------------------------------------------------
'  To use this function you must prepare DestX and DestY
'   with the declared switch positions
' ----------------------------------------------------------------------
Label CoreXY_SetNewZero
    ' ------------------------- Reset axis X and Y
	Slot(Slot_MotorA) = Reset
	Slot(Slot_MotorB) = Reset
	Wait Seconds 0.15
	' ------------------------- Set reset values with CoreXY correction
	Gosub CoreXY_MoveToDestXY_NoLimits
	Gosub WaitMotorsCoreXY
Return

Variable Numeric CoreA
Variable Numeric CoreB

Label CoreXY_PrepareCoreAB
	CoreA = (DestX - DestY) / 2
	CoreB = (DestX + DestY) / 2
Return

' =====================================================================
'  HOME - COREXY
' =====================================================================
Variable Numeric HomeAxis
Variable Numeric SlotSensor
Variable Numeric DetachMM
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DeclaredSwitchPos
Variable Numeric InitialPosition

Label HomeXY
	Button RunSequence Disabled
	Gosub CoreXY_SetNewZero
	Gosub EnableMotorsCoreXY
	Gosub HomeY
	Gosub HomeX
	'Gosub CoreXY_SetNewZero
	Gosub UpdateButtonsXY
	'Gosub "DISAB.MOTORS"
	Button RunSequence Enabled
Return

Label HomeX
	HomeAxis = 0        	  ' 0 = Axis X
	SlotSensor = Slot_SensorX ' home sensor slot
	DetachMM = 1			  ' initial detach mm (neg or pos)
	SpeedSearch = -0.6		  ' direction and speed
	SpeedReturn = +0.05		  ' detach direction and speed
	DeclaredSwitchPos = 0     ' declared switch angle
	InitialPosition = 1       ' move to mm
	Gosub  Home
Return

Label HomeY
	HomeAxis = 1        	  ' 1 = Axis Y
	SlotSensor = Slot_SensorY ' home sensor slot
	DetachMM = -1			  ' initial detach mm (neg or pos)
	SpeedSearch = +0.6		  ' direction and speed
	SpeedReturn = -0.05		  ' detach direction and speed
	DeclaredSwitchPos = 690   ' declared switch angle
	InitialPosition = 688     ' move to mm
	Gosub Home
Return

Label Home
	' -------------------------- detach if sensor is active
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		If HomeAxis = 0
			DestX = DestX + DetachMM
		Else
			DestY = DestY + DetachMM
		EndIf
		Gosub CoreXY_MoveToDestXY_NoLimits
		Gosub WaitMotorsCoreXY
	EndIf
    ' -------------------------- search the sensor
Label SearchHome
	If HomeAxis = 0
		DestX = DestX + SpeedSearch
	Else
		DestY = DestY + SpeedSearch
	EndIf
	Gosub CoreXY_MoveToDestXY_NoLimits
	'
	Wait Seconds 0.005
	If Slot(SlotSensor) > 500 Or Key("CTRL")
		If HomeAxis = 0
			DestX = 0
		Else
			DestY = 0
		EndIf
		Gosub CoreXY_SetNewZero
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' -------------------------- detach from the sensor
Label DetachSensor
	If HomeAxis = 0
		DestX = DestX + SpeedReturn
	Else
		DestY = DestY + SpeedReturn
	EndIf
	Gosub CoreXY_MoveToDestXY_NoLimits
	Wait Seconds 0.005
	If Slot (SlotSensor) < 500 Or Key("CTRL")
		Gosub WaitMotorsCoreXY
		If HomeAxis = 0
			DestX = DeclaredSwitchPos
		Else
			DestY = DeclaredSwitchPos
		EndIf
		Gosub CoreXY_SetNewZero
		Goto MoveToInitialPosition
	EndIf
Goto DetachSensor
    ' -------------------------- move to InitialPosition
Label MoveToInitialPosition
	If HomeAxis = 0
		DestX = InitialPosition
	Else
		DestY = InitialPosition
	EndIf
	Gosub CoreXY_MoveToDestXY_NoLimits
	Gosub WaitMotorsCoreXY
Return

' =====================================================================
'  WAIT until the motors A and B are stopped - COREXY
' =====================================================================
Label WaitMotorsCoreXY
	Wait Seconds 0.1         ' this ensures that the motors are running
	Label WMXY1
		If Abs(Slot(Slot_MotorA + 1)) < 0.05
			If Abs(Slot(Slot_MotorB + 1)) < 0.05
				Return
			EndIf
		EndIf
		Wait Seconds 0.005
	Goto WMXY1
Goto WaitMotorsCoreXY

Label EnableMotorsCoreXY
	Gosub WaitMotorsCoreXY
	Slot(Slot_EnableMotorsCoreXY) = 1000
	Wait Seconds 0.1
Return

Label DisableMotorsCoreXY
	Gosub WaitMotorsCoreXY
	Slot(Slot_EnableMotorsCoreXY) = 0
Return

' ====================================================================
'  SPEED
' ====================================================================
Variable Numeric Slot_Speed

Label ExecSpeed
	'v9 = Slot(Slot_Speed)
	'v9 = 6 + 3 * v9 / 1000
	'Option Speed V9
Return

' =====================================================================
'  STEPPER-MOTORS MOVE SLOW - COREXY
' =====================================================================
Variable Numeric MoveSlowXY_DestX 	' destination in mm
Variable Numeric MoveSlowXY_DestY 	' destination in mm
Variable Numeric MoveSlowXY_Speed 	' speed in mm/sec

Variable Numeric MSV1
Variable Numeric MSV2
Variable Numeric MSV3
Variable Numeric MSV4

Label MoveSlowXY
	' ----------------------------------- test for very low speed
	If MoveSlowXY_Speed < 0.1
		MoveSlowXY_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlowXY_DestX - DestX
	MSV2 = MoveSlowXY_DestY - DestY
	MSV3 = MSV1 * MSV1
	MSV3 = MSV3 + MSV2 * MSV2
	MSV3 = Sqrt(MSV3)
	' ----------------------------------- calc the loops count
	MSV4 = MSV3 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlowXY_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		DestX = MoveSlowXY_DestX
		DestY = MoveSlowXY_DestY
		Gosub CoreXY_MoveToDestXY_AndWait
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlowXY_DestX - DestX) / MSV4
	MSV2 = (MoveSlowXY_DestY - DestY) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		DestX = DestX + MSV1
		DestY = DestY + MSV2
		Gosub CoreXY_MoveToDestXY
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitMotorsCoreXY
	Gosub UpdateButtonsXY
Return

' ====================================================================
'  JOG with keyboard - COREXY
' ====================================================================
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")
		DestX = DestX + JogSpeed
	EndIf
	If Key("Left")
		DestX = DestX - JogSpeed
	EndIf
	'
	If Key("Up")
		DestY = DestY + JogSpeed		'proto
		'DestY = DestY - JogSpeed		'Leo
	EndIf
	If Key("Down")
		DestY = DestY - JogSpeed		'proto
'		DestY = DestY - JogSpeed		'Leo
	EndIf
	'
	'If Key("PageUp")
	'	Slot Slot_Z = Limit(Slot(Slot_Z) + JogSpeed, Zmin, Zmax)
	'EndIf
	'If Key("PageDown")
	'	Slot Slot_Z = Limit(Slot(Slot_Z) - JogSpeed, Zmin, Zmax)
	'EndIf
	'
	Gosub CoreXY_MoveToDestXY
	Gosub UpdateButtonsXY
Return

' ====================================================================
'  PRINT AND UPDATE COORDINATES - COREXY
' ====================================================================
'Label PrintValues
'	Controls OpenTextBox
'	s1 = "X=" + Format(DestX, "0.000")
'	s2 = "Y=" + Format(DestY, "0.000")
'	Print CRLF + s1 + " " + s2
'Return

Label UpdateButtonsXY
	Button BtnDestX Text "X=" + Format(DestX, "0.000")
	Button BtnDestY Text "Y=" + Format(DestY, "0.000")
Return


'EXEC DestY = 10
