'╔═════════════════════════════════════════════════════════╗
'║   Name: 57AIM15H_V07.txt              Date: 24 Jul 2022 ║
'║ Author: Leonardo                                        ║
'║  Scope: Test overall functionality                      ║
'╠═════════════════════════════════════════════════════════╣
'║Hardware: BLDC motor with encoder                        ║
'╟─────────────────────────────────────────────────────────╢
'║ Note: COBOT update                          14 Jul 2022 ║
'║                                                         ║
'╚═════════════════════════════════════════════════════════╝
Option Speed 9
'Beep "2500 100, 1500 100"

'Variable Numeric Debug = 0 ' 1 = DEBUG   0 = NORMAL

RestoreVars

Load images\57AIM15H.jpg
'Beep "2500 200, 1500 200, 2500 300, 0500, 1500 300"
'TTS SelectVoice Language
'TTS SetVolume  100    '0 = mute
'Wait Seconds 2
TTS SetSpeed -1

Window MinSize

' ╔══════════════════════════════════════════╗
' ║ SLOT TEXT for COMMANDS and RESPONSES     ║
' ╚══════════════════════════════════════════╝
Variable Numeric Slot_CommandsToCobot    = 51
Variable Numeric Slot_ResponsesFromCobot = 52
Variable Numeric Slot_CommandsFromCobot  = 53


' ╔══════════════════════════════════════════╗ █   █ █▀▀█ █▀▀▀█
' ║ VARIABLE INITIALIZATIONS & ASSIGNEMENTS  ║ █   █ █▄▄█ █▄▄▄█
' ╚══════════════════════════════════════════╝  ▀▄▀  █  █ █  ▀▄
Variable Numeric Slot_M_1       = 101

Variable Numeric Slot_EnMotors  =  10

Variable Numeric slot_Limits    =  12

Variable Numeric Slot_AnalogBut =   1
Variable Numeric Slot_LedR      =   7
Variable Numeric Slot_LedG      =   8
Variable Numeric Slot_LedB      =   9

Variable Numeric Slot_SignScope  =  13

' -------------------------------------- Software limits
Variable Numeric M1max =   360
Variable Numeric M1min =     0

Key 1 BtnCoord_M1

' ╔══════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                  ║ █▀▀█ █  █   █
' ╚══════════════════════════════════════════╝ █▄▄█ █▄▄█   █
Button 1 Label "BtnCoord_M1"
Button BtnCoord_M1 Text "M1 = ?"
Button BtnCoord_M1 Color LightBlue

Button 2 Label Degree_0
Button Degree_0 Text "Home = 0°"
Button Degree_0 Color Aquamarine

Button 3 Label Degree_15
Button Degree_15 Text "Degree_15"
Button Degree_15 Color Aquamarine

Button 4 Label Degree_30
Button Degree_30 Text "Degree_30"
Button Degree_30 Color Aquamarine

Button 5 Label Degree_45
Button Degree_45 Text "Degree_45"
Button Degree_45 Color Aquamarine

Button 6 Label Degree_90
Button Degree_90 Text "Degree_90"
Button Degree_90 Color Aquamarine

Button 7 Label Degree_135
Button Degree_135 Text "Degree_135"
Button Degree_135 Color Aquamarine

Button 8 Label Degree_180
Button Degree_180 Text "Degree_180"
Button Degree_180 Color Aquamarine

Button 9 Label Degree_270
Button Degree_270 Text "Degree_270"
Button Degree_270 Color Aquamarine

Button 10 Label Degree_315
Button Degree_315 Text "Degree_315"
Button Degree_315 Color Aquamarine

Button 11 Label Degree_360
Button Degree_360 Text "Degree_360"
Button Degree_360 Color Aquamarine

Button 12 Label Temperature
Button Temperature Text "Temp= "
Button Temperature Color MistyRose

Button 13 Label Voltage
Button Voltage Text "Volt="
Button Voltage Color LightGreen

Button 16 Label CloseAll
Button CloseAll Text "CLOSE ALL"
Button CloseAll Color Red

