'╔════════════════════════════════════════════════════════════╗
'║     Name: T-Pupper_15.txt                Date: 14 Sep 2023 ║
'║   Author: Leonardo                                         ║
'║    Scope: Use TSIO to manage the servo motor of Pupper     ║
'╠════════════════════════════════════════════════════════════╣
'║ MOTOR Servo1  FT90M                                        ║
'║ MOTOR Servo2  FT90M                                        ║
'║ MOTOR Servo3  FT90M                                        ║
'║ MOTOR Servo4  FT90M                                        ║
'║ MOTOR Servo5  FT90M                                        ║
'║ MOTOR Servo6  FT90M                                        ║
'║ MOTOR Servo7  FT90M                                        ║
'║ MOTOR Servo8  FT90M                                        ║
'║ MOTOR Servo9  FT90M                                        ║
'║ MOTOR Servo10 FT90M                                        ║
'║ MOTOR Servo11 FT90M                                        ║
'║ MOTOR Servo12 FT90M                                        ║
'╠════════════════════════════════════════════════════════════╣
'║ HW: TSIO-8  servo 1 to 9                                   ║
'║     TSIO-9  servo 10 to 12                                 ║
'║     Master-V1 3 encoder  and buttons                       ║
'╠════════════════════════════════════════════════════════════╣
'║ *  Note:                                                   ║
'╟────────────────────────────────────────────────────────────╢
'║ISO     Culture   Gender            Name                    ║
'╟────────────────────────────────────────────────────────────╢
'║"ENG M"  EN-US    Male      Microsoft David Desktop         ║
'║"ENG F"  EN-US    Female    Microsoft Zira Desktop          ║
'║"ITA"    IT-IT    Female    Microsoft Elsa Desktop          ║
'║"JPN"    JA-JP    Female    Microsoft Haruka Desktop        ║
'║"ZHO"    ZH-CN    Female    Microsoft Huihui Desktop        ║
'╚════════════════════════════════════════════════════════════╝
Option Speed 9
'Load images\T-Pupper.png
Beep "2500 20, 1500 100"
's2 = TTSvoices
TTS SelectVoice "ENG F"
'TTS SelectVoice "ENG M"
TTS SetVolume 100
TTS SetSpeed -2
'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_SignalScope   = 13

Variable Numeric Slot_ButtonFL      = 30
Variable Numeric Slot_ButtonFR      = 31
Variable Numeric Slot_ButtonRL      = 32
Variable Numeric Slot_ButtonRR      = 33

Variable Numeric Slot_Encoder1      = 25  'hip or shoulder
Variable Numeric Slot_Encoder2      = 27  'upper leg
Variable Numeric Slot_Encoder3      = 29  'lower leg

Variable Numeric Slot_MultiAnaBtnA   = 21
Variable Numeric Slot_MultiAnaBtnB   = 22


'Variable Numeric Slot_EncoderSlow   = 22
Variable Numeric Slot_BtnYellow1    = 36
Variable Numeric Slot_BtnBlue1      = 37
Variable Numeric Slot_BtnGreen1     = 38
Variable Numeric Slot_BtnWhite1     = 39
Variable Numeric Slot_BtnRed1       = 40
Variable Numeric Slot_BtnBlack1     = 36
Variable Numeric Slot_BtnYellow2    = 37
Variable Numeric Slot_BtnGreen2     = 38
Variable Numeric Slot_BtnBlue2      = 39
Variable Numeric Slot_BtnWhite2     = 40
'
'Variable Numeric Slot_AudioIn1      = 995
'Variable Numeric Slot_AudioIn2      = 996
'Variable Numeric Slot_AudioIn3      = 997

'Variable Numeric Slot_Joint_1       = 101
'Variable Numeric Slot_Joint_2       = 201
'Variable Numeric Slot_Joint_3       = 301

