
' ----------------------------------------------
'  Commands to Theremino_COBOT
' ----------------------------------------------
' 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
' ArrowUP             (auto-exit from run mode)
' ArrowDOWN           (auto-exit from run mode)
' EnableCollaborative
' DisableCollaborative
' AddNewPosition
' CorrectSelectedPosition
' AddOrCorrectPosition
' DeleteSelectedLine
' SetHomePosition
' SetCenterPosition
' SetSpeedToTorque n.nn    (0.00 to 1.00)
' SetHoldingTorque nnn
' SetTorqueMin nnn
' SetTorqueMax nnn
' SetAcceleration nnn
' SetSpeed nnn
' LoadSequence SeqName.seq (without spaces)

' ----------------------------------------------
'  RESPONSES
' ----------------------------------------------
' "Editing"
' "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"
' "Execution paused"

' ----------------------------------------------
'  SLOT TEXT for COMMANDS and RESPONSES
' ----------------------------------------------
Variable Numeric Slot_Commands = 51
Variable Numeric Slot_Responses = 52

' ----------------------------------------------
'  BUTTONS
' ----------------------------------------------
Button 1 Label ExitFromEdit

Button 3 Label StartExecution
Button 4 Label StopExecution

Button 6 Label GotoHome
Button 7 Label GotoCenter

Button 9 Label EnableRepeat
Button 10 Label DisableRepeat

Button 12 Label EnableCollaborative
Button 13 Label DisableCollaborative

Button 17 Label AddNewPosition
Button 18 Label CorrectSelectedPosition
Button 19 Label AddOrCorrectPosition

Button 21 Label DeleteSelectedLine

Button 23 Label SetHomePosition
Button 24 Label SetCenterPosition

Button 26 Label SetHoldingTorque9
Button 27 Label SetSafeTorqueLimit9
Button 28 Label SetTorque9
Button 29 Label SetAcceleration9
Button 30 Label SetSpeed9

Button 33 Label SetAllparamsToMin
Button 34 Label SetAllparamsToMax

Button 36 Label SelectLine3
Button 37 Label SelectLine6
Button 38 Label ExecuteSelectedLine

Button 40 Label ArrowUp
Button 41 Label ArrowDown

Gosub Loop
Stop

' ----------------------------------------------
'  LOOP
' ----------------------------------------------
Label Loop
	Variable String Response
	Response = SlotText(Slot_Responses)
	Print Response
	Wait Seconds 0.1
	' ------------------------------- how to test partial strings
	If StringContains(Response, "stopped")
		Beep
	EndIf
Goto Loop


' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
Label SendCommand
	Variable Numeric SC1       '<-- Parameter Destinat.-Slot
	Variable String  SC2       '<-- Parameter Command-String
	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
' ----------------------------------------------
Label ExitFromEdit
	SendCommand(Slot_Commands)("ExitFromEdit")
Return

Label StartExecution
	SendCommand(Slot_Commands)("StartExecution")
Return
Label StopExecution
	SendCommand(Slot_Commands)("StopExecution")
Return
Label EnableRepeat
	SendCommand(Slot_Commands)("EnableRepeat")
Return
Label DisableRepeat
	SendCommand(Slot_Commands)("DisableRepeat")
Return
Label GotoHome
	SendCommand(Slot_Commands)("GotoHome")
Return
Label GotoCenter
	SendCommand(Slot_Commands)("GotoCenter")
Return

Label EnableCollaborative
	SendCommand(Slot_Commands)("EnableCollaborative")
Return
Label DisableCollaborative
	SendCommand(Slot_Commands)("DisableCollaborative")
Return
Label AddNewPosition
	SendCommand(Slot_Commands)("AddNewPosition")
Return
Label CorrectSelectedPosition
	SendCommand(Slot_Commands)("CorrectSelectedPosition")
Return
Label AddOrCorrectPosition
	SendCommand(Slot_Commands)("AddOrCorrectPosition")
Return

Label DeleteSelectedLine
	SendCommand(Slot_Commands)("DeleteSelectedLine")
Return

Label SetHomePosition
	SendCommand(Slot_Commands)("SetHomePosition")
Return
Label SetCenterPosition
	SendCommand(Slot_Commands)("SetCenterPosition")
Return

Label SetHoldingTorque9
	SendCommand(Slot_Commands)("SetHoldingTorque 9")
Return
Label SetSafeTorqueLimit9
	SendCommand(Slot_Commands)("SetSafeTorqueLimit 9")
Return
Label SetTorque9
	SendCommand(Slot_Commands)("SetTorque 9")
Return
Label SetAcceleration9
	SendCommand(Slot_Commands)("SetAcceleration 9")
Return
Label SetSpeed9
	SendCommand(Slot_Commands)("SetSpeed 9")
Return

Label SetAllparamsToMin
	SendCommand(Slot_Commands)("SetHoldingTorque 0")
	SendCommand(Slot_Commands)("SetSafeTorqueLimit 0")
	SendCommand(Slot_Commands)("SetTorque 0")
	SendCommand(Slot_Commands)("SetAcceleration 0")
	SendCommand(Slot_Commands)("SetSpeed 0")
Return
Label SetAllparamsToMax
	SendCommand(Slot_Commands)("SetHoldingTorque 999")
	SendCommand(Slot_Commands)("SetSafeTorqueLimit 999")
	SendCommand(Slot_Commands)("SetTorque 999")
	SendCommand(Slot_Commands)("SetAcceleration 999")
	SendCommand(Slot_Commands)("SetSpeed 99999")
Return

Label SelectLine3
	SendCommand(Slot_Commands)("SelectLine 3")
Return
Label SelectLine6
	SendCommand(Slot_Commands)("SelectLine 9999")
Return
Label ExecuteSelectedLine
	SendCommand(Slot_Commands)("ExecuteSelectedLine")
Return

Label ArrowUp
	SendCommand(Slot_Commands)("ArrowUp")
Return
Label ArrowDown
	SendCommand(Slot_Commands)("ArrowDown")
Return


'BOOKMARKS 

'EDITEDLINES 215 10 

'WATCH v1
'EXEC DestX = 12
