'╔════════════════════════════════════════════════════════════╗
'║     Name: TWinGoBasicTouch_03.txt        Date: 20 Mar 2024 ║
'║   Author: Leonardo                                         ║
'║    Scope: Test all the functionality of the TWinGo BASIC1  ║
'╠════════════════════════════════════════════════════════════╣
'║ MOTOR 1 STS3215 FeeTech                                    ║
'║ MOTOR 2 STS3215 FeeTech                                    ║
'║ MOTOR 3 STS3215 FeeTech                                    ║
'║ MOTOR 4 STS3215 FeeTech                                    ║
'╠════════════════════════════════════════════════════════════╣
'║Hardware: One URT-1 from FeeTech                            ║
'║          Master with 4 encoders knob & 10 buttons          ║
'╠════════════════════════════════════════════════════════════╣
'║ISO   Culture  Gender   Age     Name                        ║
'╟────────────────────────────────────────────────────────────╢
'║ENG   EN-US    Male     Adult   Microsoft David Desktop     ║
'║ENG   EN-US    Female   Adult   Microsoft Zira Desktop      ║
'║ITA   IT-IT    Female   Adult   Microsoft Elsa Desktop      ║
'║JPN   JA-JP    Female   Adult   Microsoft Haruka Desktop    ║
'║ZHO   ZH-CN    Female   Adult   Microsoft Huihui Desktop    ║
'╚════════════════════════════════════════════════════════════╝
Option Speed 9
Load images\TWinGo_Touch.jpg
Beep "2500 20, 1500 100"
TTS SelectVoice "ENG"
'TTS SelectVoice "ZHO"
's1 = TTSvoices
TTS SetVolume 100      '100
TTS SetSpeed -1
'TTS SelectVoice 5   '3

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

' ╔════════════════════════════════════════════╗ ▄▀▀▀ █   █▀▀█ ▀▀█▀▀
' ║ SLOT INITIALIZATIONS                       ║ ▀▀▀▄ █   █  █   █
' ╚════════════════════════════════════════════╝ ▄▄▄▀ █▄▄ █▄▄█   █
'Variable Numeric Slot_Pedal              =  10
'Variable Numeric Slot_SetWayPoint        =  914

Variable Numeric Slot_MasterStatus        =  908

Variable Numeric Slot_SignalScope         =  983

Variable Numeric Slot_Encoder1            =  921
Variable Numeric Slot_Encoder2            =  922
Variable Numeric Slot_Encoder3            =  923
Variable Numeric Slot_Encoder4            =  924

Variable Numeric Slot_FirstFiveButton     =  926
Variable Numeric Slot_SecondFiveButton    =  927

Variable Numeric Slot_MotorsFPS           =  56

Variable Numeric Slot_M1_desBase          = 101
Variable Numeric Slot_M2_desInner         = 151
Variable Numeric Slot_M3_desMiddle        = 201
Variable Numeric Slot_M4_desOuter         = 251

Variable Numeric Slot_M1_posBase          = 102
Variable Numeric Slot_M2_posInner         = 152
Variable Numeric Slot_M3_posMiddle        = 202
Variable Numeric Slot_M4_posOuter         = 252

Variable Numeric Slot_M1_DeltaBase        = 149
Variable Numeric Slot_M2_DeltaInner       = 199
Variable Numeric Slot_M3_DeltaMiddle      = 249
Variable Numeric Slot_M4_DeltaOuter       = 299

' --------------------------------------------- MicroVideo
Variable Numeric Slot_TakeImage           = 915
Variable Numeric Slot_StartStopVideo      = 916
Variable Numeric Slot_SaveVideo           = 917

Variable Numeric Slot_M1_Min              = 85
Variable Numeric Slot_M1_Max              = 280
Variable Numeric Slot_M2_Min              = 80
Variable Numeric Slot_M2_Max              = 275
Variable Numeric Slot_M3_Min              = 40
Variable Numeric Slot_M3_Max              = 320
Variable Numeric Slot_M4_Min              = 37
Variable Numeric Slot_M4_Max              = 320

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

Button 2 Label "MotorTemp"

Button 3 Label CurrentMeter

'Button 4 Label OpenFPS
'Button OpenFPS Text "Open FPS scope"
'Button OpenFPS Color PaleTurquoise

Button 4 Label GoToHome
Button GoToHome Text "GoTo Home"
Button GoToHome Color Yellow