'Variable Numeric Slot_Joint_1pos    = 102
'Variable Numeric Slot_Joint_2pos    = 202
'Variable Numeric Slot_Joint_3pos    = 302
'
'Variable Numeric Slot_Joint_3acc    = 321
'Variable Numeric Slot_Joint_3speed  = 322

Variable Numeric Slot_MotorsFPS     =  56
'Variable String AnimateStatus       = "AnimateOFF"
Variable String FiveBtnStatus       = "FiveButtonOFF"
'Variable String SensorsStatus       = "SensorsOFF"

'Variable String WiFi_USB_Status = "USB_Active"

'╟─── TSIO 8 ────────────────────────────╢
'Variable Numeric Slot_SHARP       = 810   'pin ADC 1
'Variable Numeric Slot_Potentiom   = 812   'pin ADC 2
'Variable Numeric Slot_PosFeedback = 814   'pin ADC 3
'Variable Numeric Slot_IRMovSensor = 816   'pin ADC 4
'Variable Numeric Slot_HallAnalog  = 818   'pin ADC 5
'Variable Numeric Slot_Joy_Y       = 820   'pin ADC 6
'Variable Numeric Slot_Joy_X       = 822   'pin ADC 7
'Variable Numeric Slot_MultiAnaBtn = 824   'pin ADC 8
'Variable Numeric Slot_JoyTopBtn   = 826
'Variable Numeric Slot_LimitSensor = 828

'Variable Numeric Slot_Red         = 966
'Variable Numeric Slot_Gre         = 968
'Variable Numeric Slot_Blu         = 972
'Variable Numeric Slot_White       = 974

'--------------------- FRONT RIGHT = YELLOW -------
Variable Numeric Slot_Servo_FR1    = 858    'shoulder
Variable Numeric Slot_Servo_FR2    = 860    'upper
Variable Numeric Slot_Servo_FR3    = 862    'lower

'---------------------- FRONT LEFT = GREEN  -------
Variable Numeric Slot_Servo_FL1    = 864    'shoulder
Variable Numeric Slot_Servo_FL2    = 866    'upper
Variable Numeric Slot_Servo_FL3    = 868    'lower

'---------------------- REAR RIGHT = WHITE  -------
Variable Numeric Slot_Servo_RR1    = 870    'hip
Variable Numeric Slot_Servo_RR2    = 872    'upper
Variable Numeric Slot_Servo_RR3    = 874    'lower

'----------------------- REAR LEFT = BLUE   -------
Variable Numeric Slot_Servo_RL1    = 958    'hip
Variable Numeric Slot_Servo_RL2    = 960    'upper
Variable Numeric Slot_Servo_RL3    = 962    'lower

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

Button 2 Label OpenFPS
Button OpenFPS Text "Open FPS scope"
Button OpenFPS Color PaleTurquoise

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

Button 4 Label Voice
Button Voice Text "Voice"
Button Voice Color Blue

Button 5 Label SensorsSet
Button SensorsSet Text "SensorsOFF"
Button SensorsSet Color DarkSalmon

Button 6 Label Red_8
Button Red_8 Text "RED"
Button Red_8 Color Red

Button 7 Label Gre_8
Button Gre_8 Text "GREEN"
Button Gre_8 Color LimeGreen

Button 8 Label Blu_8
Button Blu_8 Text "BLUE"
Button Blu_8 Color Blue

Button 9 Label Whi_8
Button Whi_8 Text "WHITE"
Button Whi_8 Color FloralWhite

Button 10 Label Off_8
Button Off_8 Text "BLACK"
Button Off_8 Color Black

Button 17 Label Demoloop
Button Demoloop Text "Demoloop"
Button Demoloop Color Gold

Button 18 Label Ground
Button Ground Text "Ground"
Button Ground Color Yellow

Button 19 Label LieDown
Button LieDown Text "LieDown"
Button LieDown Color Yellow

Button 20 Label RaiseUp
Button RaiseUp Text "RaiseUp"
Button RaiseUp Color Yellow

