'╔═════════════════════════════════════════════════════════╗
'║     Name: RobotAnnoCOBOT_V02.txt      Date: 30 Jun 2022 ║
'║   Author: Leonardo                                      ║
'║    Scope: Test the functionality of Palletized arm      ║
'╠═════════════════════════════════════════════════════════╣
'║Hardware: RobotAnno model:                               ║
'╟─────────────────────────────────────────────────────────╢
'║ Note: Cobot version                         28 Jun 2022 ║
'║                                                         ║
'╚═════════════════════════════════════════════════════════╝
Option Speed 9
Beep "2500 100, 1500 100"

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

RestoreVars

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

' ╔══════════════════════════════════════════╗
' ║ 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 NumCycle
'Variable Numeric StartTime
'Variable String  Result
'Variable Numeric Slot_Emergency

Variable Numeric Slot_M1  = 101
Variable Numeric Slot_M2  = 201
Variable Numeric Slot_M3  = 301
Variable Numeric Slot_M4  = 401
Variable Numeric Slot_M5  = 501
Variable Numeric Slot_M6  = 601
Variable Numeric Slot_M7  = 701

'Variable Numeric Slot_EnableMotors =  11

Variable Numeric Slot_AnalogBut =   4
Variable Numeric Slot_LedR		=  19
Variable Numeric Slot_LedG		=  20
Variable Numeric Slot_LedB		=  21

Variable Numeric Slot_SignalScope       =  13

' -------------------------------------- Software limits
Variable Numeric M1max =     0
Variable Numeric M1min =   360
Variable Numeric M2max =     0
Variable Numeric M2min =   360
Variable Numeric M3max =     0
Variable Numeric M3min =   360
Variable Numeric M4max =     0
Variable Numeric M4min =   360
Variable Numeric M5max =     0
Variable Numeric M5min =   360
Variable Numeric M6max =     0
Variable Numeric M6min =   360
Variable Numeric M7max =    10
Variable Numeric M7min =   170


Key 1 BtnCoord_M1
Key 2 BtnCoord_M2
Key 3 BtnCoord_M3
Key 4 BtnCoord_M4
Key 5 BtnCoord_M5
Key 6 BtnCoord_M6
Key 7 BtnCoord_M7

' ╔══════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                  ║ █▀▀█ █  █   █
' ╚══════════════════════════════════════════╝ █▄▄█ █▄▄█   █
Button 1 Text HardHome
Button HardHome Color Coral

Button 2 Text "BtnCoord_M1"
Button BtnCoord_M1 Text "M1 = ?"
Button BtnCoord_M1 Color LightBlue
Button 3 Text "BtnCoord_M2"
Button BtnCoord_M2 Text "M2 = ?"
Button BtnCoord_M2 Color LightBlue
Button 4 Text "BtnCoord_M3"
Button BtnCoord_M3 Text "M3 = ?"
Button BtnCoord_M3 Color LightBlue
Button 5 Text "BtnCoord_M4"
Button BtnCoord_M4 Text "M4 = ?"
Button BtnCoord_M4 Color LightBlue
Button 6 Text "BtnCoord_M5"
Button BtnCoord_M5 Text "M5 = ?"
Button BtnCoord_M5 Color LightBlue
Button 7 Text "BtnCoord_M6"
Button BtnCoord_M6 Text "M6 = ?"
Button BtnCoord_M6 Color LightBlue
Button 7 Text "BtnCoord_M7"
Button BtnCoord_M7 Text "M7= ?"
Button BtnCoord_M7 Color LightBlue

Button 17 Text YELLOW
Button Yellow Color Yellow

Button 18 Text BLUE
Button Blue Color Blue

Button 19 Text RED
Button Red Color Red

Button 20 Text GREEN
Button Green Color Green

Button 21 Text WHITE
Button White Color White

Button 22 Text BLACK
Button Black Color Black

Button 23 Text SafePos
Button SafePos Text "90 90 90"
Button SafePos Color Bisque

Button 24 Text ColorButton
Button ColorButton  Text "COLOR BUTTON"
Button ColorButton  Color Plum

Button 25 Text ExecCobot
Button ExecCobot  Text "Exec Cobot"
Button ExecCobot  Color Cyan

Button 27 Text MotorTest
Button MotorTest  Text "MotorTest"
Button MotorTest  Color Orange

Button 16 Text CloseAll
Button CloseAll Text "CLOSE ALL"
Button CloseAll Color Red
'
'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 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

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

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

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


