'╔════════════════════════════════════════════════════════════╗
'║   Name: ZeroErr_ModBus_COBOT_1.txt         Date:28 May 2022║
'║ Author: Leonardo & Theremino team                          ║
'║  Scope: Test basic functionality of eR070 MODBUS           ║
'╠════════════════════════════════════════════════════════════╣
'║Hardware: ZeroErr joint motor type eRob070                  ║
'╟────────────────────────────────────────────────────────────╢
'║Note: Use Theremino ModBus                                  ║
'╚════════════════════════════════════════════════════════════╝
Option Speed 9

RestoreVars

Load images\ZeroErr.jpg
TTS SelectVoice Language
TTS SetVolume 0  '100
TTS SetSpeed -1

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

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

Variable Numeric ActualPositionDeg
Variable Numeric DestinationDeg
Variable Numeric BaseDestinationDeg

Variable Numeric Slot_LedR		  =   7
Variable Numeric Slot_LedG		  =   8
Variable Numeric Slot_LedB		  =   9
'Variable Numeric Slot_SignalScope =  13

' ╔════════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                    ║ █▀▀█ █  █   █
' ╚════════════════════════════════════════════╝ █▄▄█ █▄▄█   █
Button 1 Text CobotGotoHome

Button 2 Text CobotGotoCenter

Button 3 Text CobotShowPoint

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

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

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

'Button 9 Text StartClamp
'Button StartClamp Text "Close Clamp"
'Button StartClamp Color Lime
'
'Button 10 Text StopClamp
'Button StopClamp Text "Open Clamp"
'Button StopClamp Color Crimson
'
'Button 11 Text WayPointTest

'Button 12 Text SayQRCode
'Button SayQRCode Color Yellow
'
Button 15 Text CloseAll
Button CloseAll Text "CLOSE ALL"
Button CloseAll Color Red

Button 17 Text "Temperature"
Button Temperature Text "Temp="
Button Temperature Color PaleTurquoise
Button 18 Text "Destination"
Button Destination Text "Dest="
Button Destination Color PaleTurquoise
Button 19 Text "Position"
Button Position Text "Pos="
Button Position Color PaleTurquoise

Button 20 Text Goto0
Button Goto0 Text "Goto 0°"
Button Goto0 Color Aqua

Button 21 Text Goto45
Button Goto45 Text "Goto 45°"
Button Goto45 Color Aqua

Button 22 Text Goto180
Button Goto180 Text "Goto 180°"
Button Goto180 Color Aqua

Button 23 Text Goto360
Button Goto360 Text "Goto 360°"
Button Goto360 Color Aqua

Button 27 Text EnableMotors
Button EnableMotors Text "Enable motors"
Button EnableMotors Color Aquamarine

Button 28 Text DisableMotors
Button DisableMotors Text "Disable motors"
Button DisableMotors Color Aquamarine

Button 29 Text HighSpeed
Button HighSpeed Text "High Speed"
Button HighSpeed Color LightGreen

Button 30 Text LowSpeed
Button LowSpeed Text "Low Speed"
Button LowSpeed Color DarkSalmon

Button 31 Text HighAccel
Button HighAccel Text "High Accel"
Button HighAccel Color LightGreen

Button 32 Text LowAccel
Button LowAccel Text "Low Accel"
Button LowAccel Color DarkSalmon

'Button 33 Text RedOn
'Button RedOn Color Red
'Button 34 Text GreenOn
'Button GreenOn Color Green
'Button 35 Text MagentaOn
'Button MagentaOn Color Magenta
'Button 36 Text YellowOn
'Button YellowOn Color Yellow
'Button 37 Text BlueOn
'Button BlueOn Color MediumBlue
'Button 38 Text WhiteOn
'Button GreenOn Color Green
'Button 39 Text RgbOff
'Button RgbOff Color Black
'
' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Load Theremino_HAL\Theremino_HAL.exe
'Load Theremino_Terminal\Theremino_Terminal_Receive.exe
'Load Theremino_Terminal\Theremino_Terminal_Transmit.exe
Load Theremino_HAL\Theremino_SlotViewer.exe
Load Theremino_ModBus\Theremino_ModBus.exe
'Load Theremino_HAL\Theremino_SignalScope.exe
'Load Theremino_Cobot.exe