Button 21 Label GoMax
Button GoMax Text "GoMax"
Button GoMax Color Yellow

Button 22 Label GoMin
Button GoMin Text "GoMin"
Button GoMin Color Yellow

Button 23 Label GoSeat
Button GoSeat Text "GoSeat"
Button GoSeat Color Yellow

Button 24 Label GoTiltRight
Button GoTiltRight Text "GoTiltRight"
Button GoTiltRight Color Yellow

Button 25 Label GoTiltLeft
Button GoTiltLeft Text "GoTiltLeft"
Button GoTiltLeft Color Yellow

GoTiltLeft

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
DisableAllButtons
Ground

Load Theremino_SlotViewer_TSIO8_9.exe
Load Theremino_SlotViewer_Master_Enc.exe
Load Theremino_SlotViewer_STR.exe
'Load Theremino_QRdecoder.exe
'Load Theremino_VoiceCommands.exe
'Load Theremino_AudioInput.exe
'Load "..\Files\WiFi_controller_buttons.txt"
Load Theremino_HAL.exe
Load Theremino_Cobot.exe
Load Hide

' ╔════════════════════════════════════════════╗  █▀▀▀ ▀▀█▀▀ █▀▀▀█ █▀▀▀█ ▀▀█▀▀
' ║  EXECUTION START FROM HERE                 ║  ▀▀▀█   █   █▀▀▀█ █▀█▀▀   █
' ╚════════════════════════════════════════════╝  ▄▄▄█   █   █   █ █  ▀▄   █
'RecognizeEncoder
ResetEncoders
'SetIOTSlotsToZero
'Slot(Slot_SignalScope) = 1000
Controls OpenTextBox
Controls ClearTextBox
GoGroundHighSpeed
EnableAllButtons

Load Theremino_Motors.exe
Wait Application "Theremino_Motors"
SendCommand(Slot_CommandsToMotors)("EnableConnection")
WaitApplicationsStarted

TTS Speak "Ready"
Loop

Stop                                '

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	'AudioInput
	ShowMotorFPS
	Encoders
	FiveButtons_A
	FiveButtons_B
	'If SensorsStatus  = "SensorsON"
	'	Sensors
	'EndIf
	
Goto Loop

' ╔════════════════════════════════════════════╗
' ║ 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"
			Return
		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
	'SendCommand(Slot_CommandsToMotors)("DisableConnection")
	'Load CloseApps "..\Files\WiFi_controller_buttons.txt"
	Ground
	WaitCobotReady
	StopCobot
	Slot(Slot_SignalScope) = 0
End

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

Label AddOrCorrectPosition
	SendCommand(Slot_CommandsToCobot)("AddOrCorrectPosition")
Return

Label DeleteSelectedLine
	SendCommand(Slot_CommandsToCobot)("DeleteSelectedLine")
Return

Label AddNewPosition
	SendCommand(Slot_CommandsToCobot)("AddNewPosition")
Return

Label EnableRepeat
	SendCommand(Slot_CommandsToCobot)("EnableRepeat")
Return

Label DisableRepeat
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
Return


' ╔══════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A           ║ ▀▀▀█ █  █ █▀▀█
' ╚══════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█

'Label SensorsSet
'	If SensorsStatus  = "SensorsOFF"
'		Button SensorsSet Text "Sensors ON"
'		Button SensorsSet Color Flashing_9 Lime Snow
'		SensorsStatus  = "SensorsON"
'	Else
'		Button SensorsSet Text "Sensors OFF"
'		Button SensorsSet Color DarkSalmon
'		SensorsStatus  = "SensorsOFF"
'	EndIf
'Return

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

' ╔═══════════════════════╗ █▀▀█   █▀█  █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ FIVE PUSH BUTTONS -A- ║ █▀▀█   █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚═══════════════════════╝ █  █   █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
Variable String PressedButtonA

