'╔═══════════════════════════════════════════════════════════╗
'║     Name: CM_EndoRobo_14.txt            Date: 07 Jul 2024 ║
'║   Author: Theremino Team / Leonardo                       ║
'║    Scope: Test stepper motor sequences                    ║
'╠═══════════════════════════════════════════════════════════╣
'║Hardware: MAster V5            Power: 24 Volt              ║
'╟───────────────────────────────────────────────────────────╢
'║   Motor 1   In OUT      Stepper Driver: SL2236A           ║
'║   Oukeda OK42STH40-1684AG5     reduction 5.18:1           ║
'║       current          microstep                          ║
'║   1   2   3   4      5   6   7   8                        ║
'║  OFF  ON  ON OFF    OFF OFF  ON  ON  1.1A  1600 steps/360 ║
'║ Master   step per mm = 26.72                              ║
'╟───────────────────────────────────────────────────────────╢
'║   Motor 2   ROTATION    Stepper Driver: SL2236A           ║
'║   SHENLI DLE42-25-J8-S248A108     reduction 25:1          ║
'║       current          microstep                          ║
'║   1   2   3   4      5   6   7   8                        ║
'║  OFF  ON  ON OFF    OFF OFF  ON  ON  1.1A  1600 steps/360 ║
'║ Master   step per mm = 26.72                              ║
'║ Power: 24 V                                               ║
'╟───────────────────────────────────────────────────────────╢
'║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 8
Beep "2500 20, 1500 100"
Load images\Rotation_shaft.jpg
TTS SelectVoice "ENG"
s1 = TTSvoices
TTS SetVolume 100
TTS SetSpeed -2
TTS SelectVoice 3
'TTS Speak "Theremino Automation programming language is running."

' ╔════════════════════════════════════════════╗
' ║ 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
'Variable Numeric Slot_WordRecognized        = 55

' ╔════════════════════════════════════════════╗ █▀▀▀ █   █▀▀█ ▀▀█▀▀
' ║ SLOT INITIALIZATIONS                       ║ ▀▀▀█ █   █  █   █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄ █▄▄█   █
Variable Numeric Slot_M1_InOutDesti     = 101
Variable Numeric Slot_M1_InOutPosit     = 102
Variable Numeric Slot_LimitOut          = 103

Variable Numeric Slot_M2_RotDestina     = 151
Variable Numeric Slot_M2_RotPositio     = 152
Variable Numeric Slot_LimitRot          = 153

Variable Numeric Slot_M3_TipDestina     = 201
Variable Numeric Slot_M3_TipPositio     = 202
'Variable Numeric Slot_LimitTip          = 203

Variable Numeric Slot_M4_ElevDestin     = 251
Variable Numeric Slot_M4_ElevPositi     = 252
Variable Numeric Slot_LimitElev         = 253

Variable Numeric Slot_JoyPadX          = 500
Variable Numeric Slot_JoyPadY          = 501
Variable Numeric Slot_JoyPadSpeed      = 502
Variable Numeric Slot_JoyPadT          = 503
Variable Numeric Slot_JoyPad8W         = 506


Variable Numeric Slot_JoyBtn1          = 510
Variable Numeric Slot_JoyBtn2          = 511
Variable Numeric Slot_JoyBtn3          = 512
Variable Numeric Slot_JoyBtn4          = 513
Variable Numeric Slot_JoyBtn5          = 514
Variable Numeric Slot_JoyBtn6          = 515
Variable Numeric Slot_JoyBtn7          = 516
Variable Numeric Slot_JoyBtn8          = 517
Variable Numeric Slot_JoyBtn9          = 518
Variable Numeric Slot_JoyBtn10         = 519
Variable Numeric Slot_JoyBtn11         = 520
Variable Numeric Slot_JoyBtn12         = 521

Variable Numeric Slot_StartStopVideo   = 520
Variable Numeric Slot_SaveVideo        = 521
Variable Numeric Slot_TakeImage        = 522

Variable Numeric JoyPad_DeadBand       = 30
Variable Numeric JoyPad_Speed          = 50
Variable Numeric KeyJog_Speed          = 50

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

Variable Numeric Slot_MotorsOff    = 60
Variable Numeric Slot_SignalScope    = 13

