'╔═══════════════════════════════════════════════════════════╗
'║     Name: MotorCalib_Zero_4_LIVIO.txt   Date: 13 Dic 2022 ║
'║   Author: Leonardo                                        ║
'║    Scope: Test all the functionality of the PVC-150       ║
'╠═══════════════════════════════════════════════════════════╣
'║Hardware: 5 FeeTech STS3215   new MOTOR                    ║
'╟───────────────────────────────────────────────────────────╢
'║    Note: Vacuum motor and valve 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_FiveButton  =   26
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_SetWayPoint =  14

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_M1pos       = 102
Variable Numeric Slot_M2pos       = 202
Variable Numeric Slot_M3pos       = 302
Variable Numeric Slot_M4pos       = 402
Variable Numeric Slot_M5pos       = 502

Variable Numeric Slot_M1center    = 123

Variable Numeric Slot_MotorsFPS   =  56

' ╔════════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                    ║ █▀▀█ █  █   █
' ╚════════════════════════════════════════════╝ █▄▄█ █▄▄█   █
Button 1 Label MotorFPS
Button MotorFPS Text "NO MOTORS"
Button MotorFPS Color Flashing7 Yellow DarkOrange

Button 2 Label "MotorTemp"

Button 3 Label CurrentMeter

Button 4 Label GoTo12_70
Button GoTo12_70 Text "GoTo 12 70"
Button GoTo12_70 Color Yellow

Button 5 Label GoToHome
Button GoToHome Text "GoTo Home"
Button GoToHome Color Yellow

Button 6 Label GoTo45
Button GoTo45 Text "GoTo 45"
Button GoTo45 Color Gold

Button 7 Label GoTo70
Button GoTo70 Text "GoTo 70"
Button GoTo70 Color Yellow

Button 8 Label CopyPositionsToDestinations
Button CopyPositionsToDestinations  Text "Copy Positions" + CRLF + "To Destinations"
Button CopyPositionsToDestinations  Color Orange

'Button 8 Label ActivateMotors
'Button ActivateMotors  Text "Activate" + CRLF + "Motors"
'Button ActivateMotors  Color Orange

'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 2 Label JobQrCode
'Button JobQrCode Text "JOB QRcode"
'Button JobQrCode Color Cyan
'
'Button 4 Label Sliders
'Button Sliders Text "Sliders"
'Button Sliders Color Cyan

'Button 6 Label Voice
'Button Voice Text "Voice"
'Button Voice Color PaleTurquoise

Button 10 Label TunaCans
Button TunaCans Text "Tuna cans"
Button TunaCans Color Indigo

Button 11 Label WanpyJob5
Button WanpyJob5 Text "Wanpy Job 5"
Button WanpyJob5 Color Indigo

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

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

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

Button 23 Label RecognizeEncoder
Button RecognizeEncoder  Text "Recognize" + CRLF + "Encoder"
Button RecognizeEncoder  Color LightBlue

Button 24 Label CloseScopes
Button CloseScopes Text "Close Scopes"
Button CloseScopes Color SkyBlue

Button 25 Label OpenScope_1
Button OpenScope_1 Text "Open Scope 1"
Button OpenScope_1 Color PaleTurquoise

Button 26 Label OpenScope_2
Button OpenScope_2 Text "Open Scope 2"
Button OpenScope_2 Color PaleTurquoise

Button 27 Label OpenScope_3
Button OpenScope_3 Text "Open Scope 3"
Button OpenScope_3 Color PaleTurquoise

Button 28 Label OpenScope_4
Button OpenScope_4 Text "Open Scope 4"
Button OpenScope_4 Color PaleTurquoise

Button 29 Label OpenScope_5
Button OpenScope_5 Text "Open Scope 5"
Button OpenScope_5 Color PaleTurquoise

Button 30 Label OpenFPS
Button OpenFPS Text "Open FPS"
Button OpenFPS Color PaleTurquoise