Label FiveButtons_A
	Variable Numeric FBA0
	Variable Numeric FBA1
	Variable Numeric FBA2
	' ------------------------------ Read the value two times
	FBA1 = Slot(Slot_MultiAnaBtnA)
	
	' ------------------------------ If similar values
	If Abs(FBA2 - FBA1) < 10
		' -------------------------- If armed
		If FBA0 = 0
			FBA0 = 1
			Select True
				Case FBA1 > 500
					Print ""
					PressedButtonA = ""
				Case FBA1 > 250
					Print "red "
					PressedButtonA = "red"
					ExecCobot
				Case FBA1 > 120
					Print "white "
					PressedButtonA = "white"
					ExecuteCobotFromSelectedLine
				Case FBA1 > 56
					Print "arrowDOWN "
					PressedButtonA = "ArrowDOWN"
					ArrowDownCobot
				Case FBA1 > 15
					Print "arrowUP "
					PressedButtonA = "ArrowUP"
					ArrowUpCobot
				CaseElse
					Print "yellow "
					PressedButtonA = "yellow"
					AddOrCorrectPosition
			EndSelect
		EndIf
	EndIf
	FBA2 = FBA1
    ' ------------------------------ Rearm buttons
	If FBA1 > 500
		FBA0 = 0
	EndIf
Return

' ╔═══════════════════════╗ █▀█    █▀█  █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ FIVE PUSH BUTTONS -B- ║ █▀▀█   █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚═══════════════════════╝ █▄▄█   █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
Variable String PressedButtonB

Label FiveButtons_B
	Variable Numeric FBB0
	Variable Numeric FBB1
	Variable Numeric FBB2
	' ------------------------------ Read the value two times
	FBB1 = Slot(Slot_MultiAnaBtnB)
	
	' ------------------------------ If similar values
	If Abs(FBB2 - FBB1) < 10
		' -------------------------- If armed
		If FBB0 = 0
			FBB0 = 1
			Select True
				Case FBB1 > 500
					'Print ""
					PressedButtonB = ""
				Case FBB1 > 250
					Print "yellowB "
					PressedButtonB = "yellowB"
					AddNewPosition
				Case FBB1 > 120
					Print "blackB "
					PressedButtonB = "blackB"
					DeleteSelectedLine
				Case FBB1 > 56
					Print "small "
					PressedButtonB = "small"
					'=== spare
				Case FBB1 > 15
					Print "bluBB "
					PressedButtonB = "bluB"
					DisableRepeat
				CaseElse
					Print "greenB "
					PressedButtonB = "greenB"
					'=== spare
			EndSelect
		EndIf
	EndIf
	FBB2 = FBB1
    ' ------------------------------ Rearm buttons
	If FBB1 > 500
		FBB0 = 0
	EndIf
Return






Label SetIOTSlotsToZero
	'Slot(Slot_EncoderSlow) = 0
	Slot(Slot_BtnYellow1)  = 0
	Slot(Slot_BtnBlue1)    = 0
	Slot(Slot_BtnGreen1)   = 0
	Slot(Slot_BtnWhite1)   = 0
	Slot(Slot_BtnRed1)     = 0
Return

' ╔══════════════════╗ █▀▀▀ █▀▀ █▀▀█ █▀▀█ █▀▀ █▀▀▀
' ║  S C O P E S     ║ ▀▀▀█ █   █  █ █▄▄█ █▀▀ ▀▀▀█
' ╚══════════════════╝ ▄▄▄█ █▄▄ █▄▄█ █    █▄▄ ▄▄▄█
Label CloseScopes
	Load CloseApps Theremino_SignalScope_M1.exe
Return

Label OpenScope_1
	Load Theremino_SignalScope_M1.exe
Return