' ╔════════════════════════════════════════════╗  █▀▀▀ ▀▀█▀▀ █▀▀▀█ █▀▀▀█ ▀▀█▀▀
' ║  EXECUTION START FROM HERE                 ║  ▀▀▀█   █   █▀▀▀█ █▀█▀▀   █
' ╚════════════════════════════════════════════╝  ▄▄▄█   █   █   █ █  ▀▄   █
Controls OpenTextBox
Controls ClearTextBox
Load Hide
EnableMotors
HighSpeed
HighAccel

Loop

Stop

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	'------------------------------------------ Read actual position
	ActualPositionDeg = Slot(117)
	If ActualPositionDeg < 0
		ActualPositionDeg = 65536 + ActualPositionDeg
	EndIf
	ActualPositionDeg = ActualPositionDeg + Slot(116) * 65536
	ActualPositionDeg =	ActualPositionDeg / StepPerDegree
	
	'------------------------------------------
	Analog
	DestinationDeg = BaseDestinationDeg + ClocheDeg
	
	Digital
	
    '------------------------------------------ Move motor
	MotorID = 1
	MotorParam = DestinationDeg
	MotorGotoDestination
	
	'------------------------------------------ Update labels
	Button Temperature Text "Temp=" + Slot(115) + "°"
	Button Destination Text "Dest=" + Str(DestinationDeg, "0.000") + "°"
	Button Position Text "Pos=" + Str(ActualPositionDeg, "0.000") + "°"
	
    '------------------------------------------ Print values
	'Print "Angle =" + DestinationDeg + CRLF
	'Print "Angle =" + MotorParam + CRLF
	Print "Dest=" + Str(DestinationDeg,"0.00") + " Actual pos=" + Str(ActualPositionDeg,"0.00") + CRLF
Goto Loop

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

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

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

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

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL                                  ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	CobotStopExecution
	'StopClamp
	RedOn
	SaveVars
	'CobotGotoCenter
	'CobotGotoHome
	DisableMotors
	Load CloseApps
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	SaveVars
	CobotStopExecution
	'StopClamp
	RedOn
	'CobotGotoCenter
	'CobotGotoHome
	EnableAllButtons
	Window Sizable
	'Slot(Slot_SignalScope) = 0
	SayEndOfProgram
	DisableMotors
End

' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label WayPointTest
	CobotCommand = "StopExecution"
	SendCommandToCobot
	CobotCommand = "LoadSequence COB_WayPoinTest.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "COB_WayPoinTest.seq" + CRLF
Return

Variable Numeric ClocheDeg
Label Analog
	ClocheDeg = (Slot(1) - 500) /5        ' Cloche X
	If Abs(ClocheDeg) < 20
		ClocheDeg = 0
	EndIf
	
	If Slot(10) > 500                     ' button on the cloche
		Goto0
		Print "Pushed Zero" + CRLF
	EndIf
	
	If ClocheDeg > 0
		GreenOn
	EndIf
	If ClocheDeg < 0
		BlueOn
	EndIf
	If ClocheDeg = 0
		RgbOff
	EndIf
Return

Variable Numeric EncoderSteps
Variable Numeric OldEncoderSteps
Variable Numeric Difference
Label Digital
	'------------------------------------------------ read encoder A/B
	EncoderSteps = Slot(11)
	Difference = EncoderSteps - OldEncoderSteps
	OldEncoderSteps = EncoderSteps
	If Difference > 32000
		Difference = Difference - 65536
	EndIf
	If Difference < -32000
		Difference = Difference + 65536
	EndIf
	BaseDestinationDeg = BaseDestinationDeg + Difference / 10
	'------------------------------------------------ read button
	If Slot(10) > 500
		DestinationDeg = 0
		Print "Pushed Zero" + CRLF
	EndIf
	'------------------------------------------------ LEDs
	If Difference > 0
		GreenOn
	EndIf
	If Difference < 0
		BlueOn
	EndIf
	If Difference = 0
		RgbOff
	EndIf
Return

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

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

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

Label CobotStartExecution
	CobotCommand = "StartExecution"
	SendCommandToCobot
Return

Label CobotStopExecution
	CobotCommand = "StopExecution"
	SendCommandToCobot
Return

Label CobotDisableRepeat
	CobotCommand = "DisableRepeat"
	SendCommandToCobot
Return

Label CobotGotoHome
	CobotCommand = "GotoHome"
	SendCommandToCobot
