'╔═══════════════════════════════════════════════════════════╗
'║     Name: PVCArm150_01.txt              Date: 30 Oct 2022 ║
'║   Author: Leonardo                                        ║
'║    Scope: Test all the functionality of the PVC-150       ║
'╠═══════════════════════════════════════════════════════════╣
'║Hardware: 5 FeeTech STS3215                                ║
'╟───────────────────────────────────────────────────────────╢
'║    Note: Vacuum motor and valvle at 6V                    ║
'║          FeeTech at 7.5V                                  ║
'╟───────────────────────────────────────────────────────────╢
'║ISO   Culture  Gender   Age     Name                       ║
'╟───────────────────────────────────────────────────────────╢
'║ENG   EN-US    Male     Adult   Microsoft David Desktop    ║
'║ENG   EN-US    Female   Adult   Microsoft Zira Desktop     ║
'║ITA   IT-IT    Female   Adult   Microsoft Elsa Desktop     ║
'║JPN   JA-JP    Female   Adult   Microsoft Haruka Desktop   ║
'║ZHO   ZH-CN    Female   Adult   Microsoft Huihui Desktop   ║
'╚═══════════════════════════════════════════════════════════╝
Option Speed 9
Beep "2500 20, 1500 100"
'Load images\PVCArm_150.jpg
TTS SelectVoice "ENG"
s1 = TTSvoices
TTS SetVolume 100
TTS SetSpeed -2
TTS SelectVoice 3
'TTS Speak "Theremino Automation programming language is running."


' ╔════════════════════════════════════════════╗
' ║ SLOT TEXT for COMMANDS and RESPONSES       ║
' ╚════════════════════════════════════════════╝
Variable Numeric Slot_CommandsToMotors      = 50
Variable Numeric Slot_CommandsToCobot       = 51
Variable Numeric Slot_ResponsesFromCobot    = 52
Variable Numeric Slot_ExternalCommands      = 53
Variable Numeric Slot_ResponsesFromQRDecode = 54
Variable Numeric Slot_WordRecognized        = 55

' ╔════════════════════════════════════════════╗ █▀▀▀ █   █▀▀█ ▀▀█▀▀
' ║ SLOT INITIALIZATIONS                       ║ ▀▀▀█ █   █  █   █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄ █▄▄█   █
'Variable Numeric Slot_Vacuum      =   1
Variable Numeric Slot_LedR        =   7
Variable Numeric Slot_LedG        =   8
Variable Numeric Slot_LedB        =   9
Variable Numeric Slot_Pedal       =  10
Variable Numeric Slot_Pump        =  11

Variable Numeric Slot_SignalScope =  13

Variable Numeric Slot_Slider1     =   2
Variable Numeric Slot_Slider2     =   3
Variable Numeric Slot_Slider3     =   4
Variable Numeric Slot_Slider4     =   5
Variable Numeric Slot_Slider5     =   6

Variable Numeric Slot_Encoder1    =  21
Variable Numeric Slot_Encoder2    =  22
Variable Numeric Slot_Encoder3    =  23
Variable Numeric Slot_Encoder4    =  24
Variable Numeric Slot_Encoder5    =  25
Variable Numeric Slot_EncoderSlow =  31

Variable Numeric Slot_M1          = 101
Variable Numeric Slot_M2          = 201
Variable Numeric Slot_M3          = 301
Variable Numeric Slot_M4          = 401
Variable Numeric Slot_M5          = 501

Variable Numeric Slot_RS485_FPS   =  56

' ╔════════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                    ║ █▀▀█ █  █   █
' ╚════════════════════════════════════════════╝ █▄▄█ █▄▄█   █
Button 1 Label WanpyJob
Button WanpyJob Text "Wanpy Job"
Button WanpyJob Color Cyan

'Button 2 Label JobQrCode
'Button JobQrCode Text "JOB QRcode"
'Button JobQrCode Color Cyan
'
'Button 3 Label TunaCans
'Button TunaCans Text "Tuna cans"
'Button TunaCans Color Cyan
'
'Button 4 Label Sliders
'Button Sliders Text "Sliders"
'Button Sliders Color Cyan
'
'Button 5 Label Encoder
'Button Encoder Text "Encoder"
'Button Encoder Color Cyan

'Button 6 Label Voice
'Button Voice Text "Voice"
'Button Voice Color PaleTurquoise
'
Button 3 Label GoHome
Button GoHome  Text "Go Home"
Button GoHome  Color Yellow

Button 4 Label GoCenter
Button GoCenter  Text "Go Center"
Button GoCenter  Color Yellow

Button 6 Label SetCenterHere
Button SetCenterHere Text "Set Center Here"
Button SetCenterHere Color Orange