'Button 17 Text YellowAction
'Button YellowAction Color Yellow
'
'Button 18 Text BlueAction
'Button BlueAction Color Blue
'
'Button 19 Text RedAction
'Button RedAction Color Red
'
'Button 20 Text GreenAction
'Button GreenAction Color Green
'
'Button 21 Text WhiteAction
'Button WhiteAction Color White
'
'Button 22 Text BlackAction
'Button BlackAction Color Black
'
'Button 23 Text SafePos
'Button SafePos Text "Safe Position"
'Button SafePos Color Bisque
'
'Button 24 Text ColorButton
'Button ColorButton  Text "COLOR BUTTON"
'Button ColorButton  Color Plum
'
Button 17 Label ExecCobot
Button ExecCobot Text "Exec Cobot"
Button ExecCobot Color SteelBlue
Button 18 Label SelectSequence
Button SelectSequence Text "Select COBOT sequence"
Button SelectSequence Color SteelBlue

Button 20 Label MotorTest
Button MotorTest Text "MotorTest"
Button MotorTest Color Orange

'Button 17 Text RepTest
'Button RepTest Text " "REPET.TEST""
'Button RepTest Color Khaki

'Button 16 Text Emergency
'Button Emergency Text "EMERGENCY"
'Button Emergency Color Red
'Button Emergency Slot  Slot_Emergency

Button 22 Label RedOn
Button RedOn Color Red
Button 23 Label GreenOn
Button GreenOn Color Green
Button 24 Label YellowOn
Button YellowOn Color Yellow
Button 25 Label BlueOn
Button BlueOn Color MediumBlue
Button 26 Label WhiteOn
Button GreenOn Color Green
Button 27 Label RgbOff
Button RgbOff Color Black

Button 28 Label EnableVoice
Button EnableVoice Text "EnableVoice"
Button EnableVoice Color SkyBlue
Button 29 Label DisableVoice
Button DisableVoice Text "Disable Motors"
Button DisableVoice Color SkyBlue

' -------------------------------------- KEYS
Key Right		 ExecKeys
Key Left 		 ExecKeys
Key Up 			 ExecKeys
Key Down 		 ExecKeys
Key Pageup		 ExecKeys
Key Pagedown	 ExecKeys

' ╔══════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                ║ █   █  █ █▄▄█ █  █
' ╚══════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Load Theremino_ModBus.exe
Load Theremino_HAL.exe
Load Theremino_SlotViewer.exe
'Load Theremino_SlotViewer_Strings.exe
Load Theremino_Cobot.exe
Load Theremino_SignalScope.exe
'Load Terminal_Transmit.exe
'Load Terminal_Receive.exe

' ╔══════════════════════════════════════════╗  █▀▀▀ ▀▀█▀▀ █▀▀▀█ █▀▀▀█ ▀▀█▀▀
' ║  START FROM HERE                         ║  ▀▀▀█   █   █▀▀▀█ █▀█▀▀   █
' ╚══════════════════════════════════════════╝  ▄▄▄█   █   █   █ █  ▀▄   █
'EnableMotors
Controls OpenTextBox
DisableAllButtons
Controls ClearTextBox
GreenOn
Print TTSvoices
EnableAllButtons
UpdateButtons
ShowSequenceName
Load Hide
'EnableVoice
DisableVoice
Loop
Stop

' ╔══════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL                                ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚══════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	RedOn
	SaveVars
	StopCobot
	Window Sizable
	Slot(Slot_SignScope) = 0
	TTS Speak "End Of Program."
	Load CloseApps
Return

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	SaveVars
	RedOn
	StopCobot
	EnableAllButtons
	Window Sizable
	Slot(Slot_SignScope) = 0
	TTS Speak "End Of Program."
End

' ╔══════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                     ║ █   █  █ █  █ █▄▄█
' ╚══════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	UpdateButtons
	' ------------------------------------------
	' 0.02 sec. or more to allow time for keys and other tasks
	Wait Seconds 0.02
	' ---------------------------------------------------------
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
	s11 = SequenceName
	Save Vars
	Print CRLF + "SAVED SEQUENCENAME = " + SequenceName + CRLF
Return

Label RestoreVars
	Load Vars
	SequenceName = s11
Return

' ╔════════════════════════════════════════════╗ █▀▄▀█ █▀▀█ █   █ █▀▀▀
' ║ MOVE STEPPERS                              ║ █   █ █  █ █   █ █▀▀
' ╚════════════════════════════════════════════╝ █   █ █▄▄█  ▀▄▀  █▄▄▄
Label Degree_0
	TTS Speak "Motor go at 0 degree."
	Slot(Slot_M_1) = 0
	UpdateButtons
	Wait TTSready
	RedOn
	WaitAllMotors