Button 31 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_Motors.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_Cobot.exe
'WaitApplicationsStarted
Load Hide


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

'DisableHomeButtons
' ------------------------- GOTO Homing - Command to Cobot
'WaitCobotReady
'SendCommand(Slot_CommandsToCobot)(GotoHoming)

'Homing

'Controls OpenTextBox
'Controls ClearTextBox

ActivateMotors

Loop

Stop

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	CurrentMeter
	ShowMotorFPS
	PositionDelta
	TestTemp
	'InterpretateVoiceCommands
	Encoder
	FiveButton
	TestPedal
	'If Slot(Slot_MotorsFPS) < 5
	'	Print "The robot is not in POWER ON or serial port is not correct."
	'	End
	'EndIf
	'If Slot(0) = -2
	'	Print "Master disconnected."
	'	End
	'EndIf
Goto Loop


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_MotorsFPS) = 0
	Wait Slot(Slot_MotorsFPS) > 10
	Wait Seconds 0.1
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	StopPump
	StopCobot
	Slot(Slot_SignalScope) = 0
'	GoToShow
'	StopCobotAndGotoHoming
End

'Label StopCobotAndGotoHoming
'	'StopCobot
'	WaitMotorsNotMoving
'	'GotoSafePosition
'Return

Label GotoSafePosition
	SendCommand(Slot_CommandsToCobot)(GotoHoming)
	WaitMotorsNotMoving
Return


' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL      (when pressing X on form)   ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	StopPump
	StopCobot
	Slot(Slot_SignalScope) = 0
'	GoToShow
'	StopCobotAndGotoHoming
	Load CloseAll
End


' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
Label SendCommand			'(Destination Slot)(Command String)
	Variable Numeric SC1
	Variable String  SC2
	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

Label ArrowUpCobot
	SendCommand(Slot_CommandsToCobot)(ArrowUp)
Return

Label ArrowDownCobot
	SendCommand(Slot_CommandsToCobot)(ArrowDown)
Return

Label ExecuteCobotFromSelectedLine
	SendCommand(Slot_CommandsToCobot)(StartExecution)
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label EnableHomeButtons
	For v1 = 17 To 22
		Button v1 Enabled
	Next
Return

Label DisableHomeButtons
	For v1 = 17 To 22
		Button v1 Disabled
	Next
Return

Label CurrentMeter
	Variable Numeric mA
	mA = Slot(108) + Slot(208) + Slot (308) + Slot (408) + Slot (508)
	mA = FilterLP(mA, 0.1, 1) '<-- Filter 0.5 seconds / 1 pole
	Button CurrentMeter Text "Current " + Str(mA, "0 mA")
Return

Label ShowMotorFPS
	Variable String MotorsAreEnabled
	Variable Numeric SMF1 = Slot(Slot_MotorsFPS)
	If SMF1 > 30
		If MotorsAreEnabled <> "TRUE"
			MotorsAreEnabled = "TRUE"
			Button MotorFPS Color GreenYellow
		EndIf
		Button MotorFPS Text "FPS " + Str(SMF1)
	Else
		If MotorsAreEnabled = "TRUE"
			MotorsAreEnabled = "FALSE"
			Button MotorFPS Color Flashing7 Yellow DarkOrange
			Button MotorFPS Text "NO MOTORS"
		EndIf
	EndIf
Return

Label MotorFPS
	If MotorsAreEnabled = "TRUE"
		SendCommand(Slot_CommandsToMotors)(DisableConnection)
	Else
		SendCommand(Slot_CommandsToMotors)(EnableConnection)
	EndIf
Return

Label StartPump
	BlueOn
	'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

Label GoTo70
	MoveMotorTo(4)(70)(0.1)
	MoveMotorTo(1)(70)(0.1)
	MoveMotorTo(3)(70)(0.1)
	MoveMotorTo(2)(70)(0.1)
	MoveMotorTo(5)(70)(0.1)