Button 7 Label CalibrateToZero
Button CalibrateToZero  Text "Calibrate to Zero"
Button CalibrateToZero  Color Orange

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

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

Button 11 Label CurrentMeter

Button 12 Label MotorFPS
'Button MotorFPS Text FPS

Button 14 Label "MotorTemp"
Button MotorTemp Color Salmon

Button 15 Label CloseAll
Button CloseAll Text "CLOSE ALL"
Button CloseAll Color Red

'Button 17 Label Home
'Button Home Text "HOME"
'Button Home Color Yellow
'
'Button 18 Label Home1
'Button Home1 Text "Home 1"
'Button Home1 Color Yellow
'
'Button 19 Label Home2
'Button Home2 Text "Home 2"
'Button Home2 Color Yellow
'
'Button 20 Label Home3
'Button Home3 Text "Home 3"
'Button Home3 Color Yellow
'
'Button 21 Label Home4
'Button Home4 Text "Home 4"
'Button Home4 Color Yellow
'
'Button 22 Label Home5
'Button Home5 Text "Home 5"
'Button Home5 Color Yellow
'
Button 24 Label ShowFiches
Button ShowFiches Text "ShowFiches"
Button ShowFiches Color SteelBlue

Button 25 Label ReleaseFiches
Button ReleaseFiches Text "ReleaseFiches"
Button ReleaseFiches Color SteelBlue

Button 26 Label ShowTuna
Button ShowTuna Text "ShowTuna"
Button ShowTuna Color SteelBlue

Button 27 Label ReleaseTuna
Button ReleaseTuna Text "ReleaseTuna"
Button ReleaseTuna Color SteelBlue

Button 28 Label ShowWanpy
Button ShowWanpy Text "ShowWanpy"
Button ShowWanpy Color SteelBlue

Button 29 Label ReleaseWanpy
Button ReleaseWanpy Text "ReleaseWanpy"
Button ReleaseWanpy Color SteelBlue

Button 30 Label VoiceLoop
Button VoiceLoop Text "VoiceLoop"
Button VoiceLoop Color Blue

Button 32 Label Say
Button Say Text "Say"
Button Say Color DeepSkyBlue


' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Load Theremino_Motors.exe
WaitApplicationsStarted
'' -------------------------------------
'  MOTORS must be active before Cobot
' -------------------------------------
'Load Theremino_HAL.exe
Load Theremino_SlotViewer.exe
Load Theremino_SlotViewer_M1.exe
Load Theremino_SlotViewer_M2.exe
Load Theremino_SlotViewer_M3.exe
Load Theremino_SlotViewer_M4.exe
Load Theremino_SlotViewer_M5.exe
'Load Theremino_SlotViewer_Enc.exe
'Load Theremino_SlotViewer_STR.exe
Load Theremino_MicroVideo.exe
'Load Theremino_QRdecoder.exe
'Load Theremino_VoiceCommands.exe
'Load Theremino_SlotViewer_Strings.exe
Load Theremino_SignalScope_M1.exe
Load Theremino_SignalScope_M2.exe
Load Theremino_SignalScope_M3.exe
'Load Theremino_SignalScope_M4.exe
'Load Theremino_SignalScope_M5.exe
Load Theremino_Cobot.exe
'WaitApplicationsStarted
Load Hide


' ╔════════════════════════════════════════════╗  █▀▀▀ ▀▀█▀▀ █▀▀▀█ █▀▀▀█ ▀▀█▀▀
' ║  EXECUTION START FROM HERE                 ║  ▀▀▀█   █   █▀▀▀█ █▀█▀▀   █
' ╚════════════════════════════════════════════╝  ▄▄▄█   █   █   █ █  ▀▄   █
ResetEncoders
YellowOn
Slot(Slot_SignalScope) = 1000

' ------------------------- GOTO HOME - Command to Cobot
'WaitCobotReady
'SendCommand(Slot_CommandsToCobot)(GotoHome)

'Home

Controls OpenTextBox
Controls ClearTextBox

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	CurrentMeter
	MotorFPS
	PositionDelta
	TestTemp
	'InterpretateVoiceCommands
	'Encoder
Goto Loop
Stop

Label PositionDelta
	Slot(109) = Slot(101) - Slot(102)
	Slot(209) = Slot(201) - Slot(202)
	Slot(309) = Slot(301) - Slot(302)
	Slot(409) = Slot(401) - Slot(402)
	Slot(509) = Slot(501) - Slot(502)
Return

Label TestTemp
	Variable Numeric TT1
	Variable Numeric TT2 = 0
	'TT2 = 0
	For TT1 = 1 To 3   '5
		TT2 = Max(TT2, Slot(TT1 * 100 + 6))
	Next
	Button MotorTemp Text "MaxT=" + TT2 + "°C"