Button 5 Label Set170
Button Set170 Text "Set 170"
Button Set170 Color Yellow

Button 6 Label Needles
Button Needles Text "Needles"
Button Needles Color Gold

'Button 7 Label AIface
'Button AIface  Text "AIface"
'Button AIface  Color DarkOrange

Button 8 Label TWinGoTest
Button TWinGoTest Text "TWinGo Test"
Button TWinGoTest Color Gold

Button 9 Label Tuning
Button Tuning Text "Tuning"
Button Tuning Color Gold

'Button 10 Label SeaSun
'Button SeaSun Text "SeaSun"
'Button SeaSun Color HotPink

Button 10 Label Phone
Button Phone Text "Phone"
Button Phone Color SkyBlue

Button 12 Label RepeatOnOff
Button RepeatOnOff Text "Repeat OFF"
Button RepeatOnOff Color LightSteelBlue

Button 13 Label VoiceOutputOnOff
Button VoiceOutputOnOff Text "Voice ON"
Button VoiceOutputOnOff Color Green

Button 14 Label ButtonMeaning
Button ButtonMeaning Text "Meaning"
Button ButtonMeaning Color PeachPuff

Button 17 Label Delta
Button Delta Text "Delta"
Button Delta Color LightBlue

Button 18 Label VideoOnOff
Button VideoOnOff Text "RECORD OFF""
Button VideoOnOff Color LightCyan

Button 19 Label SaveVideo
Button SaveVideo Text "Save Video"
Button SaveVideo Color LightBlue

Button 20 Label TakeImage
Button TakeImage Text "Take image"
Button TakeImage Color LightCyan

Button 21 Label ReSetHomePosition
Button ReSetHomePosition Text "ReSetHomePosition"
Button ReSetHomePosition Color Black

Button 22 Label ReconnectMaster
Button ReconnectMaster Text "Connect ENC"
Button ReconnectMaster Color LightBlue

Button 23 Label OpenScopes
Button OpenScopes Text "Open Scopes"
Button OpenScopes Color PaleTurquoise

Button 24 Label OpenScope_FPS
Button OpenScope_FPS Text "Open Scope FPS"
Button OpenScope_FPS Color PaleTurquoise

Button 25 Label CloseScopes
Button CloseScopes Text "Close Scopes"
Button CloseScopes Color SkyBlue

Button 26 Label Safe
Button Safe Text "Safe"
Button Safe Color Green9

Button 28 Label ScopesActive
Button ScopesActive Text "Scopes ON"
Button ScopesActive Color SkyBlue

Button 29 Label Say
Button Say Text "Say"
Button Say Color NavajoWhite


' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Safe
ButtonMeaningStatus = "Display Off"

Load Theremino_Motors.exe
Load Theremino_SlotViewer_Motors.exe
Load Theremino_HAL.exe
Load Theremino_SlotViewer_Master_Enc.exe
Load Theremino_SlotViewer_STR.exe
'Load Theremino_Microvideo.exe
'Load Theremino_SlotViewer_TSIO.exe

WaitApplicationsStarted

WaitAllMotors

'DeActivateMotors
'CopyPositionsToDestinations

' -------------------------------------
'  MOTORS must be active before Cobot
' -------------------------------------

Load Theremino_Cobot.exe

'Load "..\Files\Analog_Button_Functions.txt"
Load Hide

' ╔════════════════════════════════════════════╗   █▀▀▀  ▀▀█▀▀  █▀▀▀█  █▀▀▀█  ▀▀█▀▀
' ║  EXECUTION START FROM HERE                 ║   ▀▀▀█    █    █▀▀▀█  █▀█▀▀    █
' ╚════════════════════════════════════════════╝   ▄▄▄█    █    █   █  █  ▀▄    █

ResetEncoders
Slot(Slot_SignalScope) = 1000
Controls OpenTextBox
Controls ClearTextBox

'VoiceCommandsCommandsStatus = "active"
'VoiceCommandsCommandsStatus = ""
'InterpretateVoiceCommandsCommands

VoiceOutputStatus = "active"
'VoiceOutputStatus = ""
VoiceOutputOnOff
TTS Speak "Theremino Automation language is running."

'TWinGoTest
'Needles
'Tuning
'SeaSun
'Coins

Loop

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	CurrentMeter
	'TorqueMeter
	'TestTemp
	'InterpretateVoiceCommandsCommands
	
	ShowMotorFPS
	PositionDelta
	Encoders
	StatusMaster
	FirstFiveButtons
	Delta