' ╔════════════════════════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀
' ║ BUTTONS                                    ║ █▀▀█ █  █   █
' ╚════════════════════════════════════════════╝ █▄▄█ █▄▄█   █
'---------------------------------------------------------End motor M1
Button 1 Label M1_IN_OUT
Button M1_IN_OUT Text "M1" + CRLF + "IN/OUT"
Button M1_IN_OUT Color SteelBlue

Button 2 Label M1_Destination
Button M1_Destination Text "Destination"

Button 3 Label M1_Position
Button M1_Position Text "Position"

Button 4 Label M1_Home
Button M1_Home Text "Home"
Button M1_Home Color Yellow

Button 5 Label M1_Arbitrary
Button M1_Arbitrary Text "Arbitrary"
Button M1_Arbitrary Color Cyan
'---------------------------------------------------------End motor M1

'---------------------------------------------------------Begin motor M2
Button 17 Label M2_Rotation
Button M2_Rotation Text "M2" + CRLF + "ROTATION"
Button M2_Rotation Color SteelBlue

Button 18 Label M2_Destination
Button M2_Destination Text "Destination"

Button 19 Label M2_Position
Button M2_Position Text "Position"

Button 20 Label M2_Home
Button M2_Home Text "Home"
Button M2_Home Color Yellow

Button 21 Label M2_Arbitrary
Button M2_Arbitrary Text "Arbitrary"
Button M2_Arbitrary Color Cyan
''---------------------------------------------------------End motor M2
'
''---------------------------------------------------------Begin motor M3
Button 33 Label M3_Tip
Button M3_Tip Text "M3" + CRLF + "Tip"
Button M3_Tip Color SteelBlue

Button 34 Label "M3_Destination"
Button M3_Destination Text "Destination"

Button 35 Label M3_Position
Button M3_Position Text "Position"

Button 36 Label M3_Home
Button M3_Home Text "Home"
Button M3_Home Color Yellow

Button 37 Label M3_Arbitrary
Button M3_Arbitrary Text "Arbitrary"
Button M3_Arbitrary Color Cyan
'---------------------------------------------------------End motor M3

'---------------------------------------------------------Begin motor M4
Button 49 Label M4_Elevation
Button M4_Elevation Text  "M4" + CRLF + "ELEVATION"
Button M4_Elevation Color SteelBlue

Button 50 Label "M4_Destination"
Button M4_Destination Text "Destination"

Button 51 Label M4_Position
Button M4_Position Text "Position"

Button 52 Label M4_Home
Button M4_Home Text "Home"
Button M4_Home Color Yellow

Button 53 Label M4_Arbitrary
Button M4_Arbitrary Text "Arbitrary"
Button M4_Arbitrary Color Cyan
'---------------------------------------------------------End motor M4

Button 9 Label Motors_Enable
Button Motors_Enable Text "Motors ON"
Button Motors_Enable Color Lime

Button 25 Label Sequence
Button Sequence Text "Sequence"
Button Sequence Color MediumBlue

Button 26 Label VoiceOutputOnOff
Button VoiceOutputOnOff Text "Voice output ON"
Button VoiceOutputOnOff Color Green

Button 41 Label RecognizeEncoder
Button RecognizeEncoder  Text "Recognize" + CRLF + "Encoder"
Button RecognizeEncoder  Color LightBlue

Button 42 Label JoyPad_Enable
Button JoyPad_Enable Text "JoyPad OFF"
Button JoyPad_Enable Color Red2

Button 57 Label AllZero
Button AllZero Text "Main Home"
Button AllZero Color Yellow

Button 10 Label Say
Button Say Text "Say"
Button Say Color DeepSkyBlue

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

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀▄
' ║ LOAD APPS                                  ║ █   █  █ █▄▄█ █  █
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █  █ █▄▄█
Load Theremino_HAL.exe
Load Theremino_GamePad.exe
Load Theremino_SlotViewer.exe
Load Theremino_SlotViewer_STR.exe
Load Theremino_SlotViewer_PAD.exe

'Load Theremino_VoiceCommands.exe

Load Theremino_SignalScope.exe

Load Theremino_Cobot.exe
'WaitApplicationsStarted
Load Hide