Return

' ╔════════════════════════════════════════════╗
' ║ ENABLE/DISABLE BUTTONS                     ║
' ╚════════════════════════════════════════════╝
Label EnableAllButtons
	For v1 = 1 To 48
		Button v1 Enabled
	Next
Return

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


' ╔════════════════════════════════════════════╗
' ║  WAIT COBOT READY                          ║
' ╚════════════════════════════════════════════╝
Label WaitCobotReady
	Variable Numeric WCR1
	For WCR1 = 1 To Infinity
		Wait Seconds 0.05
		If SlotText(Slot_ResponsesFromCobot) = "Ready"
			Exit
		EndIf
	Next
Return

' ╔════════════════════════════════════════════╗
' ║  WAIT APPS STARTED                         ║
' ╚════════════════════════════════════════════╝
Label WaitApplicationsStarted
	Slot(Slot_RS485_FPS) = 0
	Wait Slot(Slot_RS485_FPS) > 10
	Wait Seconds 0.1
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	StopPump
'	GoHome
'	StopCobotAndGotoHome
End

Label StopCobotAndGotoHome
	StopCobot
	WaitMotorsNotMoving
	'GotoSafePosition
Return

Label GotoSafePosition
	SendCommand(Slot_CommandsToCobot)(GotoHome)
	WaitMotorsNotMoving
Return

Label WaitMotorsNotMoving
	Variable Numeric WMN1
	Variable Numeric WMN2
	Wait Seconds 0.1
	For WMN1 = 0 To Infinity
		WMN2 = Slot(Slot_M1 + 6)
		WMN2 = WMN2 + Slot(Slot_M2 + 6)
		WMN2 = WMN2 + Slot(Slot_M3 + 6)
		WMN2 = WMN2 + Slot(Slot_M4 + 6)
		WMN2 = WMN2 + Slot(Slot_M5 + 6)
		If WMN2 = 0
			Exit For
		EndIf
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL      (when pressing X on form)   ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	StopPump
'	GoHome
'	StopCobotAndGotoHome
	Load CloseAll
End


' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
Label SendCommand
	Variable Numeric SC1       '<-- Parameter Destinat.-Slot
	Variable String  SC2       '<-- Parameter Command-String
	Variable Numeric SC3 = 0.5 '<-- The timeout in seconds
	Variable Numeric SC4
	SlotText SC1 = SC2
	For SC4 = 0.1 To SC3 Step 0.1
		If SlotText(SC1) = ""
			Exit
		EndIf
		Wait Seconds 0.1
	Next
Return


' =============================================================
'  COMMANDS to Cobot
' =============================================================
Label ExecCobot
	SendCommand(Slot_CommandsToCobot)(EnableRepeat)
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label StopCobot
	SendCommand(Slot_CommandsToCobot)(StopExecution)
Return

' =============================================================
'  CALIBRATION COMMANDS to Motors and Cobot
' =============================================================
Label SetCenterHere
	SendCommand(Slot_CommandsToMotors)("ResetAllCalibrationValues")
	'Wait Seconds 0.1
	SendCommand(Slot_CommandsToMotors)("ResetAllMultiturnValues")
	'Wait Seconds 0.1
	SendCommand(Slot_CommandsToCobot) ("SetAllMotorsCenterHere")
	'Wait Seconds 0.1
	SendCommand(Slot_CommandsToCobot) ("CopyPositionsToDestinations")
	'Wait Seconds 0.1
Return

Label CalibrateToZero
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 1 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 2 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 3 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 4 To 0")
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 5 To 0")
	SendCommand(Slot_CommandsToCobot) ("CopyPositionsToDestinations")
Return

Label CalibrateMotor1To180
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor 1 To 180")
	SendCommand(Slot_CommandsToCobot)("CopyPositionsToDestinations")
Return


' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label CurrentMeter
	Variable Numeric mA
	mA = Slot(108) + Slot(208) + Slot (308) ' <-- Motors slots
	mA = FilterLP(mA, 0.1, 1) '<-- Filter 0.5 seconds / 1 pole
	Button CurrentMeter Text "Current= " + Str(mA, "0 mA")
Return

Label MotorFPS
	Button MotorFPS Text "FPS=" + Str(Slot(Slot_RS485_FPS))
Return