Goto Loop

Label Delta
	Slot(Slot_M1_DeltaBase)  = Slot(Slot_M1_desBase) - Slot(Slot_M1_posBase)
	Slot(Slot_M2_DeltaInner) = Slot(Slot_M2_desInner) - Slot(Slot_M2_posInner)
	Slot(Slot_M3_DeltaMiddle) = Slot(Slot_M3_desMiddle) - Slot(Slot_M3_posMiddle)
	Slot(Slot_M4_DeltaOuter)   = Slot(Slot_M4_desOuter) - Slot(Slot_M4_posOuter)
Return

Label TestTemp
	Variable Numeric TT1
	Variable Numeric TT2 = 0
	'TT2 = 0
	For TT1 = 1 To 5
		TT2 = Max(TT2, Slot(TT1 * 100 + 6))                       '<<<<<<<<<<<<<<<<
	Next
	Button MotorTemp Text "MaxT " + TT2 + "°C"
Return

' ╔═════════════════════╗
' ║ MASTER STATUS QUERY ║
' ╚═════════════════════╝
Label StatusMaster
	Variable String Master
	If Slot(Slot_MasterStatus) <> 1000
		Master = "OFF"
		'Print "MASTER OFF  "
	Else
		Master = "ON"
		'Print "MASTER ON  "
	EndIf
Return

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

Label DisableAllButtons
	For v1 = 1 To 32
		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
	Safe
	Load CloseApps "..\Files\WiFi_controller_buttons.txt"
	'GoToHome
	'StopCobot
	Slot(Slot_SignalScope) = 0
	'DeActivateMotors
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)("DisableRepeat")
	'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
	If Master = "OFF"
		Return
	EndIf
	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 EnableHomeButtons
	For v1 = 17 To 22
		Button v1 Enabled
	Next
Return

Label DisableHomeButtons
	For v1 = 17 To 22
		Button v1 Disabled
	Next
Return

Label CurrentMeter
	Variable Numeric mA
	mA = Slot(108) + Slot(208) + Slot (308) + Slot (408)      '+ Slot (508) + Slot (608) + Slot (708)
	
	'If mA > 150
	'	Beep 500 10
	'EndIf
	mA = FilterLP(mA, 0.1, 1) '<-- Filter 0.5 seconds / 1 pole
	Button CurrentMeter Text "Current " + Str(mA, "0 mA")
Return

'Label TorqueMeter
'	Variable Numeric TM1 = 0
'	TM1 = Max(TM1,Abs(Slot(104)))
'	TM1 = Max(TM1,Abs(Slot(204)))
'	TM1 = Max(TM1,Abs(Slot(304)))
'	TM1 = Max(TM1,Abs(Slot(404)))
'	TM1 = Max(TM1,Abs(Slot(504)))
'	'TM1 = FilterLP(TM1, 0.1, 1) '<-- Filter 0.5 seconds / 1 pole
'	If TM1 = 200
'		Beep 1000 100
'	EndIf
'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

Label PositionDelta
	Slot(Slot_M1_DeltaBase) = Slot(Slot_M1_desBase) - Slot(Slot_M1_posBase)
	Slot(Slot_M2_DeltaInner) = Slot(Slot_M2_desInner) - Slot(Slot_M2_posInner)
	Slot(Slot_M3_DeltaMiddle) = Slot(Slot_M3_desMiddle) - Slot(Slot_M3_posMiddle)
	Slot(Slot_M4_DeltaOuter) = Slot(Slot_M4_desOuter) - Slot(Slot_M4_posOuter)
Return

Label ScopesActive
	If Slot(Slot_SignalScope) < 1000
		Button ScopesActive Text "Scopes ON"
		Button ScopesActive Color Flashing8 OrangeRed Moccasin
		Slot(Slot_SignalScope) = 1000
	Else
		Button ScopesActive Text "Scopes OFF"
		Button ScopesActive Color SkyBlue
		Slot(Slot_SignalScope) = 0
	EndIf
Return


' ╔═══════════════════════╗
' ║  V O I C E            ║
' ╚═══════════════════════╝
Variable String VoiceOutputStatus
Label StartVoice
	TTS SetVolume 100
	VoiceOutputStatus = ""
	VoiceOutputOnOff
Return

Label StopVoice
	TTS SetVolume 0
	VoiceOutputStatus = "active"
	VoiceOutputOnOff