' ╔════════════════════════════════════════════╗  █▀▀▀ ▀▀█▀▀ █▀▀▀█ █▀▀▀█ ▀▀█▀▀
' ║  EXECUTION START FROM HERE                 ║  ▀▀▀█   █   █▀▀▀█ █▀█▀▀   █
' ╚════════════════════════════════════════════╝  ▄▄▄█   █   █   █ █  ▀▄   █
'ResetEncoders
Slot(Slot_SignalScope) = 1000
Slot(Slot_MotorsOff) = 0   'All motors Enabled

' ------------------------- GOTO Homing - Command to Cobot
WaitCobotReady
'SendCommand(Slot_CommandsToCobot)("GotoHoming")

Controls OpenTextBox
Controls ClearTextBox

'AllZero

Loop

Stop

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	'InterpretateVoiceCommands
    'Encoder
	
	If Slot(0) = -2
		Print "Master disconnected."
		End
	EndIf
	UpdatePositions
	CheckStatusJoyPad
	JoyPad
Goto Loop
Stop

' ╔════════════════════════════════════════════╗
' ║ ENABLE/DISABLE BUTTONS                     ║
' ╚════════════════════════════════════════════╝
Variable Numeric EAB
Label EnableAllButtons
	For EAB = 1 To 64
		Button EAB Enabled
	Next
Return

Variable Numeric DAB
Label DisableAllButtons
	For DAB = 1 To 64
		Button DAB Disabled
	Next
Return

' -------------------------------------- TEST MASTER PRESENT
For v1 = 1 To 9e99
	For v2 = 1 To 20 ' <<< testing for 2 seconds
		If (Slot(0) >= 1 And Slot(0) <= 3) Or Key("CTRL")
			Exit 2
		EndIf
		Wait Seconds 0.1
	Next
	Print " Invalid master number: " + Slot(0)
	Slot 0 = Recognize
	Beep
Next
Print ""

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

' ╔════════════════════════════════════════════╗ █▀▀▀ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ STOP EVENT (when pressing stop button)     ║ ▀▀▀█   █   █  █ █▄▄█
' ╚════════════════════════════════════════════╝ ▄▄▄█   █   █▄▄█ █
Label EventStop
	StopCobot
	Slot(Slot_SignalScope) = 0
End

Label GotoSafePosition
	SendCommand(Slot_CommandsToCobot)("GotoHoming")
Return

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL      (when pressing X on form)   ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	StopCobot
	Slot(Slot_SignalScope) = 0
	Load CloseAll
End

' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
Label SendCommand			'(Destination Slot)(Command String)
	Variable Numeric SC1
	Variable String  SC2
	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 to Cobot
' =============================================================
Label ExecCobot
	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

' ╔════════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A             ║ ▀▀▀█ █  █ █▀▀█
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█
Label RecognizeEncoder
	Slot(0) = Recognize
Return

Label Motors_Enable
	'Slot(Slot_SignScope) = 1000
	
	If Slot(Slot_MotorsOff) = 1000
		Button Motors_Enable Text "Motors ON"
		Slot(Slot_MotorsOff) = 0
		Button Motors_Enable Color Flashing Lime LightGreen
		Wait Seconds 0.1
	Else
		Button Motors_Enable Text "Motors OFF"
		Slot(Slot_MotorsOff) = 1000
		Button Motors_Enable Color Flashing Red  Red4
	EndIf
Return

Label EnableMotors
	Slot(Slot_MotorsOff) = 0
Return

' ╔════════════════════════════════════════════╗
' ║  A R B I T R A R Y                         ║
' ╚════════════════════════════════════════════╝
Label M1_Arbitrary
	v1 = Input "Insert Destination value (max 2100) in dec of mm: "
	Print v1
	Slot(Slot_M1_InOutDesti) = V1
Return

Label M2_Arbitrary
	v1 = Input "Insert Destination value in degree (max330):"
	Slot(Slot_M2_RotDestina) = V1
Return

Label M3_Arbitrary
	v1 = Input "Insert Destination value (max 110) in degree:"
	Print v1
	Slot(Slot_M3_TipDestina) = V1
Return

Label M4_Arbitrary
	v1 = Input "Insert Destination value (max xxx) in dec of mm:"
	Print v1
	Slot(Slot_M4_ElevDestin) = V1
Return

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