Return

Label Degree_15
	TTS Speak "Motor go at 15 degree."
	Slot(Slot_M_1) = 15
	UpdateButtons
	Wait TTSready
	GreenOn
	WaitAllMotors
Return

Label Degree_30
	TTS Speak "Motor go at 30 degree."
	Slot(Slot_M_1) = 30
	UpdateButtons
	Wait TTSready
	YellowOn
	WaitAllMotors
Return

Label Degree_45
	TTS Speak "Motor go at 45 degree."
	Slot(Slot_M_1) = 45
	UpdateButtons
	Wait TTSready
	BlueOn
	WaitAllMotors
Return

Label Degree_90
	TTS Speak "Motor go at 90 degree."
	Slot(Slot_M_1) = 90
	UpdateButtons
	Wait TTSready
	WhiteOn
	WaitAllMotors
Return

Label Degree_135
	TTS Speak "Motor go at 135 degree."
	Slot(Slot_M_1) = 135
	UpdateButtons
	Wait TTSready
	RedOn
	WaitAllMotors
Return

Label Degree_180
	TTS Speak "Motor go at 180 degree."
	Slot(Slot_M_1) = 180
	UpdateButtons
	Wait TTSready
	GreenOn
	WaitAllMotors
Return

Label Degree_270
	TTS Speak "Motor go at 270 degree."
	Slot(Slot_M_1) = 270
	UpdateButtons
	Wait TTSready
	BlueOn
	WaitAllMotors
Return

Label Degree_315
	TTS Speak "Motor go at 315 degree."
	Slot(Slot_M_1) = 315
	Wait TTSready
	GreenOn
	WaitAllMotors
Return

Label Degree_360
	TTS Speak "Motor go at 360 degree."
	Slot(Slot_M_1) = 360
	UpdateButtons
	Wait TTSready
	WhiteOn
	WaitAllMotors
Return