' ╔══════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                     ║ █   █  █ █  █ █▄▄█
' ╚══════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	
	' ------------------------------------------
	' 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
	Save Vars
	Print CRLF + "SAVED SEQUENCENAME = " + s1 + CRLF
Return

Label RestoreVars
	Load Vars
Return

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

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

' ╔══════════════════════════════════════════╗ █  █ █▀▀█ █▀▀█ █▀▀▄  █  █ █▀▀█ █▄ ▄█ █▀▀▀
' ║ HARD HOME ALL 3 STEPPERS                 ║ █▀▀█ █▀▀█ ██▀▀ █  █  █▀▀█ █  █ █ ▀ █ █▀▀
' ╚══════════════════════════════════════════╝ █  █ █  █ █ ▀▄ █▄▄█  █  █ █▄▄█ █   █ █▄▄▄
Label HardHome
	HomeM1
	HomeM2
	HomeM3
	HomeM4
	HomeM5
	HomeM6
	HomeM7
Return

' ============================================================
'   Wait until the rotation motor is stopped
' ============================================================
Label HomeM1
	Slot(Slot_M1) = Reset
	Wait Seconds 0.1
	Slot(Slot_M1) = 90
	Wait Seconds 0.1
	'Gosub PrintValues
	Slot(Slot_M1) = 90
Return

Label HomeM2
	Slot(Slot_M2) = Reset
	Wait Seconds 0.1
	Slot(Slot_M2) = 90
	Wait Seconds 0.1
	'Gosub PrintValues
	Slot(Slot_M2) = 90
Return

Label HomeM3
	Slot(Slot_M3) = Reset
	Wait Seconds 0.1
	Slot(Slot_M3) = 90
	Wait Seconds 0.1
	'Gosub PrintValues
	Slot(Slot_M3) = 90
Return

Label HomeM4
	Slot(Slot_M4) = Reset
	Wait Seconds 0.1
	Slot(Slot_M4) = 90
	Wait Seconds 0.1
	'Gosub PrintValues
	Slot(Slot_M4) = 90
Return

Label HomeM5
	Slot(Slot_M5) = Reset
	Wait Seconds 0.1
	Slot(Slot_M5) = 90
	Wait Seconds 0.1
	'Gosub PrintValues
	Slot(Slot_M5) = 90
Return

Label HomeM6
	Slot(Slot_M6) = Reset
	Wait Seconds 0.1
	Slot(Slot_M6) = 90
	Wait Seconds 0.1
	'Gosub PrintValues
	Slot(Slot_M6) = 90
Return

Label HomeM7
	Slot(Slot_M7) = Reset
	Wait Seconds 0.1
	Slot(Slot_M7) = 500
	Wait Seconds 0.1
	'Gosub PrintValues
	Slot(Slot_M6) = 500
Return


' ╔══════════════════════════════════════════╗ █   █ █▀▀█ █ ▀▀█▀▀
' ║ WAIT ALL MOTOR until stopped             ║ █   █ █▄▄█ █   █
' ╚══════════════════════════════════════════╝ ▀▄▀▄▀ █  █ █   █
Label WaitAllMotors
	Variable Numeric WAM1
	Wait Seconds 0.1
	For WAM1 = 1 To Infinity
		If Abs(Slot(Slot_M1 + 1)) < 0.01
			If Abs(Slot(Slot_M2 + 1)) < 0.01
				If Abs(Slot(Slot_M3 + 1)) < 0.01
					If Abs(Slot(Slot_M4 + 1)) < 0.01
						If Abs(Slot(Slot_M5 + 1)) < 0.01
							If Abs(Slot(Slot_M6 + 1)) < 0.01
								Exit
							EndIf
						EndIf
					EndIf
				EndIf
			EndIf
		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


' ╔══════════════════════════════════════════╗ █   █▀▀ █▀▀▄
' ║ 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
	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
	Button ColorButton Color Flashing7 Black White
	For v1 = 1 To Infinity
		If Int(Slot(Slot_AnalogBut)) = 0 And Slot(Slot_AnalogBut) < 2
			YELLOW
		EndIf
		
		If Int(Slot(Slot_AnalogBut)) > 43 And Slot(Slot_AnalogBut) < 49
			BLUE
		EndIf
		
		If Int(Slot(Slot_AnalogBut)) > 244 And Slot(Slot_AnalogBut) < 250
			RED
		EndIf
		
		If Slot(Slot_AnalogBut) > 128 And Slot(Slot_AnalogBut) < 134
			GREEN
		EndIf
		
		If Int(Slot(Slot_AnalogBut)) > 521 And Slot(Slot_AnalogBut) < 527
			WHITE
		EndIf
		
		Wait Seconds 0.01
	Next