' ╔═════════════════════════╗  █▀▀▀ █▀▀█ █▄ ▄█   █▀▀  █▀▀▀ █▀▀█ █▀█
' ║  COMMANDS FROM COBOT    ║  █    █  █ █ ▀ █   █▀   █    █  █ █▀▀█
' ╚═════════════════════════╝  █▄▄▄ █▄▄█ █   █ ▄ █  ▄ █▄▄▄ █▄▄█ █▄▄█
Label Event_ExternalCommands
	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
			
			
		Case "Beep"
			Beep 880-100
			ClearCommand
		Case "Pause"
			Wait Seconds GetSeparatedString(CommandText, 1)
			ClearCommand
			
		'Case "Speed"
		'	SetMotors_SPEED
		'	ClearCommand
			
		Case "Executing"
			ClearCommand
			
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + CommandText + CRLF
			Beep
			ClearCommand
			Stop								'
	EndSelect
Return

Label ClearCommand
	SlotText(Slot_ExternalCommands) = ""
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

' ╔═══════════╗ █   █ █▀▀█ █ █▀▀▀ █▀▀▀   █▀▀█ █  █ ▀▀█▀▀
' ║ Voice OUT ║  █ █  █  █ █ █    █▀▀    █  █ █  █   █
' ╚═══════════╝   █   █▄▄█ █ █▄▄▄ █▄▄▄   █▄▄█ █▄▄█   █
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 output OFF"
		Button VoiceOutputOnOff Color DarkSlateGray
		TTS SetVolume 0
	Else
		VoiceOutputStatus = "active"
		Button VoiceOutputOnOff Text "Voice output ON"
		Button VoiceOutputOnOff Color Lime
		TTS SetVolume 100
	EndIf
Return



'Label Language
'	If CurrentLanguage = "English"
'		CurrentLanguage = "Chinese"
'		Button Language Text "Chinese"
'		Button Language Color Flashing  Firebrick  Thistle
'		TTS SelectVoice "ZHO"
'		TTS Speak 中文语言
'	Else
'		CurrentLanguage = "Chinese"
'		CurrentLanguage = "English"
'		Button Language Text "English"
'		Button Language Color DodgerBlue
'		TTS SelectVoice "ENG"
'		TTS Speak English Language
'	EndIf
'Return





' ====================================================================
'  HOME  M1 M2 M3 M4
' ====================================================================
Label AllZero
	DisableAllButtons
	M1_Home
	M2_Home
	M3_Home
	M4_Home
	'WaitAllMotors
	EnableAllButtons
	TTS SpeakWait "Homing of all axis is done"
Return

Label M1_Home
	DisableAllButtons
	Button M1_Home Color Flashing8 Yellow1 Yellow5
	TTS Speak "Homing In Out started"
	SpeedSearch = -0.8
	SpeedReturn = +0.1
	DetachFromSensor = +10
	InitialPosition = 100
	HomeMotor(Slot_LimitOut)(Slot_M1_InOutDesti)
	TTS SpeakWait "Homing In Out done"
	Button M1_Home Color Yellow
	EnableAllButtons
Return

Label M2_Home
	DisableAllButtons
	Button M2_Home Color Flashing8 Yellow1 Yellow5
	TTS Speak "Homing of rotating shaft started"
	SpeedSearch = -0.1
	SpeedReturn = +0.05
	DetachFromSensor = +1
	InitialPosition = 5
	HomeMotor(Slot_LimitRot)(Slot_M2_RotDestina)
	Slot(Slot_M2_RotDestina) = 180
	TTS SpeakWait "Rotating Home done"
	Button M2_Home Color Yellow
	EnableAllButtons
Return

Label M3_Home
	DisableAllButtons
	Button M3_Home Color Flashing8 Yellow1 Yellow5
	TTS Speak "Homing of Tip"
	Slot(Slot_M3_TipDestina) = -110
	Gosub WaitMotor(Slot_M3_TipDestina)
	Slot(Slot_M3_TipDestina) = Reset
	Wait Seconds 0.1
	Slot(Slot_M3_TipDestina) = 0
	Wait Seconds 0.1
	TTS SpeakWait "Tip home done"
	Slot(Slot_M3_TipDestina) = 60
	Button M3_Home Color Yellow
	EnableAllButtons
Return

