'╔════════════════════════════════════════════════════════════╗
'║     Name: sCOBOT_SENTEL_V09.txt          Date: 02 May 2022 ║
'║   Author: Leonardo                                         ║
'║    Scope: Test all the functionality of sCOBOT SENTEL      ║
'╠════════════════════════════════════════════════════════════╣
'║Hardware: SENTEL M1=101:1 basement, M2=50:1                 ║
'╟────────────────────────────────────────────────────────────╢
'║ Note: First run                                 31/03/2022 ║
'║       Wood plate and six motor structure         2/04/2022 ║
'║       CAN refined                                8/04/2022 ║
'║       COBOT updated                             12/04/2022 ║
'║       Wood plate and six motor structure        14/04/2022 ║
'║       Adeguate sequence                         16/04/2022 ║
'║       HAL & Master, multicolor LED              17/04/2022 ║
'║       Feetech FT2331M, Cloche and Encoder AB    17/04/2022 ║
'║       Added joint 3 and 4                       19/04/2022 ║
'║       Integration of COBOT360                   28/04/2022 ║
'║       New function RS485 for connection         02/05/2022 ║
'╚════════════════════════════════════════════════════════════╝

'Slot 602 = 1000
Option Speed 9
' 32 To DXP1 254 21-1		' Immediate value to P (Proportional
' 16 To DXP1 254 22-1		' Immediate value to D (Derivative)
'  0 To DXP1 254 23-1		' Immediate value to I (Integral)
'Beep "2500 100, 1500 100"

Variable Numeric Debug = 0 ' 1 = DEBUG   0 = NORMAL

RestoreVars

Option Speed 9
Option GlobalKeys Enabled
'Load images\SENTEL_two_joint.jpg
'Beep "2500 200, 1500 200"
TTS SelectVoice Language
TTS SetVolume 100
TTS SetSpeed -1

' ╔════════════════════════════════════════════╗
' ║ SLOT TEXT for COMMANDS and RESPONSES       ║
' ╚════════════════════════════════════════════╝
Variable Numeric SlotCobotCommands  = 1
Variable Numeric SlotCobotResponses = 2
Variable Numeric SlotQRcode         = 3

' ╔════════════════════════════════════════════╗ █   █ █▀▀█ █▀▀▀█
' ║ VARIABLE INITIALIZATIONS & ASSIGNEMENTS    ║ █   █ █▄▄█ █▄▄▄█
' ╚════════════════════════════════════════════╝  ▀▄▀  █  █ █  ▀▄
Variable Numeric NumCycle
Variable Numeric StartTime
Variable String  Result
Variable Numeric Slot_Emergency

Variable Numeric Slot_ClocheX			=  1
Variable Numeric Slot_ClocheY			=  2
'Variable Numeric Slot_SwitchAB			=  3
Variable Numeric Slot_EncoderData		=  4
'Variable Numeric Slot_Buzzed			=  5

Variable Numeric Slot_FPS              = 13
Variable Numeric Slot_EnableRS485      = 14

Variable Numeric Slot_LedR			    =  7
Variable Numeric Slot_LedG		        =  8
Variable Numeric Slot_LedB		        =  9
Variable Numeric Slot_Pump        	    =  6
Variable Numeric Slot_SignalScope       = 13

' ╔════════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                    ║ █▀▀█ █  █   █
' ╚════════════════════════════════════════════╝ █▄▄█ █▄▄█   █
Button 1 Text FramePerSec
Button FramePerSec Text "Frame Per Sec."

Button 3 Text ClocheModeJoint
Button ClocheModeJoint Text "Cloche Mode Joint M1 & M2"
Button ClocheModeJoint Color Aquamarine

Button 4 Text ClocheModeJointTF2331M
Button ClocheModeJointTF2331M Text "Cloche Mode TF2331M"
Button ClocheModeJointTF2331M Color Aquamarine

Button 6 Text EditSequence
Button EditSequence Text "Edit sequence"
Button EditSequence Color PaleGreen

Button 7 Text RunSequence
Button RunSequence Text "RUN SEQUENCE"
Button RunSequence Color Aqua

Button 8 Text SelectSequence
Button SelectSequence Text "Select Sequence"
Button SelectSequence Color PowderBlue

Button 9 Text StartPump
Button StartPump Text "Start Pump"
Button StartPump Color Lime

Button 10 Text StopPump
Button StopPump Text "Stop Pump"
Button StopPump Color Crimson

Button 11 Text WayPointTest

Button 12 Text SayQRCode
Button SayQRCode Color Yellow

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 GreenOn
Button GreenOn Color Green
Button 19 Text YellowOn
Button YellowOn Color Yellow
Button 20 Text BlueOn
Button BlueOn Color MediumBlue
Button 21 Text WhiteOn
Button GreenOn Color Green
Button 22 Text RgbOff
Button RgbOff Color Black

Button 24 Text KnobModeM2_x_1
Button KnobModeM2_x_1 Text "Knob Mode M1 x 1"
Button KnobModeM2_x_1 Color Aquamarine

Button 25 Text KnobModeM2_x_01
Button KnobModeM2_x_01 Text "Knob Mode M1 x 0.1"
Button KnobModeM2_x_01 Color Aquamarine

Button 26 Text KnobModeM2_x_001
Button KnobModeM2_x_001 Text "Knob Mode M1 x 0.01"
Button KnobModeM2_x_001 Color Aquamarine

Button 27 Text KnobModeM2_x_0001
Button KnobModeM2_x_0001 Text "Knob Mode M1 x 0.001"
Button KnobModeM2_x_0001 Color Aquamarine

Button 29 Text KnobModeFT2331MX1
Button KnobModeFT2331MX1 Text "Knob Mode FeeTech x1"
Button KnobModeFT2331MX1 Color Aquamarine

Button 30 Text KnobModeFT2331MX5
Button KnobModeFT2331MX5 Text "Knob Mode FeeTech x5"
Button KnobModeFT2331MX5 Color Aquamarine

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Load Theremino_CAN.exe
Load Theremino_HAL.exe
Load Theremino_SlotViewer.exe
Load Theremino_SignalScope_M1.exe
Load Theremino_SignalScope_M2.exe
Load Theremino_Cobot.exe
'Load Theremino_Terminal_Transmit.exe
'Load Theremino_Terminal_Receive.exe

' ╔════════════════════════════════════════════╗  █▀▀▀ ▀▀█▀▀ █▀▀▀█ █▀▀▀█ ▀▀█▀▀
' ║  START FROM HERE                           ║  ▀▀▀█   █   █▀▀▀█ █▀█▀▀   █
' ╚════════════════════════════════════════════╝  ▄▄▄█   █   █   █ █  ▀▄   █
'Slot(Slot_EnableRS485) = 1000

Controls OpenTextBox
ShowSequenceName
StopPump
DisableAllButtons
CobotStopExecution
CobotDisableRepeat
CobotCommand = "SetAcceleration 200"
SendCommandToCobot
CobotCommand = "SetSpeed 2000"
SendCommandToCobot
Controls ClearTextBox
YellowOn
Print TTSvoices
CheckMotorTemperature
EnableAllButtons
Language = 1

Slot(100) = 0
Slot(200) = 0
Slot(300) = 0
Slot(300) = 0

Wait Seconds 0.5
Slot(100) = 1000
Slot(200) = 1000
Slot(300) = 1000
Slot(400) = 1000

CheckMotorTemperature

Load Hide
Loop
Stop


' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	GetResponseFromCobot
	' ------------------------------------------
	CheckMotorTemperature
	' ------------------------------------------
	CheckFramePerSecond
	
	' ---------------------------------------------------------
	' 0.02 sec. or more to allow time for keys and other tasks
	Wait Seconds 0.02
	' ---------------------------------------------------------
Goto Loop

' ╔════════════════════════════════════════════╗
' ║ 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

' ╔════════════════════════════════════════════╗
' ║ SAVE LOAD VARS                             ║
' ╚════════════════════════════════════════════╝
Label SaveVars
	S1 = SequenceName
	s3 = Language
	Save Vars
	Print CRLF + "SAVED SEQUENCENAME = " + s1 + CRLF
Return

Label RestoreVars
	Load Vars
	's1 = "COB_RaiseAfterHome.seq"
	SequenceName = S1
	Language = s3
Return

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL                                  ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	CobotStopExecution
	StopPump
	CobotCommand = "SetAcceleration 50"
	SendCommandToCobot
	CobotCommand = "SetSpeed 1000"
	SendCommandToCobot
	RedOn
	SaveVars
	Load CloseApps
	Load CloseApps Theremino_VideoColors.exe
	Load CloseApps Theremino_QRdecoder.exe
	RedOn
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	SaveVars
	CobotStopExecution
	StopPump
	CobotCommand = "SetAcceleration 50"
	SendCommandToCobot
	CobotCommand = "SetSpeed 1000"
	SendCommandToCobot
	RedOn
	EnableAllButtons
	Window Sizable
	Slot(Slot_SignalScope) = 0
	SayEndOfProgram
End

' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀█ █   █   █▀▀█
' ║  SAY QRCODE                                ║ ▀▀▀█ █▀▀█  ▀▄▀    █  █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █  █   █     █▄██▄
Label SayQRCode
	'Load CloseApps Theremino_VideoColors.exe
	'Wait Not AppRunning("Theremino_VideoColors.exe")
	'Load Theremino_QRdecoder.exe
	'Wait AppRunning("Theremino_QRdecoder.exe")
	'
	WaitCobotReady
	CobotCommand = "StopExecution"
	SendCommandToCobot
	CobotCommand = "LoadSequence COB_move_to_QRcode.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "COB_move_to_QRcode.seq" + CRLF
	'
	For v1 = 1 To Infinity
		Wait  SlotText(SlotQRcode) <> ""
		s2 = SlotText(SlotQRcode)
		s2 = Replace(s2, CRLF, "")
		s2 = Trim(s2)
		Select s2
			Case "English male"
				Language = 1
				TTS SelectVoice 1
				SayNowTheLanguageIsEnglishMale
			Case "English female"
				Language = 2
				TTS SelectVoice 2
				SayNowTheVoiceIsEnglishFemale
			Case "Italian female"
				Language = 3
				TTS SelectVoice 3
				SayNowTheLanguageIsItalianFemale
			Case "Chinese female"
				Language = 4
				TTS SelectVoice 4
				SayNowTheLanguageIsChineseFemale
			Case "START"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "Start work."
				Wait TTSready
			Case "Coin"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "Coin work."
				Wait TTSready
			Case "Ball"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "Ball work."
				Wait TTSready
			Case "Start pump"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "Start pump."
				Wait TTSready
			Case "Stop pump"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "Stop pump."
				Wait TTSready
			Case "Home"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "Home work."
				Wait TTSready
			Case "Show"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "Show work."
				Wait TTSready
			Case "Red"
				RedOn
			Case "Green"
				GreenOn
			Case "Blue"
				BlueOn
			Case "Yellow"
				YellowOn
			Case "White"
				WhiteOn
			Case "END"
				'Language = 1
				'TTS SelectVoice 1
				TTS Speak "End work."
				Wait TTSready
			CaseElse
				Print "Invalid QRcode " + s2 + CRLF
				SayUnknowQRcode
			'Stop
		EndSelect
		Print "QRcode=" + s2 + CRLF
		WaitCobotReady
		Wait SlotText(SlotQRcode) <> s2
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label StartPump
	Slot(Slot_SignalScope) = 1000
	Slot(Slot_Pump) = 1000
	Button StartPump Disabled
	Button StopPump Enabled
	Wait Seconds 1
	Beep "1500, 100
'	If Slot(Slot_Vacuum) > 56
'		'Beep "500, 300, 0, 300"
'		SayVacuumProblem
'		Wait TTSready
'		RedOn
'		StopPump
'		Print "Vacuum Problem" + CRLF
'		CobotShowPoint
'		Print "Execution stopped" + CRLF
'		RedBlink
'		End
'	Else
'		WhiteOn
'		Wait Seconds 0.1
'		RgbOff
'		SayTargetTaken
'		Print "Target taken"
'	EndIf
'Return
	
Label StopPump
	Slot(Slot_Pump) = 0
	Button StopPump Disabled
	Button StartPump Enabled
Return

Label WayPointTest
	CobotCommand = "StopExecution"
	SendCommandToCobot
	CobotCommand = "LoadSequence COB_WayPoinTest.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "COB_WayPoinTest.seq" + CRLF
Return

' ╔════════════════════════════════════════════╗  █▀▀▀ █▀▀█ █▀█    █▀▀▀
' ║ COBOT COMMANDS                             ║  █    █  █ █▀▀█   █
' ╚════════════════════════════════════════════╝  █▄▄▄ █▄▄█ █▄▄█   █▄▄▄
Variable String CobotCommand
Label SendCommandToCobot
	SlotText SlotCobotCommands = ""
	Wait Seconds 0.1
	SlotText SlotCobotCommands = CobotCommand
	Wait Seconds 0.1
	WaitCobotReady
Return

Variable String CobotResponse
Label GetResponseFromCobot
	CobotResponse = SlotText(SlotCobotResponses)
	'Print CobotResponse + CRLF
Return

Label WaitCobotReady
	Wait SlotText(SlotCobotResponses) = "Ready"
	
	For v1 = 0 To Infinity
		If SlotText(SlotCobotResponses) = "Ready"
			Exit
		EndIf
	Next
Return

Label CobotStartExecution
	CobotCommand = "StartExecution"
	SendCommandToCobot
Return

Label CobotStopExecution
	CobotCommand = "StopExecution"
	SendCommandToCobot
Return

Label CobotDisableRepeat
	CobotCommand = "DisableRepeat"
	SendCommandToCobot
Return

Label ClocheModeJoint
	DisableAllButtons
	Button ClocheModeJoint Enabled
	Button ClocheModeJoint Color Flashing5 LightCyan Aqua
	TTS Speak "CLOCHE mode Joint."
	For v1 = 1 To Infinity
		If Abs(Slot(Slot_ClocheX) - 500) > 20
			Slot(101) = (Slot(Slot_ClocheX) - 500) / 500 * 180
		EndIf
		If Abs(Slot(Slot_ClocheY) - 500) > 20
			Slot(201) = (Slot(Slot_ClocheY) - 500) / 500 * 180
		EndIf
	Next
Return

Label ClocheModeJointTF2331M
	DisableAllButtons
	Button ClocheModeJointTF2331M Enabled
	Button ClocheModeJointTF2331M Color Flashing5 LightCyan Aqua
	TTS Speak "CLOCHE mode for smart motor."
	For v1 = 1 To Infinity
		Slot(3) = Slot(Slot_ClocheX)
	Next
Return

Label KnobModeM2_x_1
	DisableAllButtons
	Button KnobModeM2_x_1 Enabled
	Button KnobModeM2_x_1 Color Flashing6 LightCyan Aqua
	TTS Speak "Knob mode, motor 2 set for step 1."
	v73 = Slot(Slot_EncoderData)
	For v1 = 1 To Infinity
		Slot(101) = (Slot(Slot_EncoderData) - v73) * 1
	Next
Return

Label KnobModeM2_x_01
	DisableAllButtons
	Button KnobModeM2_x_01 Enabled
	Button KnobModeM2_x_01 Color Flashing6 LightCyan Aqua
	TTS Speak "Keykknob mode, motor 2 set for step 0.1."
	v73 = Slot(Slot_EncoderData)
	For v1 = 1 To Infinity
		Slot(101) = (v73 - Slot(Slot_EncoderData)) * 0.1
	Next
Return

Label KnobModeM2_x_001
	DisableAllButtons
	Button KnobModeM2_x_001 Enabled
	Button KnobModeM2_x_001 Color Flashing6 LightCyan Aqua
	TTS Speak "Keykknob mode, motor 2 set for step 0.01."
	v73 = Slot(Slot_EncoderData)
	For v1 = 1 To Infinity
		Slot(101) = (v73 - Slot(Slot_EncoderData)) * 0.01
	Next
Return

Label KnobModeM2_x_0001
	DisableAllButtons
	Button KnobModeM2_x_0001 Enabled
	Button KnobModeM2_x_0001 Color Flashing6 LightCyan Aqua
	TTS Speak "Keykknob mode, motor 2 set for step 0.001."
	v73 = Slot(Slot_EncoderData)
	For v1 = 1 To Infinity
		Slot(101) = (v73 - Slot(Slot_EncoderData)) * 0.001
	Next
Return

Label KnobModeFT2331MX1
	DisableAllButtons
	Button KnobModeFT2331MX1 Enabled
	Variable Numeric PreviousX1 = 0
	Variable Numeric ActualX1 = 0
	Button KnobModeFT2331MX1 Color Flashing6 LightCyan Aqua
	TTS Speak "Keynob mode, smart motor FeeTech with step set at 1."
	Slot(3) = 500
	PreviousX1 = Slot(4)
	ActualX1 = Slot(4)
	For v1 = 1 To Infinity
		ActualX1 = Slot(4)
		If PreviousX1  <>  ActualX1
			Slot(3) = (ActualX1 - PreviousX1) + 500
		EndIf
	Next
Return

Label KnobModeFT2331MX5
	DisableAllButtons
	Button KnobModeFT2331MX5 Enabled
	Variable Numeric PreviousX5 = 0
	Variable Numeric ActualX5 = 0
	Button KnobModeFT2331MX5 Color Flashing6 LightCyan Aqua
	TTS Speak "Keynob mode, smart motor FeeTech with step set at 5."
	Slot(3) = 500
	PreviousX5 = Slot(4)
	ActualX5 = Slot(4) + 5
	For v1 = 1 To Infinity
		ActualX5 = Slot(4)
		If PreviousX5  <>  ActualX5
			Slot(3) = (((ActualX5) - PreviousX5) * 5) + 500
		EndIf
	Next
Return

Label COBOTRaiseAfterHome
	CobotStopExecution
	CobotCommand = "LoadSequence COB_RaiseAfterHome.seq"
	SendCommandToCobot
	CobotStartExecution
	Print "COB_RaiseAfterHome.seq" + CRLF
Return

Label COB_SENTEL_2M_syncro
	TTS Speak "Small COBOT, start two motor demo execution."
	'Wait TTSready
	CobotCommand = "StopExecution"
	SendCommandToCobot
	CobotCommand = "LoadSequence COB_SENTEL_2M_syncro.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "COB_SENTEL_2M_syncro.seq" + CRLF
	Wait TTSready
Return

' ╔════════════════════════════════════════════╗ █▀█  █▀▀█ █   █
' ║  B A L L                                   ║ █▀▀█ █▀▀█ █   █
' ╚════════════════════════════════════════════╝ █▄▄█ █  █ █▄▄ █▄▄
Label CobotLoadSeqBall
	SayStartBallRoutine
	CobotCommand = "StopExecution"
	SendCommandToCobot
	CobotCommand = "LoadSequence BAL_1_Pick_A.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "BAL_1_Pick_A.seq" + CRLF
	StartPump
	
	CobotCommand = "LoadSequence BAL_2_Placel_B.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "BAL_2_Placel_B.seq" + CRLF
	StopPump
	
	CobotCommand = "LoadSequence BAL_3_Pick_B.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "BAL_3_Pick_B.seq" + CRLF
	StartPump
	
	CobotCommand = "LoadSequence BAL_4_Place_A.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "BAL_4_Place_A.seq" + CRLF
	StopPump
	
	CobotCommand = "LoadSequence BAL_5_RaiseAfterPlace_A.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "BAL_5_RaiseAfterPlace_A.seq" + CRLF
	Slot(Slot_SignalScope) = 0
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀█ █▀▀█ █ ▄▀
' ║  SPOKEN MESSAGES                           ║ ▀▀▀█ █▀▀▀ █  █ ██
' ╚════════════════════════════════════════════╝ ▄▄▄█ █    █▄▄█ █ ▀▄
Variable Numeric Language
Label SayPowerSwIsOn
	Select Language
		Case 1			'David English male
			TTS Speak "Power switch is on?"
		Case 2			'Zira English female
			TTS Speak "Power switch is on?"
		Case 3  		'Elsa Italian Female
			TTS Speak "Hai acceso l'alimentazione?"
		Case 4			'Huihui Chinese female
			TTS Speak "电源开关是否打开？"
	EndSelect
	Wait TTSready
Return

Label StartaSequenceOfRobotAction
	Select Language
		Case 1			'David English male
			TTS Speak "Start robot action sequence."
		Case 2			'Zira English female
			TTS Speak "Start robot action sequence."
		Case 3			'Elsa Italian Female
			TTS Speak "Avviata la sequenza di azioni del robot."
		Case 4			'Huihui Chinese female
			TTS Speak "启动机器人动作序列。"
	EndSelect
	Wait TTSready
Return

Label SayStartBallRoutine
	Select Language
		Case 1			'David English male
			TTS Speak "Start sequence of pick and place of ping pong ball"
		Case 2			'Zira English female
			TTS Speak "Start sequence of pick and place of ping pong ball"
		Case 3			'Elsa Italian Female
			TTS Speak "Avvia la sequenza di prelievo e posizionamento della pallina da ping pong."
		Case 4			'Huihui Chinese female
			TTS Speak "开始乒乓球的取放顺序。"
	EndSelect
	'Wait TTSready
Return

Label SayStartCoinRoutine
	Select Language
		Case 1			'David English male
			TTS Speak "Coin sequence management has been launched."
		Case 2			'Zira English female
			TTS Speak "Coin sequence management has been launched."
		Case 3			'Elsa Italian Female
			TTS Speak "Avviata la gestione della sequenza della moneta."
		Case 4			'Huihui Chinese female
			TTS Speak "硬币顺序管理开始。"
	EndSelect
	'Wait TTSready
Return

Label SayReadQRcodeFunction
	Select Language
		Case 1			'David English male
			TTS Speak "Interpretation of the QR code and start of the sequence of actions of the robot."
		Case 2			'Zira English female
			TTS Speak "Interpretation of the QR code and start of the sequence of actions of the robot."
		Case 3			'Elsa Italian Female
			TTS Speak "Interpretazione del codice QR e avvio della sequenza di azioni del robot."
		Case 4			'Huihui Chinese female
			TTS Speak "解读二维码并开始机器人的一系列动作。"
	EndSelect
	Wait TTSready
Return

Label SayEndOfRobotActions
	Select Language
		Case 1			'David English male
			TTS Speak "The action of the robot is finished and it goes to the rest position."
		Case 2			'Zira English female
			TTS Speak "The action of the robot is finished and it goes to the rest position."
		Case 3			'Elsa Italian Female
			TTS Speak "L'azione del robot è terminata e si porta in posizione di riposo."
		Case 4			'Huihui Chinese female
			TTS Speak "机器人动作完成，进入静止位置。"
	EndSelect
	'Wait TTSready
Return

Label SayRobotReadyAndOperational
	TTS Speak "Joint motors are ready and operational."
Return

Label SayNowEnterInClocheModeJoint
	TTS Speak "Now enter in cloche mode."
	'Wait TTSready
Return

Label SayNowExitFromClocheModeJoint
	TTS Speak "Now exit from cloche mode."
	'Wait TTSready
Return

Label SayNowTheLanguageIsEnglishMale
	TTS Speak "Now the language is English."
	Wait TTSready
Return

Label SayNowTheVoiceIsEnglishFemale
	TTS Speak "Now the voice is female."
	Wait TTSready
Return

Label SayNowTheLanguageIsItalianFemale
	TTS Speak "Ora parla Italiano."
	Wait TTSready
Return

Label SayNowTheLanguageIsChineseFemale
	TTS Speak "现在语言是中文"
	Wait TTSready
Return

Label SayUnknowQRcode
	Language = 1
	TTS Speak "Unknow QRcode."
Return

Label SayVacuumProblem
	Language = 1
	TTS Speak "Vacuum Problem."
Return

Label SayTargetTaken
	Language = 1
	TTS Speak "Target taken."
Return

Label SayEndOfProgram
	Language = 1
	TTS Speak "The program is terminated."
Return

' ╔════════════════════════════════════════════╗
' ║ EMERGENCY                                  ║
' ╚════════════════════════════════════════════╝
Label Emergency
'	DisableAllButtons
'	Button 15 Enabled
'	Button 15 Color Flashing7 Yellow Red
'	For v1 = 1 To Infinity
'		RedOn
'		Wait Seconds 0.4
'		RgbOff
'		Wait Seconds 0.4
'		Load sound\Alarm10b.wav
'	Next
	Stop
	Emergency
	
' ╔════════════════════════════════════════════╗ █   █▀▀ █▀▄
' ║ LED COLORS                                 ║ █   █▀  █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█
Label RedOn
	'Language = 1
	'TTS Speak "Status red."
	Slot Slot_LedR = 0
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 1000
Return

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

Label YellowOn
	'Language = 1
	'TTS Speak "Status yellow."
	Slot Slot_LedR = 100
	Slot Slot_LedG = 600
	Slot Slot_LedB = 10000
Return

Label GreenOn
	'Language = 1
	'TTS Speak "Status green."
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label BlueOn
	'Language = 1
	'TTS Speak "Status blue."
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 0
Return

Label WhiteOn
	'Language = 1
	'TTS Speak "Status white."
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
Return

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

Label RedBlink
	RedOn
	WhiteOn
Return

' ╔════════════════════════════════════════════╗ ▀▀█▀▀ █▀▀▀ █▄ ▄█ █▀▀█
' ║ TEMPERATURE MOTORS                         ║   █   █▀▀  █ ▀ █ █▀▀▀
' ╚════════════════════════════════════════════╝   █   █▄▄▄ █   █ █
Label CheckMotorTemperature
	If Debug = 0
		Variable Numeric OldTemperature
		TestMotorsTemperature
		If MotorsTemperature <> OldTemperature
			OldTemperature = MotorsTemperature
			Button MotorTemp Text "MotorTemp: " + Format(MotorsTemperature, "0")
			If MotorsTemperature > 55
				For v1 = 1 To Infinity
					TestMotorsTemperature
					If MotorsTemperature <= 68
						Exit
					EndIf
					Load Sound\Alarm14.wav
					TTS Speak "Warning, high tmperature" + MotorsTemperature
					Wait Not MediaPlaying
				Next
			EndIf
		EndIf
		If MotorsTemperature = 0
			Controls OpenTextBox
			For v1 = 1 To 5
				Print " CANbus not ready" + CRLF
				Load Sound\Alarm14.wav
				'TTS Speak "Warning, CANbus protocol is not running."
				Wait Not MediaPlaying
			Next
		EndIf
	EndIf
Return

Variable Numeric MotorsTemperature
Label TestMotorsTemperature
	MotorsTemperature = Slot(152)
	MotorsTemperature = Max(MotorsTemperature, Slot(252))
	'MotorsTemperature = Max(MotorsTemperature, Slot(306))
	'MotorsTemperature = Max(MotorsTemperature, Slot(406))
	'MotorsTemperature = Max(MotorsTemperature, Slot(506))
Return

Label WaitMotorsReady
	Slot(152) = 0
	for	v2 = 1 to Infinity
		Wait Seconds 0.2
		If Slot(152) = 0
			Print "Motor not ready "
			SayPowerSwIsOn
			Wait TTSready
			Wait Seconds 1
		Else
			Exit for
		EndIf
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀█ █▀▀▀
' ║ FRAME PER SECOND                           ║ █▀▀  █▄▄█ ▀▀▀█
' ╚════════════════════════════════════════════╝ █    █    ▄▄▄█
lkjjjkl
Label CheckFramePerSecond
	Button FramePerSec 	Text "FPS = " + Format(Slot(Slot_FPS), "0")
	Button FramePerSec Color Green9
	for	v3 = 1 to Infinity
		If Slot(Slot_FPS) > 35
			Button FramePerSec Color Salmon
			Print "Motor not comunicate correctly"
			Wait Seconds 1
			Slot(Slot_EnableRS485) = 0
			Wait Seconds 2
			Slot(Slot_EnableRS485) = 1000
			Wait Seconds 1
			Button FramePerSec Color Green9
		EndIf
		If Slot(Slot_FPS) = 0
			Button FramePerSec Color Black
			Print "Motor DISCONNECTED"
			Button FramePerSec 	Text "DISCONNECTED"
		EndIf
		If Slot(Slot_FPS) = -1
			Button FramePerSec Color Red
			Print "Motor INACTIVE"
			Button FramePerSec 	Text "INACTIVE"
		EndIf
		If Slot(Slot_FPS) = -2
			Button FramePerSec Color Yellow
			Print "Motor DISABLED"
			Button FramePerSec 	Text "DISABLED"
		EndIf
		Exit for
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀▀ █▀▀█
' ║  SEQUENCES                                 ║ ▀▀▀█ █▀▀  █  █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄▄ █▄██▄
Label InitSequencePaths
	Variable String EditorFolder
	Variable String EditorApp
	Variable String SequencePathAndName
	Variable String SequenceName
	'
	EditorFolder = ".\Apps\Theremino_Editor\"
	'
	EditorApp = EditorFolder + "Theremino_Editor.exe"
	'
	SequencePathAndName = EditorFolder + "Sequences\" + SequenceName
Return

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

Label SelectSequence
	InitSequencePaths
	s1 = SelectFile(EditorFolder + "Sequences\")
	If s1 <> ""
		SequenceName = s1
		ShowSequenceName
		EditSequence
	EndIf
Return

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


' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀▀ █▀▀█    █    █▀▀█
' ║ SEQUENCE LOADING AND EXECUTION             ║ ▀▀▀█ █▀▀▀ █  █    █    █  █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄▄ █▄██▄   █▄▄▄ █▄▄█
Label RunSequence
	CobotStartExecution
	'GreenOn
	StartaSequenceOfRobotAction
	SlotText CobotResponse = "Ready"
	DisableAllButtons
	Button EditSequence Enabled
	Button CloseAll Enabled
	'Button Emergency Enabled
	Button MotorTemp Enabled
	Variable Numeric ExitFromSequenceLoop
	'
	'Slot(Slot_SignalScope) = 1000
	Controls OpenTextBox
	StartTime = ElapsedTime
	InitSequencePaths
	'
	ExitFromSequenceLoop = 0
	SequenceToExecute = SequencePathAndName
	For NumCycle = NumCycle To Infinity
		Slot(Slot_SignalScope) = 1000
		Result = ""
		ExecSequence
		WaitCobotReady
		Slot(Slot_SignalScope) = 0
		Wait Seconds 0.2
		If ExitFromSequenceLoop > 0
			Exit
		EndIf
	Next
	Slot(Slot_SignalScope) = 0
	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  + CRLF
		Beep
		Stop
	EndIf
	'
	Print CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)  + CRLF
	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 "Restart"
					SequenceIndex = SequenceLength-1
				' ---------------------------------------------- STOP
				Case "Stop"
					ExitFromSequenceLoop = 1
					Exit
				' ---------------------------------------------- COBOT ACTIONS
				Case "COB_SENTEL_2M_syncro"
					COB_SENTEL_2M_syncro
				' ---------------------------------------------- LED COLORS
				Case "RedON"
					RedOn
				Case "MagentaON"
					MagentaOn
				Case "YellowON"
					YellowOn
				Case "GreenON"
					GreenOn
				Case "BlueOn"
					BlueOn
				Case "WhiteOn"
					WhiteOn
				Case "RgbOFF"
					RgbOff
				Case "START"
					RunSequence
				' ---------------------------------------------- With parameters
				Case "Pause"
					Wait Seconds GetSeparatedString(SequenceCmd, 1)
				Case "SignalScope"
					Slot(Slot_SignalScope) = GetSeparatedString(SequenceCmd, 1)
				CaseElse
					Controls OpenTextBox
					Print CRLF + " Unrecognized command: " + SequenceCmd + CRLF
					Beep
	'				Stop
			EndSelect
		EndIf
	Next
	Print Chars("-", 50) + CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)
	Print " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " sec" + CRLF