Return

Label CobotGotoCenter
	CobotCommand = "GotoCenter"
	SendCommandToCobot
Return

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

Label CobotShowPoint
	CobotCommand = "StopExecution"
	SendCommandToCobot
	CobotCommand = "LoadSequence COB_ShowPoint.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	Print "Run_ShowPoint.seq" + CRLF
Return

Label COB_Ball
	YellowOn
	TTS Speak "IMAKERBASE LEONARDO , start ball demo execution."
	'Wait TTSready
	CobotCommand = "StopExecution"
	SendCommandToCobot
	CobotCommand = "LoadSequence COB_move_ball.seq"
	SendCommandToCobot
	CobotCommand = "StartExecution"
	SendCommandToCobot
	GreenOn
	Print "COB_move_car.seq" + CRLF
	Wait TTSready
Return

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

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

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

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

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

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

Label SayRobotReadyAndOperational
	TTS Speak "Robot is ready and operational."
Return

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

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

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

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

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

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

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

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

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

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

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


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

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

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

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


' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀▀ █▀▀█    █    █▀▀█
' ║ SEQUENCE LOADING AND EXECUTION             ║ ▀▀▀█ █▀▀▀ █  █    █    █  █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄▄ █▄██▄   █▄▄▄ █▄▄█
Label RunSequence
	CobotStartExecution
	GreenOn
	StartaSequenceOfRobotAction
	SlotText CobotResponse = "Ready"
	DisableAllButtons
	Button EditSequence Enabled
	Button CloseAll Enabled
	'Button Emergency Enabled
	Variable Numeric ExitFromSequenceLoop
	'
	'Slot(Slot_SignalScope) = 1000
	Controls OpenTextBox
	StartTime = ElapsedTime
	InitSequencePaths
	'
	ExitFromSequenceLoop = 0
	SequenceToExecute = SequencePathAndName
	For NumCycle = NumCycle To Infinity
		'HardHome
		'Slot(Slot_SignalScope) = 1000
		Result = ""
		ExecSequence
		WaitCobotReady
		'Slot(Slot_SignalScope) = 0
		Wait Seconds 0.2
		If ExitFromSequenceLoop > 0
			Exit
		EndIf
	Next
	'Slot(Slot_SignalScope) = 0
	EnableAllButtons
Return

' █▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀▀█
' █  SEQUENCE LOADING AND EXECUTION             █
' █▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄▄█
Variable String SequenceToExecute
Variable String Sequence
Variable String SequenceCmd
Variable Numeric SequenceIndex
Variable Numeric SequenceLength


Label ExecSequence
	Load Sequence SequenceToExecute
	If Len(Sequence) = 0
		Controls OpenTextBox
		Print CRLF + " Sequence file not found: " + SequenceToExecute  + CRLF
		Beep
		Stop
	EndIf
	'
	Print CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)  + CRLF
	Print " started at " + Format(Now, "HH:mm:ss") + CRLF + Chars("-", 50) + CRLF
	' ----------------------------------------------------------
	Variable Numeric ExecSequenceStartTime
	ExecSequenceStartTime = ElapsedTime
	' ----------------------------------------------------------
	SequenceLength = GetSeparatedStringCount(Sequence, CRLF)
	For SequenceIndex = 0 To SequenceLength - 1
		' ------------------------------------------------------
		SequenceCmd = GetSeparatedString(Sequence, SequenceIndex, CRLF)
		SequenceCmd = Replace(SequenceCmd, Chr(9), " ") 'TABS to SPACES
		' ------------------------------------------------------ Do not print lines containing ''
		If GetSeparatedStringCount(SequenceCmd, "''") < 2
			Print " Exec: " + SequenceCmd + CRLF
		EndIf
		' ------------------------------------------------------ Trim and remove comments
		SequenceCmd = Trim(RemoveComments(SequenceCmd))
		' ------------------------------------------------------
		If SequenceCmd <> ""
			Select GetSeparatedString(SequenceCmd, 0)
            	' ---------------------------------------------- End sequence
				Case "Restart"
					SequenceIndex = SequenceLength-1
				' ---------------------------------------------- STOP
				Case "Stop"
					ExitFromSequenceLoop = 1
					Exit
				' ---------------------------------------------- ZeroErr Actions
				Case "Goto0"
					Goto0
				Case "Goto45"
					Goto45
				Case "Goto180"
					Goto180
				Case "Goto360"
					Goto360
             	' ---------------------------------------------- COBOT ACTIONS
				Case "ShowPoint"
					CobotShowPoint
				Case "COB_Ball"
					COB_Ball
				' ---------------------------------------------- LED COLORS
				Case "RedON"
					RedOn
				Case "MagentaON"
					MagentaOn
				Case "YellowON"
					YellowOn
				Case "GreenON"
					GreenOn
				Case "BlueOn"
					BlueOn
				Case "WhiteOn"
					WhiteOn
				Case "RgbOFF"
					RgbOff
				Case "START"
					RunSequence
				' ---------------------------------------------- With parameters
				Case "Pause"
					Wait Seconds GetSeparatedString(SequenceCmd, 1)
				Case "SignalScope"
					'Slot(Slot_SignalScope) = GetSeparatedString(SequenceCmd, 1)
				CaseElse
					Controls OpenTextBox
					Print CRLF + " Unrecognized command: " + SequenceCmd + CRLF
					Beep
	'				Stop
			EndSelect
		EndIf
	Next
	Print Chars("-", 50) + CRLF + "Sequence " + GetFileNameWithoutPath(SequenceToExecute)
	Print " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " sec" + CRLF