Label StartPump
	RedOn
	'Slot(Slot_SignScope) = 1000
	Slot(Slot_Pump) = 1000
	Button StartPump Disabled
	Button StopPump Enabled
	Wait Seconds 0.4
	
	'If Slot(Slot_Vacuum) > 56
	'	'Beep "500, 300, 0, 300"
	'	'SayVacuumProblem
	'	RedOn
	'	StopPump
	'	Print "Vacuum Problem" + CRLF
	'	'CobotShowPoint
	'	Print "Execution stopped" + CRLF
	'	RedBlink
	'	End
	'Else
	'	Print "Target picked"
	'EndIf
Return

Label StopPump
	Slot(Slot_Pump) = 0
	Button StopPump Disabled
	Button StartPump Enabled
	GreenOn
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 = 900
	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

Label RedBlink
	RedOn
	WhiteOn
Return

'' ╔════════════════════════════════════════════╗ █  █ █▀▀█ █▄ ▄█ █▀▀▀
'' ║ HOME                                       ║ █▀▀█ █  █ █ ▀ █ █▀▀
'' ╚════════════════════════════════════════════╝ █  █ █▄▄█ █   █ █▄▄▄
'Label Home
'	BlueOn
'	'GoHome
'	WaitMotorsNotMoving
'	Home4
'	Home5
'	Home1
'	Home2
'	Home3
'	GoCenter
'Return
'
'Variable Numeric HM
'Label Home1
'	'DisableAllButtons
'	Button Home1 Color Flashing6 Blue White
'	Slot(Slot_SignalScope) = 1000
'	Slot(Slot_M1) = Slot(Slot_M1) - 5
'	WaitMotorsNotMoving
'	SetCenterHere
'	CalibrateToZero   '===============================CalibrateM1ToZero
'	For HM = 1 To Infinity
'		Slot(Slot_M1) = Slot(Slot_M1) + 0.05  ' do not change
'		If Abs(Slot(Slot_M1 + 3)) > 300
'			Exit
'		EndIf
'		Wait Seconds 0.01
'	Next
'	WaitMotorsNotMoving
'	Slot(Slot_M1) = Slot(Slot_M1) - 5
'	WaitMotorsNotMoving
'	CalibrateToZero      '===============================CalibrateM1ToZero
'	EnableAllButtons
'	Slot(Slot_SignalScope) = 0
'	Button Home1 Color Yellow
'Return
'
'Label Home2
'	'DisableAllButtons
'	Button Home2 Color Flashing6 Blue White
'	Slot(Slot_SignalScope) = 1000
'	Slot(Slot_M3) = Slot(Slot_M3) + 5
'	Slot(Slot_M2) = Slot(Slot_M2) - 5
'	WaitMotorsNotMoving
'	SetCenterHere
'	CalibrateToZero
'	For HM = 1 To Infinity
'		Slot(Slot_M2) = Slot(Slot_M2) + 0.08
'		If Abs(Slot(Slot_M2 + 3)) > 400
'			Exit
'		EndIf
'		Wait Seconds 0.01
'	Next
'	WaitMotorsNotMoving
'	Slot(Slot_M2) = Slot(Slot_M2) - 5
'	WaitMotorsNotMoving
'	CalibrateToZero
'	Slot(Slot_SignalScope) = 0
'	EnableAllButtons
'	Button Home2 Color Yellow
'Return
'
'Label Home3
'	'DisableAllButtons
'	Button Home3 Color Flashing6 Blue White
'	Slot(Slot_SignalScope) = 1000
'	Slot(Slot_M3) = Slot(Slot_M3) + 5
'	WaitMotorsNotMoving
'	SetCenterHere
'	CalibrateToZero
'	For HM = 1 To Infinity
'		Slot(Slot_M3) = Slot(Slot_M3) - 0.08
'		If Abs(Slot(Slot_M3 + 3)) > 300
'			Exit
'		EndIf
'		Wait Seconds 0.01
'	Next
'	WaitMotorsNotMoving
'	Slot(Slot_M3) = Slot(Slot_M3) + 5
'	WaitMotorsNotMoving
'	CalibrateToZero
'	Slot(Slot_SignalScope) = 0
'	EnableAllButtons
'	Button Home3 Color Yellow
'Return
'
'Label Home4
'	'DisableAllButtons
'	Button Home4 Color Flashing6 Blue White
'	Slot(Slot_SignalScope) = 1000
'	Slot(Slot_M4) = Slot(Slot_M4) - 5
'	WaitMotorsNotMoving
'	SetCenterHere
'	CalibrateToZero
'	For HM = 1 To Infinity
'		Slot(Slot_M4) = Slot(Slot_M4) + 0.2
'		If Abs(Slot(Slot_M4 + 3)) > 300
'			Exit
'		EndIf
'		Wait Seconds 0.01
'	Next
'	WaitMotorsNotMoving
'	Slot(Slot_M4) = Slot(Slot_M4) - 5
'	WaitMotorsNotMoving
'	SetCenterHere
'	CalibrateToZero
'	Slot(Slot_SignalScope) = 0
'	EnableAllButtons
'	Button Home4 Color Yellow
'Return
'
'Label Home5
'	'DisableAllButtons
'	Button Home5 Color Flashing6 Blue White
'	Slot(Slot_SignalScope) = 1000
'	Slot(Slot_M5) = Slot(Slot_M5) - 5
'	WaitMotorsNotMoving
'	SetCenterHere
'	CalibrateToZero
'	For HM = 1 To Infinity
'		Slot(Slot_M5) = Slot(Slot_M5) + 0.2
'		If Abs(Slot(Slot_M5 + 3)) > 300
'			Exit
'		EndIf
'		Wait Seconds 0.01
'	Next
'	WaitMotorsNotMoving
'	Slot(Slot_M5) = Slot(Slot_M5) - 5
'	WaitMotorsNotMoving
'	SetCenterHere
'	CalibrateToZero
'	Slot(Slot_SignalScope) = 0
'	EnableAllButtons
'	Button Home5 Color Yellow
'Return
'
Label GoHome
	'Slot(Slot_M4) =  0
	'Slot(Slot_M5) =  0
	'Slot(Slot_M2) =  0
	'Slot(Slot_M3) =  0
	'Slot(Slot_M2) =  0
	'Slot(Slot_M1) =  0