Return

Label VoiceOutputOnOff
	If VoiceOutputStatus = "active"
		VoiceOutputStatus = ""
		Button VoiceOutputOnOff Text "Voice OFF"
		Button VoiceOutputOnOff Color DarkSlateGray
		TTS SetVolume 0
	Else
		VoiceOutputStatus = "active"
		Button VoiceOutputOnOff Text "Voice ON"
		Button VoiceOutputOnOff Color Lime
		TTS SetVolume 100
	EndIf
Return



' -------------------------------------------------
Label Safe
	Slot(Slot_M1_desBase)   = 180
	Slot(Slot_M2_desInner)  = 270
	Slot(Slot_M3_desMiddle) =  60
	Slot(Slot_M4_desOuter)  =  90
	
'	RecognizeEncoder
	TTS Speak "Start Safe job"
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_Safe.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return
Return

Label Set170
	Slot(Slot_M1_desBase)   = 170
	Slot(Slot_M2_desInner)  = 170
	Slot(Slot_M3_desMiddle) = 170
	Slot(Slot_M4_desOuter)  = 170
	
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_Set170.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return



' ╔════════════════════════════════════════╗ █▄ ▄█ █▀▀█ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ Move Motor (MotNum)(Dest)(Wait seconds)║ █ ▀ █ █  █   █   █  █ █▄▄▀
' ╚════════════════════════════════════════╝ █   █ █▄▄█   █   █▄▄█ █ ▀▄
Label GoToHome
	'RecognizeEncoder
	TTS Speak "Goto Home."
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_Home.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return



Label SeaSun
'	RecognizeEncoder
	TTS Speak "Start demo for SeaSun"
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_SeaSun.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label Phone
'	RecognizeEncoder
	TTS Speak "Start demo for Phone"
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_Phone.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label Needles
'	RecognizeEncoder
	TTS Speak "Start execution of Needles"
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_Needles.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label TWinGoTest
'	RecognizeEncoder
	TTS Speak "Start execution of a test"
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_Test.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label Tuning
'	RecognizeEncoder
	TTS Speak "Executing Tuning"
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_tuning.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	'SendCommand(Slot_CommandsToCobot)("StartExecution")
Return


Label SafePosition
'	RecognizeEncoder
	TTS Speak "Executing Tuning"
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_SafePosition.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

' ============ NICE BOX ==============
' ╔═════════════════════════╗ █▀▀▀   █▀▀    █▀█  █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ FIRST FIVE PUSH BUTTONS ║ █▀▀    ▀▀█    █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚═════════════════════════╝ █  ▄   ▄▄█    █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
'
' ************ WALMART BOX YELLOW BUTTONS ***********
' 999, 368, 184, 106, 49, 15  these are ADC values
' (999+368)/2 = 683
' (368+184)/2 = 276
' (184+106)/2 = 145
' (106+ 49)/2 = 77
' ( 49+ 15)/2 = 27
'****************************************************

' ============ NICE BOX YELLOW BUTTONS ==============
' 999, 341, 161, 85, 29, 0  ADC values from HAL
' (999+341)/2 = 670
' (341+161)/2 = 251
' (161+85)/2 = 123
' (85+29)/2 = 57
' (29+0)/2 = 14
'====================================================

Variable String PressedButton
Label FirstFiveButtons
	Variable Numeric FFB0
	Variable Numeric FFB1
	Variable Numeric FFB2
	'' ------------------------------ check the presence of the Master
	If Slot(Slot_MasterStatus) <> 1000
		Return
	EndIf
	' ------------------------------ Read the value two times
	FFB1 = Slot(Slot_FirstFiveButton)
	' ------------------------------ If similar values
	If Abs(FFB2 - FFB1) < 10
		' -------------------------- If armed
		If FFB0 = 0
			FFB0 = 1
			Select True
				Case FFB1 > 670
					'Print ""
					PressedButton = ""
				Case FFB1 > 251
					'Print " Begin "
					PressedButton = "Begin"
					ExecCobot
				Case FFB1 > 123
					'Print " Exec from line "
					PressedButton = "Exec from line"
					ExecuteCobotFromSelectedLine
				Case FFB1 > 57
					'Print " Arrow up "
					PressedButton = "Up"
					ArrowUpCobot
				Case FFB1 > 14
					'Print " Arrow down "
					PressedButton = "Down"
					ArrowDownCobot
				CaseElse
					'Print " Arrow inside "
					PressedButton = "Inside"
					VoiceOutputOnOff
			EndSelect
		EndIf
	EndIf
	FFB2 = FFB1
    ' ------------------------------ Rearm buttons
	If FFB1 > 700
		FFB0 = 0
	EndIf
	SecondFiveButtons