Label M4_Home
	DisableAllButtons
	Button M4_Home Color Flashing8 Yellow1 Yellow5
	TTS Speak "Homing of Elevation"
	SpeedSearch = -0.2
	SpeedReturn = +0.01
	DetachFromSensor = +10
	InitialPosition = 200
	HomeMotor(Slot_LimitElev)(Slot_M4_ElevDestin)
	TTS SpeakWait "Elevation home done"
	Button M4_Home Color Yellow
	EnableAllButtons
Return

' ====================================================================
'  Home the Motor
' ====================================================================
Variable Numeric SpeedSearch
Variable Numeric SpeedReturn
Variable Numeric DetachFromSensor
Variable Numeric InitialPosition

Label HomeMotor
	Variable Numeric SlotLimit
	Variable Numeric SlotDest
	' ------------------------- detach if sensor is active
	If Slot(SlotLimit) > 500 Or Key("CTRL")
		Slot(SlotDest) = Slot(SlotDest) + SpeedReturn
	EndIf
    ' ------------------------- search the sensor
Label SearchHome
	Slot(SlotDest) = Slot(SlotDest) + SpeedSearch
	Wait Seconds 0.001
	If Slot(SlotLimit) > 500 Or Key("CTRL")
		Slot(SlotDest) = Reset
		Wait Seconds 0.1
		Slot(SlotDest) = 0
		Wait Seconds 0.1
		Goto DetachSensor
	EndIf
Goto SearchHome
    ' ------------------------- detach from the sensor
Label DetachSensor
	Slot(SlotDest) = Slot(SlotDest) + SpeedReturn
	Wait Seconds 0.001
	If Slot (SlotLimit) < 500
		Slot(SlotDest) = Reset
		Wait Seconds 0.1
		Slot(SlotDest) = -DetachFromSensor
		Wait Seconds 0.1
		Goto SetFinalValues
	EndIf
Goto DetachSensor
    ' ------------------------- reset and move to InitialPos
Label SetFinalValues
	Slot(SlotDest) = InitialPosition
	Gosub WaitMotor(SlotDest)
Return