Return

Label GoCenter
	Slot(Slot_M1) = 0
	Slot(Slot_M2) = 0
	Slot(Slot_M3) = 0
	Slot(Slot_M4) = 0
	Slot(Slot_M5) = 0
Return


' ╔═════════════════════════╗  █▀▀▀ █▀▀█ █▄ ▄█   █▀▀  █▀▀▀ █▀▀█ █▀█
' ║  COMMANDS FROM COBOT    ║  █    █  █ █ ▀ █   █▀   █    █  █ █▀▀█
' ╚═════════════════════════╝  █▄▄▄ █▄▄█ █   █ ▄ █  ▄ █▄▄▄ █▄▄█ █▄▄█
Label Event_ExternalCommands
	' ------------------------------------------------------ Do not print lines containing ''
	'Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select CommandText
			' ---------------------------------------------- ACTIONS
		Case "Beep"
			Beep 880-100
			ClearCommand
		Case "Pause"
			Wait Seconds GetSeparatedString(CommandText, 1)
			ClearCommand
		Case "StartPump"
			StartPump
			ClearCommand
		Case "StopPump"
			StopPump
			ClearCommand
		Case "MemorizeQrCode"
			Variable Numeric MQR1
			For MQR1 = 1 To 5
				QrCode = SlotText(Slot_ResponsesFromQRDecode)
				
				Print QrCode
				If QrCode <> ""
					Exit
				EndIf
				If MQR1 = 5
					SendCommand(Slot_CommandsToCobot)(StopExecution)
				EndIf
				Wait Seconds 1
			Next
			ClearCommand
		Case "ExecQrCode"
			ExecQRcode
			ClearCommand
		Case "StartFichesPick"
			StartFichesPick
			ClearCommand
		Case "EndOfTask"
			EndOfTask
			ClearCommand
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + CommandText + CRLF
			Beep
			ClearCommand
			Stop								'
	EndSelect
Return

Label ClearCommand
	SlotText(Slot_ExternalCommands) = ""
Return


' ╔════════════════════════════════════════════╗ █   █ █▀▀█ █▄ █
' ║ WANPY                                      ║ █   █ █▄▄█ █ ▀█
' ╚════════════════════════════════════════════╝  ▀▄▀  █  █ █  █
Label WanpyJob
	TTS Speak "Wanpy job started."
	Controls OpenTextBox
	Controls ClearTextBox
	StartTime = ElapsedTime
	RedOn
	Wanpy_A5toB1
Return

Variable String Status = "A5toB1"
Label EndOfTask
	PrintTime
	StartTimePartial = ElapsedTime
	Select Status
		Case "A5toB1"
			'Wanpy_B1toA5
			Wanpy_A4toB2
		Case "A4toB2"
			Wanpy_A3toB3
		Case "A3toB3"
			Wanpy_A2toB4
		Case "A2toB4"
			Wanpy_A1toB5
		Case "A1toB5"
			Wanpy_B5toA1
		Case "B5toA1"
			Wanpy_B4toA2
		Case "B4toA2"
			Wanpy_B3toA3
		Case "B3toA3"
			Wanpy_B2toA4
		Case "B2toA4"
			Wanpy_B1toA5
		Case "B1toA5"
			StartTime = ElapsedTime
			Print CRLF
			Wanpy_A5toB1
	EndSelect