Return

' ============ NICE BOX COLORED BUTTONS ==============
' ╔══════════════════════════╗ ▄▀▀▀    █▀▀    █▀█  █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ SECOND FIVE PUSH BUTTONS ║ ▀▀▀▄    ▀▀█    █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚══════════════════════════╝ ▄▄▄▀ ▄  ▄▄█    █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
' ============ NICE BOX COLORED BUTTONS ==============
' 996, 341, 161, 85, 29, 0  ADC values from HAL
' (996+341)/2 = 628
' (341+161)/2 = 251
' (161+85)/2 = 123
' (85+29)/2 = 57
' (29+0)/2 = 14
'=====================================================



Label SecondFiveButtons
	Variable Numeric SFB0
	Variable Numeric SFB1
	Variable Numeric SFB2
	'' ------------------------------ check the presence of the Master
	If Slot(Slot_MasterStatus) <> 1000
		Return
	EndIf
	' ------------------------------ Read the value two times
	SFB1 = Slot(Slot_SecondFiveButton)
	
	' ------------------------------ If similar values
	If Abs(SFB2 - SFB1) < 10
		' -------------------------- If armed
		If SFB0 = 0
			SFB0 = 1
			Select True
				Case SFB1 > 628
					'Print ""
					PressedButton = ""
					EncoderSpeed = ""
				Case SFB1 > 251
					'Print " red "
					PressedButton = "red"
					EncoderSpeed = "high"
				Case SFB1 > 123
					'Print " green "
					PressedButton = "green"
					EncoderSpeed = "low"
				Case SFB1 > 57
					'Print " black "
					PressedButton = "black"
					DeleteSelectedLine
				Case SFB1 > 14
					'Print " yellow "
					PressedButton = "yellow"
					AddNewPosition
				CaseElse
					'Print " blue "
					PressedButton = "blue"
					RepeatOnOff
			EndSelect
		EndIf
	EndIf
	SFB2 = SFB1
    ' ------------------------------ Rearm buttons
	If SFB1 > 700
		SFB0 = 0
	EndIf
Return

Label  ReconnectMaster
	Slot(0) = Recognize
Return

' ╔════════════════════════╗ █▀▀▀ █▄ █ █▀▀▀ █▀▀█ █▀▀▄ █▀▀▀ █▀▀█ ▄▀▀▀
' ║ ENCODERS USB or  Wi-Fi ║ █▀▀  █ ▀█ █    █  █ █  █ █▀▀  █▄▄█ ▀▀▀█
' ╚════════════════════════╝ █▄▄▄ █  █ █▄▄▄ █▄▄█ █▄▄█ █▄▄▄ █ ▀▄ ▄▄▄▀
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
Variable Numeric EncoderSteps4
Variable Numeric OldEncoderSteps4 = Slot(Slot_Encoder4)
Variable Numeric Difference4
'
''Label RecognizeEncoder
''	Slot(0) = Recognize
''Return