' ╔═══════════════╗ █   █▀▀ █▀▄
' ║ LED COLORS    ║ █   █▀  █  █
' ╚═══════════════╝ █▄▄ █▄▄ █▄▄█
'Label Red_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 0
'Return
'
'Label Gre_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 0
'Return
'
'Label Blu_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 1000
'Return
'
'Label Yel_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 0
'Return
'
'Label Mag_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 1000
'Return
'
'Label Cya_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 1000
'Return
'
'Label Whi_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 1000
'Return
'
'Label Off_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 0
'Return
'
'Label RedBlink_8
For v1 = 1 To 10
'		Red_8
	Beep
'	Off_8
	Beep
Next
Return


Variable Numeric WF1
Label WiFiLive
	If WF1 > 30
		WF1 = 0
'		Slot(Slot_Gre) =  1000
	Else
		WF1 = WF1 + 1
'		Slot(Slot_Gre) =  0
	EndIf
Return

' ╔══════════════╗ █▀▀▀ █▀▀ █▀▀█ █   █ █▀▀█
' ║  S E R V O   ║ ▀▀▀█ █▀▀ █▄▄█ █   █ █  █
' ╚══════════════╝ ▄▄▄█ █▄▄ █ ▀▄  ▀▄▀  █▄▄█
Label ClearCommand
	SlotText(Slot_ExternalCommands) = ""
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

' ╔══════════╗ ▄▀▀▀ █▀▀█ ▀▄ ▄▀
' ║  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   ║  █ █  █  █ █ █    █▀▀
' ╚═════════╝   █   █▄▄█ █ █▄▄▄ █▄▄▄
Label Voice
	'Load images\LIST_VOICE_COMMAND.jpg
	ClearVoiceCommand
	If VoiceCommandsStatus = "active"
		VoiceCommandsStatus = ""
		Button Voice Color Blue
	Else
		VoiceCommandsStatus = "active"
		Button Voice Color Flashing7 Orange White
	EndIf
Return

Label AIGPT
	Load Theremino_GPT.exe
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 "greencar.pick"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_pick.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "windshield.side"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_front.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "see.backside"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_back.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
			
		Case "driver.prospect"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_left.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "right.outlook"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_right.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "bottom.view"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_underside.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "release"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_place.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
			'-------------------------------------------------------------------------
		Case "goto.square"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_mechArm_square.txt  ")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "goto.home"
			If ShowingType = ""
'				Mag_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_mechArm_home.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
			'-------------------------------------------------------------------------
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + Word + CRLF
			Beep
			ClearCommand
	EndSelect
Return

Label ClearVoiceCommand
	SlotText(Slot_WordRecognized) = ""
Return

'╔═══════════════════╗ █▀▀▀ █▀▀█ █▄ ▄█ █▄ ▄█   █▀▀ █▀▀█   █▀▀▀ █▀▀█ █▀█  █▀▀█ ▀▀█▀▀
'║COMMANDS FROM COBOT║ █    █  █ █ ▀ █ █ ▀ █   █▀  █▄▄█   █    █  █ █▀▀█ █  █   █
'╚═══════════════════╝ █▄▄▄ █▄▄█ █   █ █   █   █   █ ▀▄   █▄▄▄ █▄▄█ █▄▄█ █▄▄█   █
Label Event_ExternalCommands
	' ------------------------------------------------------ Do not print lines containing ''
	'Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select GetSeparatedString(CommandText, 0)
		' ---------------------------------------------- ACTIONS
		Case "Say"
			TTS Speak Mid(CommandText, 5)
			ClearCommand
		Case "SayW"
			TTS Speak Mid(CommandText, 5)
			Wait TTSready
			ClearCommand
			
		Case "Beep"
			Beep 880-100
			ClearCommand
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + CommandText + CRLF
			Beep
			ClearCommand
	EndSelect
Return


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