' ╔══════════════════════════════════════════╗ █   █ █▀▀█ █ ▀▀█▀▀
' ║ WAIT ALL MOTOR until stopped             ║ █   █ █▄▄█ █   █
' ╚══════════════════════════════════════════╝ ▀▄▀▄▀ █  █ █   █
Label WaitAllMotors
	Variable Numeric WAM1
	Wait Seconds 0.1
	For WAM1 = 1 To Infinity
		If Abs(Slot(Slot_M_1 + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.05
		UpdateButtons
	Next
Return

' ╔══════════════════════════════════════════╗
' ║ EMERGENCY                                ║
' ╚══════════════════════════════════════════╝
Label Emergency
'	DisableAllButtons
'	Button 15 Enabled
'	Button 15 Color Flashing7 Yellow Red
'	For v1 = 1 To Infinity
'		RedOn
'		Wait Seconds 0.4
'		RgbOff
'		Wait Seconds 0.4
'		Load sound\Alarm10b.wav
'	Next
Stop

' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label EnableVoice
	TTS SetVolume 100
	Button EnableVoice Disabled
	Button DisableVoice Enabled
	
Return

Label DisableVoice
	TTS SetVolume 0
	Button EnableVoice Enabled
	Button DisableVoice Disabled
Return





'Label EnableMotors
'	Slot(Slot_EnMotors) = 1000
'	Button EnableMotors Disabled
'	Button DisableMotors Enabled
'	Wait Seconds 0.01
'Return
'
'Label DisableMotors
'	Slot(Slot_EnMotors) = 0
'	Button DisableMotors Disabled
'	Button EnableMotors Enabled
'	Wait Seconds 0.01
'Return


' ╔══════════════════════════════════════════╗ █   █▀▀ █▀▀▄
' ║ LED COLORS                               ║ █   █▀  █  █
' ╚══════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█
Label RedOn
	TTS Speak "Status red."
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
	Wait TTSready
Return

Label MagentaOn
	TTS Speak "Status magenta."
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
Return

Label YellowOn
	TTS Speak "Status yellow."
	Slot Slot_LedR = 900
	Slot Slot_LedG = 400
	Slot Slot_LedB = 0
	Wait TTSready
Return

Label GreenOn
	TTS Speak "Status green."
	Slot Slot_LedR = 0
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 0
	Wait TTSready
Return

Label BlueOn
	TTS Speak "Status blue."
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 1000
	Wait TTSready
Return

Label WhiteOn
	TTS Speak "Status white."
	Slot Slot_LedR = 1000
	Slot Slot_LedG = 1000
	Slot Slot_LedB = 1000
	Wait TTSready
Return

Label RgbOff
	TTS Speak "Status black."
	Slot Slot_LedR = 0
	Slot Slot_LedG = 0
	Slot Slot_LedB = 0
	Wait TTSready
Return

Label RedBlink
	RedOn
	WhiteOn
Return

' ╔══════════════════════════════════════════╗ █▀▀▀ █▀▀█ █     █▀▀▄ █  █ ▀▀█▀▀
' ║  COLOR BUTTON                            ║ █    █  █ █     █▀▀█ █  █   █
' ╚══════════════════════════════════════════╝ █▄▄▄ █▄▄█ █▄▄▄  █▄▄█ █▄▄█   █
Label ColorButton
	Variable Numeric CB1
	'Button ColorButton Color Flashing7 Black White
	For v1 = 1 To Infinity
		'------------------------------------- double check
		CB1 = Slot(Slot_AnalogBut)
		Wait Seconds 0.01
		If Abs(Slot(Slot_AnalogBut) - CB1) > 10
			CB1 = 99999
		EndIf
		'------------------------------------- Test values
		Select True
			Case CB1 < 16
				'YellowAction
			Case CB1 < 55
				'BlueAction
			Case CB1 < 120
				'GreenAction
			Case CB1 < 190
				'RedAction
			Case CB1 < 500
				'WhiteAction
		EndSelect
	Next
Return


Label SayYellow
	TTS Speak "Going Yellow target."
	Wait TTSready
Return

Label SayGreen
	TTS Speak "Going Green target, time interpolate 1 second."
	Wait TTSready
Return

Label SayBlue
	TTS Speak "RobotANNO Going Blue target."
	Wait TTSready
	ClearCommand
Return

Label SayRed
	TTS Speak "Going Red target, time interpolate 2 value."
	Wait TTSready
Return

Label SayWhite
	TTS Speak "Going White target, time interpolate 2.5 value."
	Wait TTSready
Return

Label SayBlack
	TTS Speak "Going Black target, time interpolate 3 value."
	Wait TTSready
Return

Label SayFastRndMove
	TTS Speak "Making random movement at high speed."
	Wait TTSready
Return

' ╔══════════════════════════════════════════╗ ▀▀█▀ █▀▀█ █▀▀█  █  █ █▀▀▀ █   █
' ║ JOG WITH KEYBOARD                        ║   █  █  █ █ ▄▄  █▄▀  █▀▀   ▀▄▀
' ╚══════════════════════════════════════════╝ █▄█  █▄▄█ █▄▄█  █ ▀▄ █▄▄▄   █
Label ExecKeys
	Variable Numeric JogSpeed = 0.1
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 0.01
		Case Key("CTRL")
			JogSpeed = JogSpeed * 0.001
	EndSelect
	'
	Select MotorToEdit
		Case 1
			If Key("Up")
				Slot(Slot_M_1) = Limit(Slot(Slot_M_1) - JogSpeed, M1min, M1max)
			EndIf
			If Key("Down")
				Slot(Slot_M_1) = Limit(Slot(Slot_M_1) + JogSpeed, M1min, M1max)
			EndIf
	EndSelect
	'
	PrintValues
	'EnableMotors
	'YellowOn
	UpdateButtons
Return

' ╔══════════════════════════════════════════╗ ▀▀█▀ █▀▀█ █▀▀▀   █▄ ▄█ █▀▀█ ▀▀█▀▀
' ║  MOTOR SELECTION FOR JOG                 ║   █  █  █ █ ▄▄   █ ▀ █ █  █   █
' ╚══════════════════════════════════════════╝ █▄█  █▄▄█ █▄▄█   █   █ █▄▄█   █
Variable Numeric MotorToEdit
Label defaultcolors
	Button BtnCoord_M1 Color LightBlue
Return

Label BtnCoord_M1
	MotorToEdit = 1
	defaultcolors
	Button BtnCoord_M1 Color Blue
Return

' ====================================================================
'  PRINT COORDINATES AND UPDATE COORDINATE BUTTONS
' ====================================================================
Label PrintValues
	s1 = "MoveTo " + Format(Slot(Slot_M_1), "0.00")
	Print CRLF + s1
Return

Label UpdateButtons
	Button BtnCoord_M1 Text "M1=" + Format(Slot(Slot_M_1) - Slot(Slot_M_1 + 1), "0.00") + "°"
	Button Temperature Text "Temp=" + Format(Slot(156), "0.0") + "°"
	Button Voltage Text "Volt=" + Format((Slot(155)/327), "0.0")
Return

' ╔══════════════════════════════════════════╗ █▀▀▀ █▀▀█ █▄ ▄█ █▄ ▄█   █▀▀▀ ▀▄ ▄▀ █▀▀▀ █▀▀▀
' ║ COMMANDS FROM COBOT                      ║ █    █  █ █ ▀ █ █ ▀ █   █▀▀    █   █▀   █
' ╚══════════════════════════════════════════╝ █▄▄▄ █▄▄█ █   █ █   █   █▄▄▄ ▄▀ ▀▄ █▄▄▄ █▄▄▄
Label Event_CommandsFromCobot
	' ------------------------------------------------------ Do not print lines containing ''
	Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select CommandText
			' ---------------------------------------------- ACTIONS
		Case "WaitAllMotors"
			WaitAllMotors
			'	-------------------------------------------- LED colors
		Case "Red"
			RedOn
		Case "Green"
			GreenOn
		Case "Blue"
			BlueOn
		Case "Yellow"
			YellowOn
		Case "RGBOFF"
			RgbOff
			'	-------------------------------------------- Speaking
		Case "SayYellow"
			SayYellow
		Case "SayGreen"
			SayGreen
		Case "SayBlue"
			SayBlue
		Case "SayRed"
			SayRed
		Case "SayWhite"
			SayWhite
		Case "SayBlack"
			SayBlack
		Case "SayFastRndMove"
			SayFastRndMove
			'	---------------------------------------------- LED COLORS
		Case "RedOn"
			RedOn
		Case "GreenON"
			GreenOn
		Case "MagentaOn"
			MagentaOn
		Case "GreenON"
			GreenOn
		Case "BlueOn"
			BlueOn
		Case "YellowOn"
			YellowOn
		Case "WhiteOn"
			WhiteOn
		Case "RGBOFF"
			RgbOff
		Case "Pause"
			Wait Seconds GetSeparatedString(CommandText, 1)
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + CommandText + CRLF
			Beep
			Stop								'
	EndSelect
	ClearCommand
Return

Label ClearCommand
	SlotText(Slot_CommandsFromCobot) = ""
Return

' ====================================================================
'  COBOT FILE MANAGEMENT
' ====================================================================
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

' ====================================================================
'  COMMANDS TO Cobot
' ====================================================================
Label SendCommand
	Variable String Command
	SlotText Slot_CommandsToCobot = Command
	Variable Numeric SC1
	' ------------------- timeout 1 Sec (10x0.1)
	For SC1 = 1 To 10
		If SlotText(Slot_CommandsToCobot) = ""
			Exit
		EndIf
		Wait Seconds 0.1
	Next
Return

Label ExecCobot
	Variable Numeric EC1
	For EC1 = 1 To Infinity
		SendCommand(DisableRepeat)
		SendCommand(SelectLine 1)
		SendCommand(StartExecution)
		WaitCobotReady
	Next
Return

Label StopCobot
	SendCommand(StopExecution)
Return

Label WaitCobotReady
	Variable Numeric WCR1
	For WCR1 = 1 To Infinity
		UpdateButtons
		Wait Seconds 0.05
		If SlotText(Slot_ResponsesFromCobot) = "Ready"
			Exit
		EndIf
	Next
Return

Label SelectSequence
	Variable String SCF1
	SCF1 = SelectFile("Apps\Theremino_Cobot\Sequences")
	If SCF1 <> ""
		SequenceName = SCF1
	EndIf
	SendCommand(LoadSequence + SequenceName)
	ShowSequenceName
Return

Label ShowSequenceName
	Button SelectSequence Text SequenceName
Return

'--------------------------------------------------------------
'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
'--------------------------------------------------------------


' ╔════════════════════════════════════════════╗  █▀▀▀ █▀▀█ █▀█    █▀▀▀ █▀▀█ █▄ ▄█
' ║ COMMANDS to COBOT                          ║  █    █  █ █▀▀█   █    █  █ █ ▀ █
' ╚════════════════════════════════════════════╝  █▄▄▄ █▄▄█ █▄▄█   █▄▄▄ █▄▄█ █   █