Return



' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀█ █▄ ▄█ █▄ ▄█
' ║  C O M M A N D S                           ║ █    █  █ █ ▀ █ █ ▀ █
' ╚════════════════════════════════════════════╝ █▄▄▄ █▄▄█ █   █ █   █
Label EnableMotors
	MotorID = 1
	MotorEnable
Return

Label DisableMotors
	MotorID = 1
	MotorDisable
Return

Label HighSpeed
	MotorID = 1
	MotorParam = 130000  ' max 131072
	MotorSetSpeed
Return

Label LowSpeed
	MotorID = 1
	MotorParam = 10000   ' max 131072
	MotorSetSpeed
Return

Label HighAccel
	MotorID = 1
	MotorParam = 350000  ' max 393216
	MotorSetAccel
	MotorSetDecel
Return

Label LowAccel
	MotorID = 1
	MotorParam = 10000   ' max 393216
	MotorSetAccel
	MotorSetDecel
Return

Label Goto0
	BaseDestinationDeg = 0
Return

Label Goto45
	BaseDestinationDeg = 45
Return

Label Goto180
	BaseDestinationDeg = 180
Return

Label Goto360
	BaseDestinationDeg = 360
Return

' ╔════════════════════════════════════════════╗  █▄ ▄█ █▀▀█ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ MOTOR CONTROL - DO NOT CHANGE IN THIS AREA ║  █ ▀ █ █  █   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝  █   █ █▄▄█   █   █▄▄█ █ ▀▄
Variable Numeric MotorID
Variable Numeric MotorParam
Variable Numeric StepPerDegree = 1456.3555

Label MotorEnable
	Slot (MotorID * 100 + 1) = 1
	Wait Seconds 0.3
Return

Label MotorDisable
	Slot (MotorID * 100 + 1) = 0
	Wait Seconds 0.3
Return

Label MotorSetSpeed
	MotorParam = Int(MotorParam)
	Slot (MotorID * 100 + 2) = Int(MotorParam / 65536)
	Slot (MotorID * 100 + 3) = MotorParam And 65535
Return

Label MotorSetAccel
	MotorParam = Int(MotorParam)
	Slot (MotorID * 100 + 4) = Int(MotorParam / 65536)
	Slot (MotorID * 100 + 5) = MotorParam And 65535
Return

Label MotorSetDecel
	MotorParam = Int(MotorParam)
	Slot (MotorID * 100 + 6) = Int(MotorParam / 65536)
	Slot (MotorID * 100 + 7) = MotorParam And 65535
Return

Label MotorGotoDestination
	Variable Numeric mgt
	mgt = MotorParam * StepPerDegree
	mgt = Int(mgt)
	Slot (MotorID * 100 + 10) = Int(mgt / 65536)
	Slot (MotorID * 100 + 11) = mgt And 65535
	MotorStartRun
Return

Label MotorStartRun
	Slot (MotorID * 100 + 12) = 0
	Wait Seconds 0.06
	Slot (MotorID * 100 + 12) = 1
	Wait Seconds 0.06
Return



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

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

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


'WATCH COM_Status