Return

' ╔══════════════════════════════════════════╗ █▀▀█ █▀▀█ █▄ ▄█  █▀▀▀ █▀▀█ █
' ║  ARM COLOR POSITION                      ║ █▄▄█ █▄▄█ █ ▀ █  █    █  █ █
' ╚══════════════════════════════════════════╝ █  █ █ ▀▄ █   █  █▄▄▄ █▄▄█ █▄▄▄
Label MotorTest
	Variable Numeric MO1
	For MO1 = 1 To Infinity
		Slot(Slot_M1) =  0
		Slot(Slot_M2) =  0
		Slot(Slot_M3) =  0
		Slot(Slot_M4) =  0
		Slot(Slot_M5) = 45
		Slot(Slot_M6) = 0
		Slot(Slot_M7) = 100
		WaitAllMotors
		Slot(Slot_M6) = 180
		WaitAllMotors
		Slot(Slot_M6) = 0
		WaitAllMotors
'		Wait Seconds 0.5
		Slot(Slot_M1) =  90
		Slot(Slot_M2) =  90
		Slot(Slot_M3) = 180
		Slot(Slot_M4) = 180
		Slot(Slot_M5) = 135
		Slot(Slot_M6) = 180
		Slot(Slot_M7) = 900
		WaitAllMotors
		Wait Seconds 0.5
	Next
Return

Label YELLOW
	TTS Speak "Going to yellow dear Kemiao."
	SafePos
	Slot(Slot_M1) = 66
	Slot(Slot_M2) = 67.35
	Slot(Slot_M3) = 72.50
	WaitAllMotors
Return

Label BLUE
	TTS Speak "Going to blue."
	SafePos
	Slot(Slot_M1) = 84
	Slot(Slot_M2) = 69.49
	Slot(Slot_M3) = 68.75
	WaitAllMotors
Return

Label RED
	TTS Speak "Going to red."
	SafePos
	Slot(Slot_M1) = 93
	Slot(Slot_M2) = 69
	Slot(Slot_M3) = 62
	WaitAllMotors
Return

Label GREEN
	TTS Speak "Going to green."
	SafePos
	Slot(Slot_M1) = 69
	Slot(Slot_M2) = 70.20
	Slot(Slot_M3) = 64.27
	WaitAllMotors
Return

Label BLACK
	TTS Speak "Going to black."
	SafePos
	Slot(Slot_M1) = 122
	Slot(Slot_M2) = 70
	Slot(Slot_M3) = 68
	WaitAllMotors
Return

Label WHITE
	TTS Speak "Going to white."
	SafePos
	Slot(Slot_M1) = 104
	Slot(Slot_M2) = 69.49
	Slot(Slot_M3) = 68.75
	WaitAllMotors
Return

Label SafePos
	Slot(Slot_M1) =  90
	Slot(Slot_M2) =  90
	Slot(Slot_M3) =  90
	Slot(Slot_M4) =  90
	Slot(Slot_M5) =  90
	Slot(Slot_M6) =  90
	Slot(Slot_M7) = 170
	WaitAllMotors
Return

Label RepTest
	For v99 = 1 To Infinity
		Slot(Slot_M1) = 70
		Slot(Slot_M2) = 90
		Slot(Slot_M3) = 90
		WaitAllMotors
		
		Slot(Slot_M1) = 110
		Slot(Slot_M2) = 90
		Slot(Slot_M3) = 90
		WaitAllMotors
		
		Slot(Slot_M1) = 90
		Slot(Slot_M2) = 76
		Slot(Slot_M3) = 82
		WaitAllMotors
		
		Wait Seconds 0.2
		Slot(Slot_M1) = 90
		Slot(Slot_M2) = 76
		Slot(Slot_M3) = 76
		WaitAllMotors
		
		Wait Seconds 1.3
		Slot(Slot_M1) = 90
		Slot(Slot_M2) = 76
		Slot(Slot_M3) = 82
		WaitAllMotors
	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


' =====================================================================
'  STEPPER-MOTORS MOVE SLOW
' =====================================================================
Variable Numeric MSV1
Variable Numeric MSV2
Variable Numeric MSV3
Variable Numeric MSV4
Variable Numeric MSV5