Return
' nota: quando si muove il 2, il 3 non deve mai avere
'       un valore superiore a 50 altrimenti salta

Label GoTo12_70
	MoveMotorTo(4)(12)(0.1)
	MoveMotorTo(5)(70)(0.1)
	MoveMotorTo(1)(12)(0.1)
	MoveMotorTo(3)(12)(0.1)						 'Breakpoint
	MoveMotorTo(2)(12)(0.1)
	MoveMotorTo(3)(12)(0.1)
Return

Label GoToHome
	MoveMotorTo(1)(10)(0.1)
	MoveMotorTo(2)(10)(0.1)
	MoveMotorTo(3)(125)(0.1)
	MoveMotorTo(5)(120)(0.1)
	MoveMotorTo(4)(10)(0.1)
Return

Label GoTo45
	MoveMotorTo(4)(45)(0.1)
	MoveMotorTo(1)(45)(0.1)
	MoveMotorTo(2)(45)(0.1)
	MoveMotorTo(3)(45)(0.1)
	MoveMotorTo(5)(135)(0.1)
	MoveMotorTo(4)(45)(0.1)
Return

Variable Numeric TestPedalEnabled = 1
Label TestPedal
	If TestPedalEnabled = 1
		If Slot(Slot_Pedal) > 500
			Slot(Slot_SetWayPoint) = 1000
		Else
			Slot(Slot_SetWayPoint) = 0
		EndIf
	EndIf
Return

' ╔════════════════════════════════════════════╗ █▀▀   █▀█  █  █ ▀▀█▀▀
' ║ FIVE PUSH BUTTONS                          ║ ▀▀█   █▀▀█ █  █   █
' ╚════════════════════════════════════════════╝ ▄▄█   █▄▄█ █▄▄█   █
Variable String PressedButton
Label FiveButton
	Variable Numeric FB0
	Variable Numeric FB1
	Variable Numeric FB2
	' ------------------------------ Read the value two times
	FB1 = Slot(Slot_FiveButton)
	FB2 = Slot(Slot_FiveButton)
	' ------------------------------ If similar values
	If Abs(FB2 - FB1) < 5
		' -------------------------- If armed
		If FB0 = 0
			FB0 = 1
			Select True
				Case FB1 > 500
					TestPedalEnabled = 1
					Print ""
					PressedButton = ""
				Case FB1 > 250
					Print "red"
					PressedButton = "red"
					ExecCobot
				Case FB1 > 120
					Print "white"
					PressedButton = "white"
					ExecuteCobotFromSelectedLine
				Case FB1 > 56
					Print "green"
					PressedButton = "green"
					ArrowDownCobot
				Case FB1 > 15
					Print "blue"
					PressedButton = "blue"
					ArrowUpCobot
				CaseElse
					Print "yellow"
					PressedButton = "yellow"
					TestPedalEnabled = 0
					Slot(Slot_SetWayPoint) = 1000
			EndSelect
		EndIf
	EndIf
	' ------------------------------ Rearm buttons
	If FB1 > 500
		FB0 = 0
	EndIf
Return

Label CloseScopes
	Load CloseApps Theremino_SignalScope_M1.exe
	Load CloseApps Theremino_SignalScope_M2.exe
	Load CloseApps Theremino_SignalScope_M3.exe
	Load CloseApps Theremino_SignalScope_M4.exe
	Load CloseApps Theremino_SignalScope_M5.exe
	Load CloseApps Theremino_SignalScope_FPS.exe
Return

Label OpenScope_1
	Load Theremino_SignalScope_M1.exe
Return

Label OpenScope_2
	Load Theremino_SignalScope_M2.exe
Return

Label OpenScope_3
	Load Theremino_SignalScope_M3.exe
Return

Label OpenScope_4
	Load Theremino_SignalScope_M4.exe
Return

Label OpenScope_5
	Load Theremino_SignalScope_M5.exe
Return

Label OpenFPS
	Load Theremino_SignalScope_FPS.exe
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