Return

Variable Numeric StartTime
Variable Numeric StartTimePartial
Variable String Time
Label PrintTime
	Time = "Partial time = "
	Time = Time + PadLeft(Format(ElapsedTime - StartTimePartial, "0.0"),4)
	Time = Time + "   Total time = "
	Time = Time + PadLeft(Format(ElapsedTime - StartTime, "0.0"),4)
	Print Status + "  " + Time + CRLF
Return

Label Wanpy_A5toB1
	Status = "A5toB1"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_A5toB1.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_A4toB2
	Status = "A4toB2"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_A4toB2.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_A3toB3
	Status = "A3toB3"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_A3toB3.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_A2toB4
	Status = "A2toB4"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_A2toB4.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_A1toB5
	Status = "A1toB5"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_A1toB5.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_B5toA1
	Status = "B5toA1"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_B5toA1.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_B4toA2
	Status = "B4toA2"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_B4toA2.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_B3toA3
	Status = "B3toA3"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_B3toA3.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_B2toA4
	Status = "B2toA4"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_B2toA4.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label Wanpy_B1toA5
	Status = "B1toA5"
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Wanpy_B1toA5.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

' ╔════════════════════════════════════════════╗ █▀▀█  █▀▀█   █▀▀▀
' ║ QR CODE                                    ║ █  █  █▄▄█   █
' ╚════════════════════════════════════════════╝ █▄██▄ █ ▀▄   █▄▄▄
Label JobQrCode
	TTS Speak "QR code recognition and action related is started."
	StartFichesPick
Return

Label StartFichesPick
	BlueOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence fiches_pick.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label ExecQRcode
	Variable String QrCode
	Select QrCode
		Case "WHITE"
			WhiteOn
			SendCommand(Slot_CommandsToCobot)("LoadSequence Fiches_place_white.txt")
			SendCommand(Slot_CommandsToCobot)(SelectLine 1)
			SendCommand(Slot_CommandsToCobot)(StartExecution)
		Case "RED"
			RedOn
			SendCommand(Slot_CommandsToCobot)("LoadSequence Fiches_place_red.txt")
			SendCommand(Slot_CommandsToCobot)(SelectLine 1)
			SendCommand(Slot_CommandsToCobot)(StartExecution)
		Case "GREEN"
			GreenOn
			SendCommand(Slot_CommandsToCobot)("LoadSequence Fiches_place_green.txt")
			SendCommand(Slot_CommandsToCobot)(SelectLine 1)
			SendCommand(Slot_CommandsToCobot)(StartExecution)
		Case "BLUE"
			BlueOn
			SendCommand(Slot_CommandsToCobot)("LoadSequence Fiches_Place_blue.txt")
			SendCommand(Slot_CommandsToCobot)(SelectLine 1)
			SendCommand(Slot_CommandsToCobot)(StartExecution)
		Case "BLACK"
			RgbOff
			SendCommand(Slot_CommandsToCobot)("LoadSequence Fiches_place_black.txt")
			SendCommand(Slot_CommandsToCobot)(SelectLine 1)
			SendCommand(Slot_CommandsToCobot)(StartExecution)
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized color command: " + CommandText + CRLF
			StopPump
			Beep
			ClearCommand
			Stop
	EndSelect
Return

' ╔════════════════════════════════════════════╗ ▀▀█▀▀ █  █ █▄ █
' ║ TUNA                                       ║   █   █  █ █ ▀█
' ╚════════════════════════════════════════════╝   █   █▄▄█ █  █
Label TunaCans
	TTS Speak "Tuna can handling."
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Tuna_Cans.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return


' ╔════════════════════════════════════════════╗ █▀▀▀ █   █ █▀▀▄
' ║   SLIDERS                                  ║ ▀▀▀█ █   █ █  █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄ █ █▄▄█
Variable Numeric SLD
Label Sliders
	BlueOn
	'Button Sliders Color Flashing7 Blue White
	For SLD = 1 To Infinity
		Slot(Slot_M1)  = Slot(Slot_Slider1)
		Slot(Slot_M2)  = Slot(Slot_Slider2)
		Slot(Slot_M3)  = Slot(Slot_Slider3) + Slot(Slot_Slider2)
		Slot(Slot_M4)  = Slot(Slot_Slider4)
		Slot(Slot_M5)  = Slot(Slot_Slider5)
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀█ ▀▄ ▄▀
' ║  SAY                                       ║ ▀▀▀█ █▄▄█  ▀▄▀
' ╚════════════════════════════════════════════╝ ▄▄▄█ █  █   █
Label Say
	TTS SelectVoice "ENGF"
	TTS Speak "Today is a beautiful day because we spend time together."
	Wait TTSready
	
	TTS SelectVoice "ITA FEM"
	TTS Speak "Oggi è una bella giornata perche passeremo del tempo insieme."
	Wait TTSready
	
	TTS SelectVoice "JPN"
	TTS Speak "私たちは一緒に時間を過ごすので、今日は美しい日です。"
	Wait TTSready
	
	TTS SelectVoice "ZHO"
	TTS Speak "今天是美好的一天，因为我们共度时光。"
	Wait TTSready