Label Demoloop
	For Loop_Count = 1 To 10
		RaiseUp
		WaitCobotReady
		LieDown
		WaitCobotReady
	'	RaiseUp
	'	WaitCobotReady
	'	GoMax
	'	WaitCobotReady
	'	RaiseUp
	'	WaitCobotReady
	'	GoTiltLeft
	'	WaitCobotReady
	'	GoMin
	'	WaitCobotReady
	'	GoSeat
	'	WaitCobotReady
	'	LieDown
	'	WaitCobotReady
	'	GoTiltRight
	'	WaitCobotReady
	'	GoMax
	'	WaitCobotReady
	'	RaiseUp
	'	WaitCobotReady
	'	LieDown
	'	WaitCobotReady
	'	GoMax
	'	WaitCobotReady
	'	GoTiltRight
	'	WaitCobotReady
	'	GoMax
	Next
Return

Label Ground
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Ground.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoGroundHighSpeed
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_GroundHighSpeed.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label LieDown
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Liedown.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label RaiseUp
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Raiseup.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoMax
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Max.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoMin
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Min.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoSeat
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Seat.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoTiltRight
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_TiltRight.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoTiltLeft
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_TiltLeft.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return





' ╔════════════════════════╗ █▀▀▀ █▄ █ █▀▀▀ █▀▀█ █▀▀▄ █▀▀▀ █▀▀█ ▄▀▀▀
' ║ ENCODERS USB           ║ █▀▀  █ ▀█ █    █  █ █  █ █▀▀  █▄▄█ ▀▀▀█
' ╚════════════════════════╝ █▄▄▄ █  █ █▄▄▄ █▄▄█ █▄▄█ █▄▄▄ █ ▀▄ ▄▄▄▀
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

Label ReadButtons
	Slot(Slot_ButtonFL) = 31   'gre
	Slot(Slot_ButtonFR) = 30   'yel
	Slot(Slot_ButtonRL) = 33   'why
	Slot(Slot_ButtonRR) = 32   'blu
Return

Label RecognizeEncoder
	Slot(0) = Recognize
Return

Label Encoders
	'------------------------------------------------ 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
	Difference1 = Difference1 * 0.5
	'If Slot(Slot_EncoderSlow) < 500
	'	Difference1 = Difference1 * 0.1
	'EndIf
	If Slot(Slot_ButtonFL) < 500
		Slot(Slot_Servo_FL1) = Limit(Slot(Slot_Servo_FL1) + Difference1, 110, 899)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR1) = Limit(Slot(Slot_Servo_FR1) + Difference1, 110, 899)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL1) = Limit(Slot(Slot_Servo_RL1) + Difference1, 110, 899)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR1) = Limit(Slot(Slot_Servo_RR1) + Difference1, 110, 899)
	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
	Difference2 = Difference2 * 0.5
	'If Slot(Slot_EncoderSlow) < 500
	'	Difference2 = Difference2 * 0.1
	'EndIf
	If Slot(Slot_ButtonFL) < 500
		Slot(Slot_Servo_FL2) = Limit(Slot(Slot_Servo_FL2) + Difference2, 110, 999)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR2) = Limit(Slot(Slot_Servo_FR2) + Difference2, 110, 999)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL2) = Limit(Slot(Slot_Servo_RL2) + Difference2, 110, 999)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR2) = Limit(Slot(Slot_Servo_RR2) + Difference2, 110, 999)
	EndIf
    '------------------------------------------------ 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
	If Slot(Slot_ButtonFL) < 500
		Slot(Slot_Servo_FL3) = Limit(Slot(Slot_Servo_FL3) + Difference3, 110, 999)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR3) = Limit(Slot(Slot_Servo_FR3) + Difference3, 110, 999)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL3) = Limit(Slot(Slot_Servo_RL3) + Difference3, 110, 999)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR3) = Limit(Slot(Slot_Servo_RR3) + Difference3, 110, 999)
	EndIf
Return

Label ResetEncoders
	OldEncoderSteps1 = Slot(Slot_Encoder1)
	OldEncoderSteps2 = Slot(Slot_Encoder2)
	OldEncoderSteps3 = Slot(Slot_Encoder3)
Return


'WATCH Variable