Return

' ----------------------------------------------
'  COMMANDS
' ----------------------------------------------
' "ExitFromEdit"      (exit from editing state)
' "StartExecution"    (auto-exit from editing)
' "StopExecution"     (auto-exit from editing)
' "GotoHome"          (auto-exit from editing)
' "GotoCenter"        (auto-exit from editing)
' "EnableRepeat"
' "DisableRepeat"
' "SelectLine nnn"
' "ExecuteSelectedLine"
' "EnableCollaborative
' "DisableCollaborative
' "AddNewPosition
' "SetHomePosition
' "SetCenterPosition
' "SetHoldingTorque nnn"
' "SetSafeTorqueLimit nnn"
' "SetTorque nnn"
' "SetAcceleration nnn"
' "SetSpeed nnn"
' "LoadSequence SeqName.seq" (without spaces)

' ----------------------------------------------
'  RESPONSES
' ----------------------------------------------
' "Ready"
' "Execution running at line nnn"
' "Motors disconnected"
' "Human detected - Speed reduced"
' "Human too near - Execution stopped"
' "Too much torque - Execution stopped"
' "Motors disconnected - Execution stopped"

'--------------------------------------------------------------
'Index  Name                          Culture  Gender   Age
'--------------------------------------------------------------
'1      Microsoft David Desktop       en-US    Male     Adult
'2      Microsoft Zira Desktop        en-US    Female   Adult
'3      Microsoft Elsa Desktop        it-IT    Female   Adult
'4      Microsoft Huihui Desktop      zh-CN    Female   Adult
'--------------------------------------------------------------
''


'WATCH PreviousX10
'WATCH ActualX10
'WATCH Slot(3)