' =====================================================================
'  MOVE SLOW
' =====================================================================
Variable Numeric MoveSlow_DestM1 	' destination in mm
Variable Numeric MoveSlow_DestM2 	' destination in mm
Variable Numeric MoveSlow_DestM3 	' destination in mm
Variable Numeric MoveSlow_Speed 	' speed in mm/sec

Label MoveSlowM1
	' ----------------------------------- test for out of limit
	Slot(Slot_M1) = Limit(Slot(Slot_M1), M1min, M1max)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_DestM1 - Slot(Slot_M1)
	MSV1 = Abs(MSV1)
	' ----------------------------------- calc the loops count
	MSV4 = MSV1 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(Slot_M1) = Limit(MoveSlow_DestM1, M1min, M1max)
		Return
	EndIf
	' ----------------------------------- prepare the increment
	MSV1 = (MoveSlow_DestM1 - Slot(Slot_M1)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_M1) = Limit(Slot(Slot_M1) + MSV1, M1min, M1max)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return

Label MoveSlowM2
	' ----------------------------------- test for out of limit
	Slot(Slot_M2) = Limit(Slot(Slot_M2), M2min, M2max)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_DestM2 - Slot(Slot_M2)
	MSV1 = Abs(MSV1)
	' ----------------------------------- calc the loops count
	MSV4 = MSV1 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(Slot_M2) = Limit(MoveSlow_DestM2, M2min, M2max)
		Return
	EndIf
	' ----------------------------------- prepare the increment
	MSV1 = (MoveSlow_DestM2 - Slot(Slot_M2)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_M2) = Limit(Slot(Slot_M2) + MSV1, M2min, M2max)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return

Label MoveSlowM3
	' ----------------------------------- test for out of limit
	Slot(Slot_M3) = Limit(Slot(Slot_M3), M3min, M3max)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_DestM3 - Slot(Slot_M3)
	MSV1 = Abs(MSV1)
	' ----------------------------------- calc the loops count
	MSV4 = MSV1 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(Slot_M3) = Limit(MoveSlow_DestM3, M3min, M3max)
		Return
	EndIf
	' ----------------------------------- prepare the increment
	MSV1 = (MoveSlow_DestM3 - Slot(Slot_M3)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_M3) = Limit(Slot(Slot_M3) + MSV1, M3min, M3max)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return

Label MoveSlowM1M2
	' ----------------------------------- test for out of limit
	Slot(Slot_M1) = Limit(Slot(Slot_M1), M1min, M1max)
	Slot(Slot_M2) = Limit(Slot(Slot_M2), M2min, M2max)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_DestM1 - Slot(Slot_M1)
	MSV2 = MoveSlow_DestM2 - Slot(Slot_M2)
	MSV3 = MSV1 * MSV1
	MSV3 = MSV3 + MSV2 * MSV2
	MSV3 = Sqrt(MSV3)
	' ----------------------------------- calc the loops count
	MSV4 = MSV3 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(Slot_M1) = Limit(MoveSlow_DestM1, M1min, M1max)
		Slot(Slot_M2) = Limit(MoveSlow_DestM2, M2min, M2max)
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlow_DestM1 - Slot(Slot_M1)) / MSV4
	MSV2 = (MoveSlow_DestM2 - Slot(Slot_M2)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV3 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_M1) = Limit(Slot(Slot_M1) + MSV1, M1min, M1max)
		Slot(Slot_M2) = Limit(Slot(Slot_M2) + MSV2, M2min, M2max)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return

