'╔═══════════════════════════════════════════════════════════╗
'║     Name: SentelArmStudy_00.txt         Date: 03 Giu 2022 ║
'║   Author: Leonardo                                        ║
'║    Scope: Test SENTEL six axis arm                        ║
'╠═══════════════════════════════════════════════════════════╣
'║Hardware: RoboTell USB-CANbus Adapter                      ║
'╟───────────────────────────────────────────────────────────╢
'║    Note: Start tests                           14/05/2022 ║
'║    Note: Re-start tests                        03/06/2022 ║
'╚═══════════════════════════════════════════════════════════╝
Load SENTEL_arm.png
Option Speed 9
ComPort = 17

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

Variable Numeric Slot_Clamp             =  1
Variable Numeric Slot_LedR			    =  7
Variable Numeric Slot_LedG		        =  8
Variable Numeric Slot_LedB		        =  9
'Variable Numeric Slot_Pump        	     =  6
'Variable Numeric Slot_SignalScope       = 13


' ╔════════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀ ▀▀█▀▀
' ║ BUTTONS                                    ║ █▀▀█ █  █   █     █
' ╚════════════════════════════════════════════╝ █▄▄█ █▄▄█   █     █
Button  1 Text Enable
Button Enable Text "ENABLE JOINT"
Button Enable Color Cyan5

Button  2 Text Disable
Button Disable Text "DISABLE JONT"
Button Disable Color Cyan5

'=================================== Experimental functions
Button  3 Text ReadStatus           ' 00 Read the servo status (0: Off / 1:On)
Button ReadStatus Text ReadStatus
Button ReadStatus Color LightBlue

Button  4 Text ReadCumPos
Button ReadCumPos Text ReadCumPos   'Read the absolute single-loop position of the motor encoder
Button ReadCumPos Color LightBlue

Button  5 Text RunStatus
Button RunStatus Text RunStatus   'Read running status (82,83 Function code function is valid)
Button RunStatus Color LightBlue

Button  6 Text SetOrigin
Button SetOrigin Text SetOrigin
Button SetOrigin Color Red1

Button  7 Text SaveParam
Button SaveParam Text SaveParam
Button SaveParam Color Red1

Button  8 Text AllZero
Button AllZero Text AllZero
Button AllZero Color LightBlue

Button  9 Text 10_20_30_40_50_60
Button 10_20_30_40_50_60 Text 10_20_30_40_50_60
Button 10_20_30_40_50_60 Color LightBlue

Button 10 Text 3=90_5=90
Button 3=90_5=90 Text "3=90_5=90"
Button 3=90_5=90 Color LightBlue

'=================================== Actions ===
Button 12 Text ApproachBox
Button ApproachBox Text "Approach Box"
Button ApproachBox Color Aqua

Button 13 Text ApproachCar
Button ApproachCar Text "Approach Car"
Button ApproachCar Color Aqua

Button 14 Text Motion
Button Motion Text "Motion test"
Button Motion Color Aquamarine

Button 15 Text CLOSE_ALL
Button CLOSE_ALL Text "CLOSE ALL"
Button CLOSE_ALL Color Red

'Button 17 Text RedOn
'Button RedOn Color Red
'Button 18 Text GreenOn
'Button GreenOn Color Green
'Button 19 Text YellowOn
'Button YellowOn Color Yellow
'Button 20 Text BlueOn
'Button BlueOn Color MediumBlue
'Button 21 Text WhiteOn
'Button GreenOn Color Green
'Button 22 Text RgbOff
'Button RgbOff Color Black
' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Load Theremino_HAL\Theremino_HAL.exe
Load ..\Theremino_Terminal_RX\Theremino_Terminal_Receive.exe
Load ..\Theremino_Terminal_TX\Theremino_Terminal_Transmit.exe
Load ..\Terminal\Terminal.exe
''''''Load ..\Theremino_CAN\Theremino_CAN.exe  'do not use, we send data directly
'Load ..\Clamp\Clamp.exe
Load ..\Theremino_HAL\Theremino_SlotViewer.exe
'Load ..\Theremino_HAL\Theremino_SignalScope.exe

' ╔════════════════════════════════════════════╗  █▀▀▀ ▀▀█▀▀ █▀▀▀█ █▀▀▀█ ▀▀█▀▀
' ║  START FROM HERE                           ║  ▀▀▀█   █   █▀▀▀█ █▀█▀▀   █
' ╚════════════════════════════════════════════╝  ▄▄▄█   █   █   █ █  ▀▄   █
Load Vars
TestNumber = v1
TestErrors = v2

Controls OpenTextBox
Controls ClearTextBox
'Beep "1000, 100"
'TTS SelectVoice "ENG"
'TTS SetVolume 100
'TTS SetSpeed -3

COM Open ComPort 115200

s1 = COM_Status
s1 = "Port status: "  + "COM" + ComPort + " IS " + s1 + CRLF
s1 = s1 + "Valid ports: " + COM_PortNames + CRLF + CRLF
Print s1

'If COM_Status = "OPENED"
'	TTS Speak "Robot ready and operational."
'Else
'	TTS Speak "Robot is NOT connected."
'EndIf
'Wait TTSready
Load Hide