Return

' ╔════════════════════════════════════════════╗ █   █ █▀▀█ █ █▀▀▀ █▀▀▀
' ║  VOICE COMMANDS                            ║  █ █  █  █ █ █    █▀▀
' ╚════════════════════════════════════════════╝   █   █▄▄█ █ █▄▄▄ █▄▄▄
Label Voice
	ClearVoiceCommand
	If VoiceCommandsStatus = "active"
		VoiceCommandsStatus = ""
		'Button Voice Color PaleTurquoise
	Else
		VoiceCommandsStatus = "active"
		'Button Voice Color Flashing7 Orange White
	EndIf
Return

Variable String Word
Variable String VoiceCommandsStatus
Label InterpretateVoiceCommands
	If VoiceCommandsStatus = "active"
		If SlotText(Slot_WordRecognized) <> ""
			Word = SlotText(Slot_WordRecognized)
			Print Word + " "
			Interpretate
			ClearVoiceCommand
		EndIf
	EndIf
Return

Variable String ShowingType
Label Interpretate
	Select Word
		Case "show.fiches"
			If ShowingType = ""
				WhiteOn
				ShowingType = "Fiches"
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Show_Fiches.txt")
				SendCommand(Slot_CommandsToCobot)(SelectLine 1)
				SendCommand(Slot_CommandsToCobot)(StartExecution)
			EndIf
		Case "show.wanpy"
			If ShowingType = ""
				YellowOn
				ShowingType = "Wanpy"
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Show_Wanpy.txt")
				SendCommand(Slot_CommandsToCobot)(SelectLine 1)
				SendCommand(Slot_CommandsToCobot)(StartExecution)
			EndIf
		Case "show.tuna"
			If ShowingType = ""
				GreenOn
				ShowingType = "Tuna"
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Show_Tuna.txt")
				SendCommand(Slot_CommandsToCobot)(SelectLine 1)
				SendCommand(Slot_CommandsToCobot)(StartExecution)
			EndIf
			'-------------------------------------------------------------------------
		Case "release"
			Select ShowingType
				Case "fiches"
					WhiteOn
					SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Release_Fiches.txt")
					SendCommand(Slot_CommandsToCobot)(SelectLine 1)
					SendCommand(Slot_CommandsToCobot)(StartExecution)
					ShowingType = ""
				Case "wanpy"
					YellowOn
					SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Release_Wanpy.txt")
					SendCommand(Slot_CommandsToCobot)(SelectLine 1)
					SendCommand(Slot_CommandsToCobot)(StartExecution)
					ShowingType = ""
				Case "tuna"
					GreenOn
					SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Release_Tuna.txt")
					SendCommand(Slot_CommandsToCobot)(SelectLine 1)
					SendCommand(Slot_CommandsToCobot)(StartExecution)
					ShowingType = ""
				CaseElse
					'
			EndSelect
			'-------------------------------------------------------------------------
		Case "goto.center"
			If ShowingType = ""
				RedOn
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_goto_Center.txt")
				SendCommand(Slot_CommandsToCobot)(SelectLine 1)
				SendCommand(Slot_CommandsToCobot)(StartExecution)
			EndIf
		Case "goto.home"
			If ShowingType = ""
				MagentaOn
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_goto_Home.txt")
				SendCommand(Slot_CommandsToCobot)(SelectLine 1)
				SendCommand(Slot_CommandsToCobot)(StartExecution)
			EndIf
			'-------------------------------------------------------------------------
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + Word + CRLF
			Beep
			ClearCommand
			Stop
	EndSelect
Return

Label ClearVoiceCommand
	SlotText(Slot_WordRecognized) = ""
Return


'============================= TRIALS ====================
Label ShowFiches
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Show_Fiches.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label ReleaseFiches
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Release_Fiches.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label ShowTuna
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Show_Tuna.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label ReleaseTuna
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Release_Tuna.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label ShowWanpy
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Show_Wanpy.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Label ReleaseWanpy
	YellowOn
	SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_Release_Wanpy.txt")
	SendCommand(Slot_CommandsToCobot)(SelectLine 1)
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