' ====================================================================
'   Wait until the motor is stopped
' ====================================================================
Variable Numeric WaitMot1
Label WaitMotor
	Variable Numeric WaitMotor_SlotDest
	Wait Seconds 0.1
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(WaitMotor_SlotDest + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
Return

' ╔════════════════════════════════════════════╗ █   █ █ █▀▀█ ▀▀█▀▀   █▀▀▀ █▄  █ █▀▀▀
' ║  VIRTUAL ENCODER                           ║  █ █  █ ██▀▀   █     █▀▀  █ ▀▄█ █
' ╚════════════════════════════════════════════╝   █   █ █ ▀▄   █  ▄  █▄▄▄ █   █ █▄▄▄ ▄
Label UpdatePositions
	Button M1_Destination Text Format(Slot(Slot_M1_InOutDesti), "0000.00" + "mm")
	Button M1_Position Text Format(Slot(Slot_M1_InOutPosit), "0000.00") + "mm"
	
	Button M2_Destination Text Format(Slot(Slot_M2_RotDestina), "000.00" + "°")
	Button M2_Position Text Format(Slot(Slot_M2_RotPositio), "000.00") + "°"
	
	Button M3_Destination Text Format(Slot(Slot_M3_TipDestina), "000.00" + "°")
	Button M3_Position Text Format(Slot(Slot_M3_TipPositio), "000.00") + "°"
	
	Button M4_Destination Text Format(Slot(Slot_M4_ElevDestin), "0000.00" + "mm")
	Button M4_Position Text Format(Slot(Slot_M4_ElevPositi), "0000.0") + "mm"
Return

' ====================================================================
'   Wait until all motors are stopped
' ====================================================================
Label WaitAllMotors
	Wait Seconds 0.05
	Variable Numeric WM1
	For WM1 = 1 To 9e99
		If Abs(Slot(Slot_M1_InOutPosit)) < 0.01
			If Abs(Slot(Slot_M2_RotPositio)) < 0.01
				If Abs(Slot(Slot_M3_TipPositio)) < 0.01
					If Abs(Slot(Slot_M4_ElevPositi)) < 0.01
						Exit
					EndIf
				EndIf
			EndIf
		EndIf
		Wait Seconds 0.001
	Next
Return

' ===================================================================
'  PRINT COORDINATES
' ===================================================================
Label PrintValues
'	s1 =       "X=" + Format(Slot(Slot_X), "0.000")
'	s1 = s1 + " Y=" + Format(Slot(Slot_Y), "0.000")
'	s1 = s1 + " Z=" + Format(Slot(Slot_Z), "0.000")
	s1 = s1 + "   "
'	s1 = s1 + " M0err=" + Format(Slot(Slot_CounterReaderOutM0) - Slot(Slot_M0), "0.000")
'	s1 = s1 + " M1err=" + Format(Slot(Slot_CounterReaderOutM1) - Slot(Slot_M1), "0.000")
'	s1 = s1 + " M2err=" + Format(Slot(Slot_CounterReaderOutM2) - Slot(Slot_M2), "0.000")
	
	Print CRLF + s1
Return

'===================================================================
'  JoyPad
' ===================================================================
Variable Numeric JoyPad_SpeedMult
Variable Numeric JoyPad_X0
Variable Numeric JoyPad_Y0
Variable Numeric JoyPad_T0

Variable Numeric JoyPad_V1
Variable Numeric JoyPad_V2
Variable Numeric JoyPad_V3
Variable String JoyPad_Status = "OFF"

' ---------------------------------------- Software limits
Variable Numeric Ymax = 2145
Variable Numeric Ymin = 5
Variable Numeric Xmax = 325
Variable Numeric Xmin = 5
Variable Numeric Tmax = 105
Variable Numeric Tmin = 5
Variable Numeric Emax = 300
Variable Numeric Emin = 5

' --------------------------------------- JoyPad Start/Stop
Label JoyPad_Enable
	If JoyPad_Status = "ON"
		JoyPad_Status = "OFF"
		Button JoyPad_Enable Text "JoyPad OFF"
		Button JoyPad_Enable Color Flashing Red  Red4
	Else
		JoyPad_Status = "ON"
		Button JoyPad_Enable Text "JoyPad ON"
		Button JoyPad_Enable Color Green9
	EndIf
Return

Label CheckStatusJoyPad
	If Slot(Slot_JoyBtn12) > 500
		JoyPad_Enable
		Wait Slot(Slot_JoyBtn12) < 500
	EndIf
Return

Label JoyPad
	' --------------------------------------- JoyPad Initializations
	JoyPad_X0 = Slot(Slot_JoyPadX)
	JoyPad_Y0 = Slot(Slot_JoyPadY)
	JoyPad_T0 = Slot(Slot_JoyPadT)
	' --------------------------------------- JoyPad Zero-Test
	If JoyPad_X0 > 550 Or JoyPad_X0 < 450
		Print "JoyPad Error : X = " + Int(JoyPad_X0)
		JoyPad_Status = "OFF"
	EndIf
	If JoyPad_Y0 > 550 Or JoyPad_Y0 < 450
		Print "JoyPad Error : Y = " + Int(JoyPad_Y0)
		JoyPad_Status = "OFF"
	EndIf
	If JoyPad_T0 > 550 Or JoyPad_T0 < 450
		Print "JoyPad Error : T = " + Int(JoyPad_T0)
		JoyPad_Status = "OFF"
	EndIf
	' --------------------------------------- JoyPad Execution
	For JoyPad_V1 = 1 To Infinity
		If JoyPad_Status = "OFF"
			Exit
		EndIf
		' -----------------------------------
		Wait Seconds 0.01
		
		' ----------------------------------- Logaritmic speed adjustment
		JoyPad_SpeedMult = Slot(Slot_JoyPadSpeed) / 1000 - 0.5
		JoyPad_SpeedMult = Pow(10, JoyPad_SpeedMult)
		
        ' ----------------------------------- JoyPad Axis y
		JoyPad_V2 = Slot(Slot_JoyPadY) - JoyPad_Y0
		JoyPad_V3 = Abs(JoyPad_V2) - JoyPad_DeadBand
		If JoyPad_V3 > 0
			JoyPad_V3 = JoyPad_V3 * JoyPad_Speed * JoyPad_SpeedMult * 0.00005
			Slot(Slot_M1_InOutDesti) = Limit(Slot(Slot_M1_InOutDesti) - JoyPad_V3 * Sign(JoyPad_V2), Ymin, Ymax)
		EndIf
		' ----------------------------------- JoyPad Axis X
		JoyPad_V2 = Slot(Slot_JoyPadX) - JoyPad_X0
		JoyPad_V3 = Abs(JoyPad_V2) - JoyPad_DeadBand
		If JoyPad_V3 > 0
			JoyPad_V3 = JoyPad_V3 * JoyPad_Speed * JoyPad_SpeedMult * 0.0001
			Slot(Slot_M2_RotDestina) = Limit(Slot(Slot_M2_RotDestina) + JoyPad_V3 * Sign(JoyPad_V2), Xmin, Xmax)
		EndIf
		' ----------------------------------- JoyPad Axis T
		JoyPad_V2 = Slot(Slot_JoyPadT) - JoyPad_T0
		JoyPad_V3 = Abs(JoyPad_V2) - JoyPad_DeadBand
		If JoyPad_V3 > 0
			JoyPad_V3 = JoyPad_V3 * JoyPad_Speed * JoyPad_SpeedMult * 0.00001
			Slot(Slot_M3_TipDestina) = Limit(Slot(Slot_M3_TipDestina) + JoyPad_V3 * Sign(JoyPad_V2), Tmin, Tmax)
		EndIf
		
        ' ----------------------------------- JoyPad Axis E
		Select Slot(Slot_JoyPad8W)
			Case 360
				Slot(Slot_M4_ElevDestin) = Limit(Slot(Slot_JoyPad8W) + JoyPad_Speed * JoyPad_SpeedMult * 0.001, Emin, Emax)
				
			Case 180
				Slot(Slot_M4_ElevDestin) = Limit(Slot(Slot_JoyPad8W) - JoyPad_Speed * JoyPad_SpeedMult * 0.001, Emin, Emax)
		EndSelect
	Next
Return


' ----------------------------------- JoyPad Open/Close coordinate text box
'Variable Numeric TextBoxIsOpened
'	If Slot(Slot_JoyPadTexBox) > 500
'If TextBoxIsOpened = 0
'Controls OpenTextBox
'TextBoxIsOpened = 1
'Else
'Controls CloseTextBox
'TextBoxIsOpened = 0
'EndIf
'	Wait Slot(Slot_JoyPadTexBox) < 500
'EndIf
		' ----------------------------------- JoyPad Sve coordinate on a file
'If Slot(Slot_JoyPadSaveCoord) > 500
'			s1 =        "X  = " + Format(Slot(Slot_X), "0.00")
'			s1 = s1 + "  Y  = " + Format(Slot(Slot_Y), "0.00")
'			s1 = s1 + "  Z  = " + Format(Slot(Slot_Z), "0.00")
'			Save StringToFile s1 FileName
'Wait Slot(Slot_JoyPadSaveCoord) < 500
'EndIf
'
'' ----------------------------------- JoyPad White Front button
'If Slot(Slot_JoyPadFrontWhite) > 500
'	Slot(Slot_WhiteFront) = 1000 - Slot(Slot_WhiteFront)
'	Wait Slot(Slot_JoyPadFrontWhite) < 500
'EndIf

' ------------------------- detach from the sensor
		' ----------------------------------- JoyPad Dispenser Button
'		Slot(Slot_Dispenser) = Slot(Slot_JoyPadDispenser)

	' --------------------------------------- JoyPad Finalizations
'Button JoyPad Text JoyPad
'Button JoyPad Color Orange
'	Gosub WhiteFrontOFF
'	Gosub BluOff


'================================ VIDEO
Label StartVideo
	Slot(Slot_StartStopVideo) = 1000
Return

Label StopVideo
	Slot(Slot_StartStopVideo) = 0
Return

Label SaveVideo
	Slot(Slot_SaveVideo) = 1000
	Wait Seconds 0.5
	Slot(Slot_SaveVideo) = 0
Return

Label TakeImage
	Wait Seconds 1.5
	Slot(Slot_TakeImage) = 1000
	Wait Seconds 0.2
	Slot(Slot_TakeImage) = 0
Return
' ╔════════════════════════════════════════════╗ ▀▀█▀▀ █▀▀█ █ █▀▀█ █
' ║ T R I A L S                                ║   █   █▄▄▀ █ █▄▄█ █
' ╚════════════════════════════════════════════╝   █   █ ▀▄ █ █  █ █▄▄


'BOOKMARKS 