Variable String "EncoderSpeed" = ""
Label Encoders
	'------------------------------------------------ Encoder BASE
	EncoderSteps1 = Slot(Slot_Encoder1) / 4
	Difference1 = EncoderSteps1 - OldEncoderSteps1
	OldEncoderSteps1 = EncoderSteps1
	If Difference1 > 32000 / 4
		Difference1 = Difference1 - 65536 / 4
	EndIf
	If Difference1 < -32000 / 4
		Difference1 = Difference1 + 65536 / 4
	EndIf
	Difference1 = Difference1 * 1       'default speed
	If EncoderSpeed  = "low"
		Difference1 = Difference1 / 4
	EndIf
	If EncoderSpeed = "high"
		Difference1 = Difference1 * 2
	EndIf
	WriteSlotMinMax(Slot_M1_desBase)(Difference1)(Slot_M1_Min)(Slot_M1_Max)
	
    '------------------------------------------------ Encoder Inner
	EncoderSteps2 = Slot(Slot_Encoder2) / 4
	Difference2 = EncoderSteps2 - OldEncoderSteps2
	OldEncoderSteps2 = EncoderSteps2
	If Difference2 > 32000 / 4
		Difference2 = Difference2 - 65536 / 4
	EndIf
	If Difference2 < -32000 / 4
		Difference2 = Difference2 + 65536 / 4
	EndIf
	Difference2 = Difference2 * 1
	If EncoderSpeed  = "low"
		Difference2 = Difference2 / 4
	EndIf
	If EncoderSpeed  = "high"
		Difference2 = Difference2 * 2
	EndIf
	WriteSlotMinMax(Slot_M2_desInner)(Difference2)(Slot_M2_Min)(Slot_M2_Max)
	
    '------------------------------------------------ Encoder Outer
	EncoderSteps3 = Slot(Slot_Encoder3) / 4
	Difference3 = EncoderSteps3 - OldEncoderSteps3
	OldEncoderSteps3 = EncoderSteps3
	If Difference3 > 32000 / 4
		Difference3 = Difference3 - 65536 / 4
	EndIf
	If Difference3 < -32000 / 4
		Difference3 = Difference3 + 65536 / 4
	EndIf
	Difference3 = Difference3 * 1
	If EncoderSpeed  = "low"
		Difference3 = Difference3 / 4
	EndIf
	If EncoderSpeed  = "high"
		Difference3 = Difference3 * 2
	EndIf
	WriteSlotMinMax(Slot_M3_desMiddle)(Difference3)(Slot_M3_Min)(Slot_M3_Max)
	
	'------------------------------------------------ Encoder end
	EncoderSteps4 = Slot(Slot_Encoder4) / 4
	Difference4 = EncoderSteps4 - OldEncoderSteps4
	OldEncoderSteps4 = EncoderSteps4
	If Difference4 > 32000 / 4
		Difference4 = Difference4 - 65536 / 4
	EndIf
	If Difference4 < -32000 / 4
		Difference4 = Difference4 + 65536 / 4
	EndIf
	Difference4 = Difference4 * 1
	If EncoderSpeed  = "low"
		Difference4 = Difference4 / 4
	EndIf
	If EncoderSpeed  = "high"
		Difference4 = Difference4 * 2
	EndIf
	WriteSlotMinMax(Slot_M4_desOuter)(Difference4)(Slot_M4_Min)(Slot_M4_Max)
	
		'------------------------------------------------
	If Difference1 <> 0 Or Difference2 <> 0 Or Difference3 <> 0 Or Difference4 <> 0
		UpdateCobot
		'SendCommand(Slot_CommandsToCobot)("SetTorqueMin 333")
		'SendCommand(Slot_CommandsToCobot)("SetTorqueMax 555")
	EndIf
Return

Label ResetEncoders
	OldEncoderSteps1 = Slot(Slot_Encoder1) / 4
	OldEncoderSteps2 = Slot(Slot_Encoder2) / 4
	OldEncoderSteps3 = Slot(Slot_Encoder3) / 4
	OldEncoderSteps4 = Slot(Slot_Encoder4) / 4
Return

Label WriteSlotMinMax
	Variable Numeric WSSlot
	Variable Numeric WSdiff
	Variable Numeric WSMin
	Variable Numeric WSMax
	WSdiff = Slot(WSSlot) + WSdiff
	If WSdiff > WSMax
		WSdiff = WSMax
		Beep(30,10)
	EndIf
	If WSdiff < WSMin
		WSdiff = WSMin
		Beep(30,10)
	EndIf
	Slot(WSSlot) = WSdiff
Return


' ╔═══════════════════════╗ █▀▀█ █▀▀▀ █▀▀█ █▀▀▀ █▀▀█ ▀▀█▀▀
' ║  REPEAT ON-OFF        ║ █▄▄█ █▀▀  █▄▄█ █▀▀  █▄▄█   █
' ╚═══════════════════════╝ █ ▀▄ █▄▄▄ █    █▄▄▄ █  █   █
Variable String RepeatStatus = "Repeat Off"
Label RepeatOnOff
	If RepeatStatus = "Repeat Off"
		RepeatStatus = "Repeat On"
		EnableRepeat
		TTS Speak RepeatStatus
		Button RepeatOnOff Text "REPEAT ON"
		Button RepeatOnOff Color Flashing7 Yellow Red
	Else
		DisableRepeat
		RepeatStatus = "Repeat Off"
		TTS Speak RepeatStatus
		Button RepeatOnOff Text "REPEAT OFF"
		Button RepeatOnOff Color DarkSlateGray
	EndIf
