'╔════════════════════════════════════════════════════════════╗
'║     Name: T-Pupper_07.txt                Date: 08 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-Putter.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_SetWayPoint  =  14

Variable Numeric Slot_SignalScope   = 13
Variable Numeric Slot_MultiAnaBtn   = 21

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  'Omerus
Variable Numeric Slot_Encoder2      = 27  'Femur
Variable Numeric Slot_Encoder3      = 29  'Tibia

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_M4          = 401
''Variable Numeric Slot_M5          = 501
''Variable Numeric Slot_M6          = 601
''Variable Numeric Slot_M7          = 701

'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_Joint_1center = 123
'
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

Variable Numeric Slot_Servo_FL1    = 864
Variable Numeric Slot_Servo_FL2    = 866
Variable Numeric Slot_Servo_FL3    = 868

Variable Numeric Slot_Servo_FR1    = 858
Variable Numeric Slot_Servo_FR2    = 860
Variable Numeric Slot_Servo_FR3    = 862

Variable Numeric Slot_Servo_RL1    = 958
Variable Numeric Slot_Servo_RL2    = 960
Variable Numeric Slot_Servo_RL3    = 962

Variable Numeric Slot_Servo_RR1    = 870
Variable Numeric Slot_Servo_RR2    = 872
Variable Numeric Slot_Servo_RR3    = 874

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

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

Load Theremino_Motors.exe

Load Theremino_SlotViewer_Motors.exe
Load Theremino_SlotViewer_TSIO8.exe
Load Theremino_SlotViewer_TSIO9.exe
Load Theremino_SlotViewer_Master_Enc.exe
Load Theremino_HAL.exe
Wait Application "Theremino_Motors"
'Wait Seconds 3
SendCommand(Slot_CommandsToMotors)("EnableConnection")
WaitApplicationsStarted

' -------------------------------------
'  MOTORS must be active before Cobot
' -------------------------------------
'Load Theremino_HAL.exe
Load Theremino_SlotViewer_Master_Enc.exe
Load Theremino_SlotViewer_STR.exe
Load Theremino_Cobot.exe
'Load Theremino_QRdecoder.exe
'Load Theremino_VoiceCommands.exe
Load Theremino_AudioInput.exe
'Load "..\Files\WiFi_controller_buttons.txt"

Load Hide


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

Stop                                '

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	'AudioInput
	'CurrentMeter
	'TorqueMeter
	ShowMotorFPS
	Encoders
	'TestTemp
	
	If FiveBtnStatus  = "FiveButtonON"
		FiveButtons
	EndIf
	'
	'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"
			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
	'SendCommand(Slot_CommandsToMotors)("DisableConnection")
	'Load CloseApps "..\Files\WiFi_controller_buttons.txt"
	StopCobot
	Slot(Slot_SignalScope) = 0
End

'' ╔═══════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
'' ║ CLOSE ALL  (when pressing X on form)  ║ █   █   █  █ ▀▀▀█ █▀▀
'' ╚═══════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
'Label CloseAll
'	Beep
'	Wait Seconds 1
'	Beep
'	Wait Seconds 1
'	Beep
'	Wait Seconds 1
'
'	StopPump
'	StopCobot
'	Slot(Slot_SignalScope) = 0
'	'GoToHome
'	DeActivateMotors
'	Load CloseApps "..\Files\WiFi_controller_buttons.txt"
'	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.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 UpdateCobot
	SendCommand(Slot_CommandsToCobot)("CorrectSelectedPosition")
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  ║ ▀▀█   █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚════════════════════╝ ▄▄█   █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
Variable String PressedButton
Label FiveButtons
	Variable Numeric FB0
	Variable Numeric FB1
	Variable Numeric FB2
	' ------------------------------ Read the value two times
	FB1 = Slot(Slot_MultiAnaBtn)
	
	' ------------------------------ If similar values
	If Abs(FB2 - FB1) < 10
		'--------------------------- If armed
		If FB0 = 0
			FB0 = 1
			Select True
				Case FB1 > 500
					Print ""
					PressedButton = ""
					'Slot(Slot_White) = 0
					'Off_8
				Case FB1 > 250
					Print "red"
					'PressedButton = "red"
					'Red_8
					
				Case FB1 > 120
					Print "white"
					'PressedButton = "white"
					'Whi_8
					'Slot(Slot_White) = 1000
				Case FB1 > 56
					Print "green"
					'PressedButton = "green"
					'Gre_8
				Case FB1 > 15
					Print "blue"
					'PressedButton = "blue"
					'Blu_8
				CaseElse
					Print "yellow"
					'PressedButton = "yellow"
					'Yel_8
					
			EndSelect
		EndIf
	EndIf
	FB2 = FB1
    ' ------------------------------ Rearm buttons
	If FB1 > 500
		FB0 = 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                      ║   █   █▄▄▀ █ █▄▄█ █
' ╚══════════════════════════════════╝   █   █ ▀▄ █ █  █ █▄▄
'

' ╔════════════════════════╗ █▀▀▀ █▄ █ █▀▀▀ █▀▀█ █▀▀▄ █▀▀▀ █▀▀█ ▄▀▀▀
' ║ 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, 330, 700)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR1) = Limit(Slot(Slot_Servo_FR1) + Difference1, 330, 700)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL1) = Limit(Slot(Slot_Servo_RL1) + Difference1, 330, 700)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR1) = Limit(Slot(Slot_Servo_RR1) + Difference1, 330, 700)
	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, 320, 780)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR2) = Limit(Slot(Slot_Servo_FR2) + Difference2, 330, 700)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL2) = Limit(Slot(Slot_Servo_RL2) + Difference2, 330, 700)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR2) = Limit(Slot(Slot_Servo_RR2) + Difference2, 330, 700)
	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, 320, 780)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR3) = Limit(Slot(Slot_Servo_FR3) + Difference3, 330, 700)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL3) = Limit(Slot(Slot_Servo_RL3) + Difference3, 330, 700)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR3) = Limit(Slot(Slot_Servo_RR3) + Difference3, 330, 700)
	EndIf
Return

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


'WATCH Variable