Label MoveSlowM1M2M3
	' ----------------------------------- test for out of limit
	Slot(Slot_M1) = Limit(Slot(Slot_M1), M1min, M1max)
	Slot(Slot_M2) = Limit(Slot(Slot_M2), M2min, M2max)
	Slot(Slot_M3) = Limit(Slot(Slot_M3), M3min, M3max)
	' ----------------------------------- test for very low speed
	If MoveSlow_Speed < 0.1
		MoveSlow_Speed = 1
	EndIf
	' ----------------------------------- calc the movement in mm
	MSV1 = MoveSlow_DestM1 - Slot(Slot_M1)
	MSV2 = MoveSlow_DestM2 - Slot(Slot_M2)
	MSV3 = MoveSlow_DestM3 - Slot(Slot_M3)
	MSV3 = MSV1 * MSV1 + MSV2 * MSV2 + MSV3 * MSV3
	MSV3 = Sqrt(MSV3)
	' ----------------------------------- calc the loops count
	MSV4 = MSV3 / 0.01 '<<< loop time
	MSV4 = Int(MSV4 / MoveSlow_Speed)
	' ----------------------------------- too much speed ?
	If MSV4 = 0
		Slot(Slot_M1) = Limit(MoveSlow_DestM1, M1min, M1max)
		Slot(Slot_M2) = Limit(MoveSlow_DestM2, M2min, M2max)
		Slot(Slot_M3) = Limit(MoveSlow_DestM3, M3min, M3max)
		Return
	EndIf
	' ----------------------------------- prepare the increments
	MSV1 = (MoveSlow_DestM1 - Slot(Slot_M1)) / MSV4
	MSV2 = (MoveSlow_DestM2 - Slot(Slot_M2)) / MSV4
	MSV3 = (MoveSlow_DestM3 - Slot(Slot_M3)) / MSV4
	' ----------------------------------- loop for v4 times
	For MSV5 = 1 To MSV4
		' ------------------------------- increment, move and wait
		Slot(Slot_M1) = Limit(Slot(Slot_M1) + MSV1, M1min, M1max)
		Slot(Slot_M2) = Limit(Slot(Slot_M2) + MSV2, M2min, M2max)
		Slot(Slot_M3) = Limit(Slot(Slot_M3) + MSV3, M3min, M3max)
		Wait Seconds 0.01 '<<< loop time
	Next
	Gosub WaitAllMotors
Return

' ====================================================================
'   CONFIGURATION FILE
'=====================================================================
Label EditConfigVars
	Variable String VarsFilePathAndName
	Variable String EditorFolder
	Variable String EditorApp
    '
	EditorFolder = ".\Theremino_HAL\Theremino_Notepad\"
	'
	EditorApp = EditorFolder + "Theremino_Notepad.exe"
	'
	VarsFilePathAndName = EditorFolder + "ConfigVars.txt"
	'
	Load EditorApp + " " + VarsFilePathAndName
Return

' ╔══════════════════════════════════════════╗ ▀▀█▀ █▀▀█ █▀▀█  █  █ █▀▀▀ █   █
' ║ JOG WITH KEYBOARD                        ║   █  █  █ █ ▄▄  █▄▀  █▀▀   ▀▄▀
' ╚══════════════════════════════════════════╝ █▄█  █▄▄█ █▄▄█  █ ▀▄ █▄▄▄   █
Label ExecKeys
	Variable Numeric JogSpeed = 1
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 0.1
		Case Key("CTRL")
			JogSpeed = JogSpeed * 0.01
	EndSelect
	'
	Select MotorToEdit
		Case 1
			If Key("Up")
				Slot(Slot_M1) = Limit(Slot(Slot_M1) - JogSpeed, M1min, M1max)
			EndIf
			If Key("Down")
				Slot(Slot_M1) = Limit(Slot(Slot_M1) + JogSpeed, M1min, M1max)
			EndIf
		Case 2
			If Key("Up")
				Slot(Slot_M2) = Limit(Slot(Slot_M2) + JogSpeed, M2min, M2max)
			EndIf
			If Key("Down")
				Slot(Slot_M2) = Limit(Slot(Slot_M2) - JogSpeed, M2min, M2max)
			EndIf
		Case 3
			If Key("Up")
				Slot(Slot_M3) = Limit(Slot(Slot_M3) + JogSpeed, M3min, M3max)
			EndIf
			If Key("Down")
				Slot(Slot_M3) = Limit(Slot(Slot_M3) - JogSpeed, M3min, M3max)
			EndIf
		Case 4
			If Key("Up")
				Slot(Slot_M4) = Limit(Slot(Slot_M4) - JogSpeed, M1min, M1max)
			EndIf
			If Key("Down")
				Slot(Slot_M4) = Limit(Slot(Slot_M4) + JogSpeed, M4min, M4max)
			EndIf
		Case 5
			If Key("Up")
				Slot(Slot_M5) = Limit(Slot(Slot_M5) + JogSpeed, M5min, M5max)
			EndIf
			If Key("Down")
				Slot(Slot_M5) = Limit(Slot(Slot_M5) - JogSpeed, M5min, M5max)
			EndIf
		Case 6
			If Key("Up")
				Slot(Slot_M6) = Limit(Slot(Slot_M6) + JogSpeed, M6min, M6max)
			EndIf
			If Key("Down")
				Slot(Slot_M6) = Limit(Slot(Slot_M6) - JogSpeed, M6min, M6max)
			EndIf
		Case 7
			If Key("Up")
				Slot(Slot_M7) = Limit(Slot(Slot_M7) - JogSpeed, M7min, M7max)
			EndIf
			If Key("Down")
				Slot(Slot_M7) = Limit(Slot(Slot_M7) + JogSpeed, M7min, M7max)
			EndIf
	EndSelect
	'
	PrintValues
	'EnableMotors
	YellowOn
	UpdateButtons