Return

Variable String ButtonMeaningStatus = "Display Off"
Label ButtonMeaning
	If ButtonMeaningStatus = "Display OFF"
		ButtonMeaningStatus = "Display ON"
		Load images\Button_meanings.png
		'TTS Speak RepeatStatus
		Button ButtonMeaning Text "Meaning ON"
	Else
		Load Hide
		ButtonMeaningStatus = "Display OFF"
		'TTS Speak RepeatStatus
		Button ButtonMeaning Text "Meaning OFF"
	EndIf
Return



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

Label OpenScopes
	Load Theremino_SignalScope_M1.exe
	Load Theremino_SignalScope_M2.exe
	Load Theremino_SignalScope_M3.exe
	Load Theremino_SignalScope_M4.exe
Return

Label OpenScope_FPS
	Load Theremino_SignalScope_FPS.exe
Return

Label StartScope
	Slot(Slot_SignalScope) = 1000
Return

Label StopScope
	Slot(Slot_SignalScope) = 0
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


'╔═══════════════════╗ █▀▀█ █   █▀▀▀ █▀▀█ █▀▀▀ █▀▀▀
'║  AIface           ║ █▄▄█ █   █▀▀  █▄▄█ █    █▀▀
'╚═══════════════════╝ █  █ █   █    █  █ █▄▄▄ █▄▄▄
Label AIface
'	RecognizeEncoder
	TTS Speak "AIface demonstration start. AIface演示开始。"
	Wait Seconds 0.1
	SendCommand(Slot_CommandsToCobot)("LoadSequence TWinGo_AIface.txt")
	SendCommand(Slot_CommandsToCobot)("EnableRepeat")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return


Label CopyPositionsToDestinations
	Slot Slot_M1_desBase = Slot(Slot_M1_posBase)
	Slot Slot_M2_desInner = Slot(Slot_M2_posInner)
	Slot Slot_M3_desMiddle = Slot(Slot_M3_posMiddle)
	Slot Slot_M4_desOuter = Slot(Slot_M4_posOuter)
Return



Label SetMotor_ON 	'(Motor Number)
	Variable Numeric SMON1
	'SMON1 = Slot_M1_desBase_center + (SMON1*100-100)    ' livio
	Slot SMON1 = 0
	Wait Seconds 0.1
	Slot SMON1 = 1
	Wait Seconds 0.1
Return

Label SetMotor_OFF 	'(Motor Number)
	Variable Numeric SMOFF1
	'SMOFF1 = Slot_M1_desBase_center + (SMOFF1*100-100)    ' livio
	Slot SMOFF1 = 1
	Wait Seconds 0.1
	Slot SMOFF1 = 0
	Wait Seconds 0.1
Return

Label WaitAllMotors
	Variable Numeric WMN1
	For WMN1 = 1 To 4
		WaitMotor(WMN1)
	Next
Return

' -------------------------------------------------------
' The "Wait seconds" parameter is a initial delay
' If this delay is short and the motor starts slowly
' then the "WaitMotor" functions could exit immediately
' -------------------------------------------------------
Label WaitMotor 			'(Motor Number)
	Variable Numeric WM1
	WM1 = (WM1*100-100)
	Wait Seconds 0.1
	Variable Numeric WM3
	For WM3 = 0 To Infinity
		If Slot(Slot_M1_desBase + 2 + WM1) = 0 Or Key("CTRL")  ' test Velocity
			Exit 1
		EndIf
		'If Slot(Slot_M1_desBase + 6 + WM1) = 0 Or Key("CTRL") ' test Moving
		'	Exit For
		'EndIf
	Next
Return

Label MoveMotorTo 			'(Motor Number)(Destination)
	Variable Numeric MMT1
	Variable Numeric MMT2
	Slot(Slot_M1_desBase + MMT1*100-100) = MMT2
	WaitMotor(MMT1)
Return

' ╔════════════════════════════════════════════╗
' ║ ACTIVATE MOTORS                            ║
' ╚════════════════════════════════════════════╝
Label ActivateMotors
	Variable Numeric AM1
	For AM1 = 1 To 4
		SetMotor_ON(AM1)
	Next
	SendCommand(Slot_CommandsToCobot)("SetTorqueMin 200")
	SendCommand(Slot_CommandsToCobot)("SetAcceleration 100")
	SendCommand(Slot_CommandsToCobot)("SetSpeed 1000")
	'SendCommand(Slot_CommandsToMotors)(ShowWindow)