Variable Numeric VOL
Label VoiceLoop
	For VOL = 1 To Infinity
		ShowFiches
		Wait Seconds 1
		ReleaseFiches
		Wait Seconds 1
		ShowTuna
		Wait Seconds 1
		ReleaseTuna
		Wait Seconds 1
		ShowWanpy
		Wait Seconds 1
		ReleaseWanpy
		Wait Seconds 1
	Next
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ █▄ █ █▀▀▀
' ║ ENCODERS                                   ║ █▀▀  █ ▀█ █
' ╚════════════════════════════════════════════╝ █▄▄▄ █  █ █▄▄▄
Variable Numeric EncoderSteps1
Variable Numeric OldEncoderSteps1
Variable Numeric Difference1
Variable Numeric EncoderSteps2
Variable Numeric OldEncoderSteps2
Variable Numeric Difference2
Variable Numeric EncoderSteps3
Variable Numeric OldEncoderSteps3
Variable Numeric Difference3
Variable Numeric EncoderSteps4
Variable Numeric OldEncoderSteps4
Variable Numeric Difference4
Variable Numeric EncoderSteps5
Variable Numeric OldEncoderSteps5
Variable Numeric Difference5

Label Encoder
	'------------------------------------------------ read encoder1
	EncoderSteps1 = Slot(Slot_Encoder1)
	Difference1 = EncoderSteps1 - OldEncoderSteps1
	OldEncoderSteps1 = EncoderSteps1
	If Difference1 > 32000
		Difference1 = Difference1 - 65536
	EndIf
	If Difference1 < -32000
		Difference1 = Difference1 + 65536
	EndIf
	If Slot(Slot_EncoderSlow) < 500
		Difference1 = Difference1 * 0.1
	EndIf
	Slot(Slot_M1) = Limit(Slot (Slot_M1) + Difference1, -70, 0)
	
    '------------------------------------------------ read encoder2
	EncoderSteps2 = Slot(Slot_Encoder2)
	Difference2 = EncoderSteps2 - OldEncoderSteps2
	OldEncoderSteps2 = EncoderSteps2
	If Difference2 > 32000
		Difference2 = Difference2 - 65536
	EndIf
	If Difference2 < -32000
		Difference2 = Difference2 + 65536
	EndIf
	If Slot(Slot_EncoderSlow) < 500
		Difference2 = Difference2 * 0.1
	EndIf
	Slot(Slot_M2) = Limit(Slot (Slot_M2) + Difference2, -160, 0)
	
    '------------------------------------------------ read encoder3
	EncoderSteps3 = Slot(Slot_Encoder3)
	Difference3 = EncoderSteps3 - OldEncoderSteps3
	OldEncoderSteps3 = EncoderSteps3
	If Difference3 > 32000
		Difference3 = Difference3 - 65536
	EndIf
	If Difference3 < -32000
		Difference3 = Difference3 + 65536
	EndIf
	If Slot(Slot_EncoderSlow) < 500
		Difference3 = Difference3 * 0.1
	EndIf
	Slot(Slot_M3) = Limit(Slot (Slot_M3) + Difference3, -100, 100)
	
	'------------------------------------------------ read encoder4
	EncoderSteps4 = Slot(Slot_Encoder4)
	Difference4 = EncoderSteps4 - OldEncoderSteps4
	OldEncoderSteps4 = EncoderSteps4
	If Difference4 > 32000
		Difference4 = Difference4 - 65536
	EndIf
	If Difference4 < -32000
		Difference4 = Difference4 + 65536
	EndIf
	If Slot(Slot_EncoderSlow) < 500
		Difference4 = Difference4 * 0.1
	EndIf
	Slot(Slot_M4) = Limit(Slot (Slot_M4) + Difference4, -180, 0)
	
	'------------------------------------------------ read encoder5
	EncoderSteps5 = Slot(Slot_Encoder5)
	Difference5 = EncoderSteps5 - OldEncoderSteps5
	OldEncoderSteps5 = EncoderSteps5
	If Difference5 > 32000
		Difference5 = Difference5 - 65536
	EndIf
	If Difference5 < -32000
		Difference5 = Difference5 + 65536
	EndIf
	If Slot(Slot_EncoderSlow) < 500
		Difference5 = Difference5 * 0.1
	EndIf
	Slot(Slot_M5) = Limit(Slot (Slot_M5) + Difference5, -240, 0)
Return

Label ResetEncoders
	OldEncoderSteps1 = Slot(Slot_Encoder1)
	OldEncoderSteps2 = Slot(Slot_Encoder2)
	OldEncoderSteps3 = Slot(Slot_Encoder3)
	OldEncoderSteps4 = Slot(Slot_Encoder4)
	OldEncoderSteps5 = Slot(Slot_Encoder5)
Return