' ╔═════════════════════════╗  █▀▀▀ █▀▀█ █▄ ▄█   █▀▀  █▀▀▀ █▀▀█ █▀█
' ║  COMMANDS FROM COBOT    ║  █    █  █ █ ▀ █   █▀   █    █  █ █▀▀█
' ╚═════════════════════════╝  █▄▄▄ █▄▄█ █   █ ▄ █  ▄ █▄▄▄ █▄▄█ █▄▄█
Label Event_ExternalCommands
	' ------------------------------------------------------ Do not print lines containing ''
	'Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select CommandText
			' ---------------------------------------------- ACTIONS
		Case "RedOn"
			RedOn
			ClearCommand
		Case "GreenOn"
			GreenOn
			ClearCommand
		Case "BlueOn"
			BlueOn
			ClearCommand
		Case "WhiteOn"
			WhiteOn
			ClearCommand
		Case "MagentaOn"
			MagentaOn
			ClearCommand
		Case "YellowOn"
			YellowOn
			ClearCommand
		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 5                                    ║ █   █ █▄▄█ █ ▀█   ▀▀▀█
' ╚════════════════════════════════════════════╝  ▀▄▀  █  █ █  █   ▄▄▄█
Label WanpyJob5
	TTS Speak "Wanpy job started."
	Controls OpenTextBox
	Controls ClearTextBox
	StartTime = ElapsedTime
	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.Homing"
			If ShowingType = ""
				MagentaOn
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_goto_Homing.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

' ╔════════════════════════════════════════════╗ ▀▀█▀▀ █▀▀█ █ █▀▀█ █
' ║ T R I A L S                                ║   █   █▄▄▀ █ █▄▄█ █
' ╚════════════════════════════════════════════╝   █   █ ▀▄ █ █  █ █▄▄


' ╔════════════════════════════════════════════╗ █▀▀▀ █▄ █ █▀▀▀
' ║ ENCODERS                                   ║ █▀▀  █ ▀█ █
' ╚════════════════════════════════════════════╝ █▄▄▄ █  █ █▄▄▄
Variable Numeric EncoderSteps1
Variable Numeric OldEncoderSteps1 = Slot(Slot_Encoder1)
Variable Numeric Difference1
Variable Numeric EncoderSteps2
Variable Numeric OldEncoderSteps2 = Slot(Slot_Encoder2)
Variable Numeric Difference2
Variable Numeric EncoderSteps3
Variable Numeric OldEncoderSteps3 = Slot(Slot_Encoder3)
Variable Numeric Difference3
Variable Numeric EncoderSteps4
Variable Numeric OldEncoderSteps4 = Slot(Slot_Encoder4)
Variable Numeric Difference4
Variable Numeric EncoderSteps5
Variable Numeric OldEncoderSteps5 = Slot(Slot_Encoder5)
Variable Numeric Difference5

Label RecognizeEncoder
	Slot(0) = Recognize
Return

Label Encoder
	'------------------------------------------------ encoder1 to mot 1 or 6
	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
	If PressedButton <> "yellow"
		Slot(Slot_M1) = Limit(Slot (Slot_M1) + Difference1, 10, 163)
	Else
		'Slot(Slot_M6) = Limit(Slot (Slot_M6) + Difference1, -130, +130)
	EndIf
    '------------------------------------------------ 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, 10, 98)
	
    '------------------------------------------------ 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
	Difference3 = Difference3 * 0.5
	If Slot(Slot_EncoderSlow) < 500
		Difference3 = Difference3 * 0.1
	EndIf
	Slot(Slot_M3) = Limit(Slot (Slot_M3) + Difference3, 5, 94)
	
	'------------------------------------------------ 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.01
	EndIf
	Slot(Slot_M4) = Limit(Slot (Slot_M4) + Difference4, 10, 190)
	
	'------------------------------------------------ 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, 62, 298)
Return

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