Return

'Label DeActivateMotors
'	Variable Numeric DM1
'	For DM1 = 1 To 4
'		SetMotor_OFF(DM1)
'	Next
'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
					' ---------------------------------------------- START/STOP Voice
		Case "StartVoice"
			StartVoice
			ClearCommand
		Case "StopVoice"
			StopVoice
			ClearCommand
			
        '------------------------------------ MicroVideo
		Case "StartVideo"
			StartVideo
			ClearCommand
		Case "StopVideo"
			StopVideo
			ClearCommand
		Case "SaveVideo"
			SaveVideo
			ClearCommand
		Case "TakeImage"
			TakeImage
			ClearCommand
			
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + CommandText + CRLF
			'Beep
			ClearCommand
	EndSelect
Return

' ╔═════════════════════╗ █   █ █ █▀▀▄ █▀▀▀ █▀▀█
' ║ VIDEO COMMANDS      ║  █ █  █ █  █ █▀▀  █  █
' ╚═════════════════════╝   █   █ █▄▄█ █▄▄▄ █▄▄█
Variable Numeric RecordEnabled
Label VideoOnOff
	If RecordEnabled = 0
		StartScope
		StartVideo
		RecordEnabled = 1
		Button VideoOnOff Text "RECORDING"
		Button VideoOnOff Color Flashing5 Red Black
	Else
		RecordEnabled = 0
		Button VideoOnOff Text "RECORD OFF"
		Button VideoOnOff Color LightCyan
		StopVideo
		StopScope
	EndIf
Return

Label StartVideo
	'ResetTime
	Slot(Slot_StartStopVideo) = 1000
	Wait Seconds 0.2
Return

Label StopVideo
	Print "Seconds " + Format(ElapsedTime - StartTime, "0.000000")
	Slot(Slot_StartStopVideo) = 0
	Wait Seconds 0.2
Return

Label SaveVideo
	Button SaveVideo Text "Saving"
	Button SaveVideo Color Flashing8 Red Yellow
	Slot(Slot_SaveVideo) = 1000
	Wait Seconds 0.5
	Slot(Slot_SaveVideo) = 0
	Wait Seconds 0.2
	Button SaveVideo Text "Save Video"
	Button SaveVideo Color LightBlue
Return

Label TakeImage
	'Wait Seconds 1.5
	Slot(Slot_TakeImage) = 1000
	Wait Seconds 0.2
	Slot(Slot_TakeImage) = 0
	Wait Seconds 0.2
Return




' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	's1 =       "X=" + Format(Slot(Slot_M1_desBase_X), "0.000")
	's1 = s1 + " Y=" + Format(Slot(Slot_M2_desInner_Y), "0.000")
	's1 = s1 + " Z=" + Format(Slot(Slot_M3_desMiddle_Z), "0.000")
	'Print CRLF + s1
Return

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


' ====================================================================
'  re-set home position. Before use, all motor must be to 180
' ====================================================================
Label ReSetHomePosition
	'ZeroFeetech(1)
	ZeroFeetech(2)
Return

Label ZeroFeetech
	' ----------------------------- initialize motor address
	Variable Numeric ZF1
	ZF1 = (ZF1 + 1) * 50
	' -----------------------------
	Slot(ZF1 + 23) = 128  'align current position to 2048
	Wait Seconds 0.05
	Slot(ZF1 + 23) = 1    'restore normal function
	Wait Seconds 0.05
	' ----------------------------- copy position to destination
	Slot(ZF1 + 1) = Slot(ZF1 + 2)
	Wait Seconds 0.05
	' ----------------------------- move to desidered position
	'Slot(ZF1 + 1) = Slot(ZF1 + 2) + 10
	'For v1 = 0 To 1000
	'	Wait Seconds 0.05
	'	If Slot(ZF1 + 7) = 0
	'		Exit
	'	EndIf
	'Next
	' ----------------------------- zero to motor
	'Slot(ZF1 + 23) = 128  'align current position to 2048
	'Wait Seconds 0.05
	'Slot(ZF1 + 23) = 1    'restore normal function
	'Wait Seconds 0.05
	'' ----------------------------- copy position to destination
	'Slot(ZF1 + 1) = Slot(ZF1 + 2)
	'Wait Seconds 0.05
Return


'WATCH CR