Stop                                            '

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL                                  ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CLOSE_ALL
	Load CloseApps "Theremino_Editor.exe"
	Load CloseApps
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	v1 = TestNumber
	v2 = TestErrors
	Save Vars
	'Controls CloseTextBox
End                                             '

' ╔════════════════════════════════════════════╗ █   █ █▀▀█ █▀▀▀█
' ║ VARIABLE INITIALIZATIONS & ASSIGNEMENTS    ║ █   █ █▄▄█ █▄▄▄█
' ╚════════════════════════════════════════════╝  ▀▄▀  █  █ █  ▀▄
Variable Numeric ComPort
'Variable Numeric StopAll
Variable Numeric TestNumber
Variable Numeric TestErrors

' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label Enable
	s1 = "00 80 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 03"
    'TTS Speak "Robot enabled."
	SendCommand
Return

Label Disable
	s1 = "00 80 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03"
	'TTS Speak "Robot disabled."
	SendCommand
Return


'Read the servo status (0: Off / 1:On)
Label ReadStatus
	s1 = "00"
	SendCommand
Return

'Read the cumulative position of themotor (power off clearance)
Label ReadCumPos
	s1 = "00 03"
	SendCommand
Return

'Read running status (82,83 Function code function is valid)
Label RunStatus
	s1 = "00 29"
	SendCommand
Return

'Set, origin
Label SetOrigin
	s1 =  "00 89 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00"
	SendCommand
Return


'Save parameter (0x 91~0x94 function code parameter repair
'It must be saved after the modification, otherwise the power-off is invalid)
'Note: 0x91~0x93 The current parameters can be modified appropriately, dependingon the actual
'situation of the load.
Label SaveParam
	s1 = "00 95 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00 03"
	SendCommand
Return

Label AllZero
	s1 =  "00 82 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 E8"
	SendCommand
Return

Label 10_20_30_40_50_60
	s1 = "00 82 00 01 86 A0 00 03 0D 40 00 04 93 E0 00 06 1A 80 00 07 A1 20 00 09 27 C0 03 E8"
	SendCommand
Return

Label 3=90_5=90
	s1 =  "00 82 00 00 00 00 00 00 00 00 00 0D BB A0 00 00 00 00 00 0D BB A0 00 00 00 00 03 E8"
	SendCommand
Return


Label ApproachBox
	s1 = "00 82 FF F2 1D B4 00 06 42 58 00 08 C0 A0 FF FF FE 70 00 0C 9C E8 00 0E A6 00 03 E7 "
	SendCommand
Return

Label ApproachCar
'   s1 = "00 82 00 0E 0A 24 00 04 13 5C 00 10 71 60 FF FF 63 C0 00 06 1C D8 00 00 81 4C 03 E7"
	s1 = "00 82 00 0E 0A 24 00 04 13 5C 00 10 71 60 FF FF 63 C0 00 06 1C D8 00 00 C3 50 03 E7"
	
	SendCommand
Return

Label JointAll_0
	's1 = "00 82 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 E7"
	
	s1 = "00 82 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 03 E8"
	SendCommand
Return

Label JointAll_11
	s1 =  "00 82 00 01 AD B0 00 01 AD B0 00 01 AD B0 00 01 AD B0 00 01 AD B0 00 01 AD B0 03 E7"
	SendCommand
Return

Label Enquire
	s1 = "00 03 00 29 00 00 00 05"
	SendCommand
Return


Label Motion
	For v4 = 0 To 40
		JointAll_0
		Wait Seconds 2.5
		ApproachCar
		Wait Seconds 3
		Slot(Slot_Clamp) = 500         'Close
		Wait Seconds 1
		JointAll_0
		Wait Seconds 2
		ApproachBox
		Wait Seconds 2
		Slot(Slot_Clamp) = 350         'Open
		Wait Seconds 0.5
	Next
Return


' ╔════════════════════════════════════════════╗ █   █▀▀ █▀▄
' ║ LED COLORS                                 ║ █   █▀  █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█
Label RedOn
'	Language = 1
'	TTS Speak "Status red."
	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
'	Language = 1
'	TTS Speak "Status yellow."
	Slot Slot_LedR = 900
	Slot Slot_LedG = 400
	Slot Slot_LedB = 0
Return

Label GreenOn
'	Language = 1
'	TTS Speak "Status green."
	Slot Slot_LedR = 0
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 0
Return

Label BlueOn
	'Language = 1
	'TTS Speak "Status blue."
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label WhiteOn
	'Language = 1
	'TTS Speak "Status white."
	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

' ╔════════════════════════════════════════════╗ █▀▀▀ █▀▀▀ █▀▄ █ █▀▀▄
' ║ S E N D                                    ║ ▀▀▀█ █▀▀▀ █  ▀█ █  █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄▄ █   █ █▄▄█
Label SendCommand
	'Slot(14) = 1000		'show start signal on SignalScope
	COM DiscardOutBuffer
	Wait Seconds 0.1
	COM WriteStringHEX s1
	Print s1 + CRLF
	'Slot(14) = 0		'show stop signal on SignalScope
	
Return


'WATCH COM_Status