' ╔════════════════════════════════════════════╗ █▀▀█ █▀▀█ █
' ║ CALIBRATIONS                               ║ █    █▄▄█ █
' ╚════════════════════════════════════════════╝ █▄▄█ █  █ █▄▄▄
Label ResetAllMultiturnValues
	SendCommand(Slot_CommandsToMotors)("ResetAllMultiturnValues")
Return

Label CopyPositionsToDestinations
	Slot Slot_M1 = Slot(Slot_M1pos)
	Slot Slot_M2 = Slot(Slot_M2pos)
	Slot Slot_M3 = Slot(Slot_M3pos)
	Slot Slot_M4 = Slot(Slot_M4pos)
	Slot Slot_M5 = Slot(Slot_M5pos)
Return

Label CalibrateMotorToZero 	'(Motor Number)
	Variable Numeric CMTZ1
	SendCommand(Slot_CommandsToMotors)("Calibrate Motor " + CMTZ1 + " To 0")
	CMTZ1 = (CMTZ1*100-100)
	Slot(Slot_M1 + CMTZ1) = Slot(Slot_M1pos + CMTZ1)
Return

'Label SetMotorCenterHere 	'(Motor Number)
'	Variable Numeric SMC1
'	SMC1 = Slot_M1center + (SMC1*100-100)
'	Slot SMC1 = 128
'	Wait Seconds 0.1
'	Slot SMC1 = 1
'	Wait Seconds 0.1
'Return

Label SetMotor_ON 	'(Motor Number)
	Variable Numeric SMON1
	SMON1 = Slot_M1center + (SMON1*100-100)
	Slot SMON1 = 0
	Wait Seconds 0.1
	Slot SMON1 = 1
	Wait Seconds 0.1
Return

Label SetMotor_OFF 	'(Motor Number)
	Variable Numeric SMOFF1
	SMOFF1 = Slot_M1center + (SMOFF1*100-100)
	Slot SMOFF1 = 0
	Wait Seconds 0.1
	Slot SMOFF1 = 1
	Wait Seconds 0.1
Return

Label WaitMotorsNotMoving
	Variable Numeric WMN1
	For WMN1 = 1 To 5
		WaitMotor(WMN1)(0.1)
	Next
Return

' -------------------------------------------------------
' The "Wait seconds" parameter is a initial delay
' If this delay is short and the motor starts slowly
' then the "WaitMotor" functions could exit immediately
' -------------------------------------------------------
Label WaitMotor 			'(Motor Number)(Wait seconds)
	Variable Numeric WM1
	Variable Numeric WM2
	WM1 = (WM1*100-100)
	Wait Seconds WM2
	Variable Numeric WM3
	For WM3 = 0 To Infinity
		If Slot(Slot_M1 + 2 + WM1) = 0 Or Key("CTRL")  ' test Velocity
			Exit For
		EndIf
		'If Slot(Slot_M1 + 6 + WM1) = 0 Or Key("CTRL") ' test Moving
		'	Exit For
		'EndIf
	Next
Return

Label MoveMotorTo 			'(Motor Number)(Destination)(Wait seconds)
	Variable Numeric MMT1
	Variable Numeric MMT2
	Variable Numeric MMT3
	Slot(Slot_M1 + MMT1*100-100) = MMT2
	WaitMotor(MMT1)(MMT3)
Return


' ╔════════════════════════════════════════════╗
' ║ ACTIVATE MOTORS                            ║
' ╚════════════════════════════════════════════╝

Label ActivateMotors
	ResetAllMultiturnValues
	Variable Numeric AM1
	For AM1 = 1 To 5
		SetMotor_ON(AM1)
	Next
	SendCommand(Slot_CommandsToCobot)("SetTorque 200")
	SendCommand(Slot_CommandsToCobot)("SetAcceleration 100")
	SendCommand(Slot_CommandsToCobot)("SetSpeed 1000")
	SendCommand(Slot_CommandsToMotors)(ShowWindow)
Return