Return

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

Label BtnCoord_M1
	MotorToEdit = 1
	defaultcolors
	Button BtnCoord_M1 Color Blue
Return

Label BtnCoord_M2
	MotorToEdit = 2
	defaultcolors
	Button BtnCoord_M2 Color Blue
Return

Label BtnCoord_M3
	MotorToEdit = 3
	defaultcolors
	Button BtnCoord_M3 Color Blue
Return

Label BtnCoord_M4
	MotorToEdit = 4
	defaultcolors
	Button BtnCoord_M4 Color Blue
Return

Label BtnCoord_M5
	MotorToEdit = 5
	defaultcolors
	Button BtnCoord_M5 Color Blue
Return

Label BtnCoord_M6
	MotorToEdit = 6
	defaultcolors
	Button BtnCoord_M6 Color Blue
Return

Label BtnCoord_M7
	MotorToEdit = 7
	defaultcolors
	Button BtnCoord_M7 Color Blue
Return

' ====================================================================
'  PRINT COORDINATES AND UPDATE COORDINATE BUTTONS
' ====================================================================
Label PrintValues
	s1 = "Move_M1M2M3  " + Format(Slot(Slot_M1), "0.00")
	s1 = s1 + "  " + Format(Slot(Slot_M2), "0.00")
	s1 = s1 + "  " + Format(Slot(Slot_M3), "0.00")
	Print CRLF + s1
Return

Label UpdateButtons
	Button BtnCoord_M1 Text "M1=" + Format(Slot(Slot_M1) - Slot(Slot_M1 + 1), "0.00")
	Button BtnCoord_M2 Text "M2=" + Format(Slot(Slot_M2) - Slot(Slot_M2 + 1), "0.00")
	Button BtnCoord_M3 Text "M3=" + Format(Slot(Slot_M3) - Slot(Slot_M3 + 1), "0.00")
	Button BtnCoord_M4 Text "M4=" + Format(Slot(Slot_M4) - Slot(Slot_M4 + 1), "0.00")
	Button BtnCoord_M5 Text "M5=" + Format(Slot(Slot_M5) - Slot(Slot_M5 + 1), "0.00")
	Button BtnCoord_M6 Text "M6=" + Format(Slot(Slot_M6) - Slot(Slot_M6 + 1), "0.00")
	Button BtnCoord_M7 Text "M7=" + Format(Slot(Slot_M7) - Slot(Slot_M7 + 1), "0.00")
Return

' ╔══════════════════════════════════════════╗ █▀▀▀ █▀▀█ █▄ ▄█ █▄ ▄█  █▀▀▀ ▀▄ ▄▀ █▀▀▀ █▀▀▀
' ║ COMMANDS FROM COBOT                      ║ █    █  █ █ ▀ █ █ ▀ █  █▀▀    █   █▀   █
' ╚══════════════════════════════════════════╝ █▄▄▄ █▄▄█ █   █ █   █  █▄▄▄ ▄▀ ▀▄ █▄▄▄ █▄▄▄
Label Event_CommandsFromCobot
	' ------------------------------------------------------ Do not print lines containing ''
	Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select CommandText
			' ---------------------------------------------- ACTIONS
		Case "HardHome"
			HardHome
		Case "WaitAllMotors"
			WaitAllMotors
			'	-------------------------------------------- Motor movementD COLO
		Case "Red"
			RED
		Case "Green"
			GREEN
		Case "Blue"
			BLUE
		Case "Yellow"
			YELLOW
		Case "BLACK"
			BLACK
			'	-------------------------------------------- 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

' ====================================================================
'  COMMANDS TO Cobot
' ====================================================================
Variable String Command
Label SendCommand
	SlotText Slot_CommandsToCobot = ""
	Wait Seconds 0.05
	SlotText Slot_CommandsToCobot = Command
	Wait Seconds 0.05
Return

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

Label StopCobot
	Command = "StopExecution"
	SendCommand
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

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


