'╔═══════════════════════════════════════════════════════════╗
'║     Name: T_JZERA_arm_Master_10.txt     Date: 20 Jul 2024 ║
'║   Author: Theremino Team / Leonardo                       ║
'║    Scope: Test stepper motor sequences and cloche         ║
'╠═══════════════════════════════════════════════════════════╣
'║Hardware: MAster V5                                        ║
'╟───────────────────────────────────────────────────────────╢
'║  M1 - LEFT ARM                                            ║
'║  Stepper Driver: SL2236A   25:1                           ║
'║   1   2   3   4     microstep                             ║
'║  ON  ON  OFF OFF    400 Step/run    step/mm = 28          ║
'╟───────────────────────────────────────────────────────────╢
'║  M2 - RIGHT ARM                                           ║
'║  Stepper Driver: SL2236A   25:1                           ║
'║   1   2   3   4     microstep                             ║
'║  ON  ON  OFF OFF    400 Step/run    step/mm = 28          ║
'╟───────────────────────────────────────────────────────────╢
'║  M3 - HEAD                                                ║
'║  Stepper Driver: SL2236A   25:1                           ║
'║   1   2   3   4     microstep                             ║
'║   x   x   x   xF    xx Step/run     step/mm = xx          ║
'╟───────────────────────────────────────────────────────────╢
'║ Power: 12 V                                               ║
'║   5   6   7   8                                           ║
'║  ON  ON  ON  OFF = 1 Amper                                ║
'╟───────────────────────────────────────────────────────────╢
'║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\T_JZERA_ARM_Master.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_ResponsesFromQRDecode = 54

' ╔════════════════════════════════════════════╗ █▀▀▀ █   █▀▀█ ▀▀█▀▀
' ║ SLOT INITIALIZATIONS                       ║ ▀▀▀█ █   █  █   █
' ╚════════════════════════════════════════════╝ ▄▄▄█ █▄▄ █▄▄█   █
Variable Numeric Slot_M1_Destinat  =   101
Variable Numeric Slot_M2_Destinat  =   201
'Variable Numeric Slot_M3_Destinat  =   301

Variable Numeric Slot_Limit_M1     =   103
Variable Numeric Slot_Limit_M2     =   203
'Variable Numeric Slot_Limit_M3     =   303

Variable Numeric Enable_Motor     =  10

Variable Numeric Slot_SignalScope =  13

' ╔═══════════════════════════╗ █▀▀▄ █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ BUTTONS                   ║ █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚═══════════════════════════╝ █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
Button 1 Label EnableMotorBTN
Button EnableMotorBTN Text "Motor OFF"
Button EnableMotorBTN Color Flashing Red  Red4

Button 2 Label HomeAll
Button HomeAll Text "Home All"
Button HomeAll Color Yellow

Button 3 Label M1_Home
Button M1_Home Text "M1 Home"
Button M1_Home Color Yellow

Button 4 Label M2_Home
Button M2_Home Text "M2 Home"
Button M2_Home Color Yellow

Button 5 Label M3_Home
Button M3_Home Text "M3 Home"
Button M3_Home Color Yellow

Button 6 Label ArbitraryLeft
Button ArbitraryLeft Text "Arbitrary Left"
Button ArbitraryLeft Color Cyan5

Button 7 Label ArbitraryRight
Button ArbitraryRight Text "Arbitrary Right"
Button ArbitraryRight Color Cyan5

Button 8 Label ArbitraryHead
Button ArbitraryHead Text "Arbitrary Head"
Button ArbitraryHead Color Cyan5

Button 9 Label Waddled
Button Waddled Text "Waddled"
Button Waddled Color LightSteelBlue

Button 11 Label Sequence
Button Sequence Text "Sequence"
Button Sequence Color MediumBlue

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

Button 13 Label Say
Button Say Text "Say"
Button Say Color DeepSkyBlue

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

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

Load Theremino_SignalScope.exe

Load Theremino_Cobot.exe
'WaitApplicationsStarted
Load Hide										 'Breakpoint

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

Slot(Slot_SignalScope) = 1000

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

'Controls OpenTextBox
'Controls ClearTextBox

Slot(Enable_Motor) = 1000
EnableMotorBTN

HomeAll

Loop

Stop

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

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


' ╔════════════════════════════════════════════╗
' ║  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
'	GoToShow
'	StopCobotAndGotoHoming
	DisableMotor
End

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

' ╔════════════════════════════════════════════╗ █▀▀ █   █▀▀█ █▀▀▀ █▀▀▀
' ║ CLOSE ALL      (when pressing X on form)   ║ █   █   █  █ ▀▀▀█ █▀▀
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄ █▄▄█ ▄▄▄█ █▄▄▄
Label CloseAll
	StopCobot
	Slot(Slot_SignalScope) = 0
'	GoToShow
'	StopCobotAndGotoHoming
	DisableMotor
	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 EnableMotorBTN
	'Slot(Slot_SignScope) = 1000
	
	If Slot(Enable_Motor) = 1000
		Slot(Enable_Motor) = 0
		Button EnableMotorBTN Text "Motor ON"
		Button EnableMotorBTN Color Flashing Lime LightGreen
		Wait Seconds 0.1
	Else
		Slot(Enable_Motor) = 1000
		Button EnableMotorBTN Text "Motor OFF"
		Button EnableMotorBTN Color Flashing Red  Red4
	EndIf
Return

Label EnableMotor
	Slot(Enable_Motor) = 0
Return

Label DisableMotor
	Slot(Enable_Motor) = 1000
Return

Variable Numeric StopOnlimit
Label GoTo180
	For StopOnlimit = 1 To 180
		If Slot(Slot_Limit_M1) < 500
			Slot(Slot_M1_Destinat) = Slot(Slot_M1_Destinat) + 1
			Wait Seconds 0.01
		Else
			Wait Seconds 0.1
			Slot(Slot_M1_Destinat) = Reset
			Wait Seconds 0.1
			Slot(Slot_M1_Destinat) = 0
			Wait Seconds 0.1
			Slot(Slot_M1_Destinat) = Slot(Slot_M1_Destinat) - 5
			Return
		EndIf
	Next
Return

Variable Numeric Waddling
Label Waddled
	M1_Home
	For Waddling = 1 To 10
		Slot(Slot_M1_Destinat) = 0
		WaitMotor(Slot_M1_Destinat)
		Slot(Slot_M2_Destinat) = 0
		WaitMotor(Slot_M2_Destinat)
		
		Slot(Slot_M1_Destinat) = 50
		WaitMotor(Slot_M1_Destinat)
		Slot(Slot_M2_Destinat) = 50
		WaitMotor(Slot_M2_Destinat)
	Next
	Slot(Slot_M1_Destinat) = 22
	Slot(Slot_M2_Destinat) = 26
Return


' -------------------------------------- ARBITRARY LEFT
Variable Numeric AL1
Variable Numeric AL2
Variable Numeric AL3
Label ArbitraryLeft
	AL1 = Input "Insert Destination value in degree:" + M1_Min + "..." + M1_Max
	AL1 = Limit(AL1, M1_Min, M1_Max)
	If AL1 > Slot(Slot_M1_Destinat)
		AL2 = M1_Max
		AL3 = M1_Max - 5
	Else
		AL2 = M1_Min
		AL3 = M1_Min + 5
	EndIf
	Slot(Slot_M1_Destinat) = AL1
	Wait Seconds 0.1
	For StopOnlimit = Slot(Slot_M1_Destinat) To Infinity
		If Slot(Slot_Limit_M1) > 500
			Wait Seconds 0.2
			Slot(Slot_M1_Destinat) = Reset
			Wait Seconds 0.2
			Slot(Slot_M1_Destinat) = AL2
			Wait Seconds 0.2
			Slot(Slot_M1_Destinat) = AL3
			WaitAllMotors
		EndIf
		If Abs(Slot(Slot_M1_Destinat + 1)) < 0.01
			Exit
		EndIf
	Next
Return

' -------------------------------------- ARBITRARY RIGHT
Variable Numeric AR1
Variable Numeric AR2
Variable Numeric AR3
Label ArbitraryRight
	AR1 = Input "Insert Destination value in degree:" + M2_Min + "..." + M2_Max
	AR1 = Limit(AR1, M2_Min, M2_Max)
	If AR1 > Slot(Slot_M2_Destinat)
		AR2 = M2_Max
		AR3 = M2_Max - 5
	Else
		AR2 = M2_Min
		AR3 = M2_Min + 5
	EndIf
	Slot(Slot_M2_Destinat) = AR1
	Wait Seconds 0.1
	For StopOnlimit = Slot(Slot_M2_Destinat) To Infinity
		If Slot(Slot_Limit_M2) > 500
			Wait Seconds 0.2
			Slot(Slot_M2_Destinat) = Reset
			Wait Seconds 0.2
			Slot(Slot_M2_Destinat) = AR2
			Wait Seconds 0.2
			Slot(Slot_M2_Destinat) = AR3
			WaitAllMotors
		EndIf
		If Abs(Slot(Slot_M2_Destinat + 1)) < 0.01
			Exit
		EndIf
	Next
Return

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

' ╔═════════════════════════╗  █▀▀▀ █▀▀█ █▄ ▄█   █▀▀  █▀▀▀ █▀▀█ █▀█
' ║  COMMANDS FROM COBOT    ║  █    █  █ █ ▀ █   █▀   █    █  █ █▀▀█
' ╚═════════════════════════╝  █▄▄▄ █▄▄█ █   █ ▄ █  ▄ █▄▄▄ █▄▄█ █▄▄█
Label Event_ExternalCommands
	' ------------------------------------------------------ Do not print lines containing ''
	'Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select CommandText
			' ---------------------------------------------- ACTIONS
		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


' ╔════════════════════════════════════════╗ █▄ ▄█ █▀▀█ ▀▀█▀▀ █▀▀█ █▀▀█
' ║ Motor                                  ║ █ ▀ █ █  █   █   █  █ █▄▄▀
' ╚════════════════════════════════════════╝ █   █ █▄▄█   █   █▄▄█ █ ▀▄

Variable Numeric M1_Min = 002
Variable Numeric M1_Max = 135

Variable Numeric M2_Min = 002
Variable Numeric M2_Max = 140

'Variable Numeric M3_Min = 002
'Variable Numeric M3_Max = 180

' ====================================================================
'  HOME  M1 - M2 - M3
' ====================================================================
Label HomeAll
	M1_Home
	M2_Home
	'M3_Home
Return

Label M1_Home
	DisableAllButtons
	EnableMotor
	Button M1_Home Color Flashing8 Yellow1 Yellow5
	'TTS	Speak "Home M1"
	SpeedSearch = -0.1
	SpeedReturn = +0.01
	DetachFromSensor = +1
	InitialPosition = +20
	HomeMotor(Slot_Limit_M1)(Slot_M1_Destinat)
	'TTS Speak "Home M1 done"
	Button M1_Home Color Yellow
	EnableAllButtons
Return

Label M2_Home
	DisableAllButtons
	EnableMotor
	Button M2_Home Color Flashing8 Yellow1 Yellow5
	'TTS	Speak "Home M1"
	SpeedSearch = -0.1
	SpeedReturn = +0.01
	DetachFromSensor = +1
	InitialPosition = +20
	HomeMotor(Slot_Limit_M2)(Slot_M2_Destinat)
	'TTS Speak "Home M1 done"
	Button M2_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(SlotMotor) is stopped
' ====================================================================
Variable Numeric WaitMot1
Label WaitMotor
	Wait Seconds 0.1
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(Slot_M1_Destinat + 1)) < 0.01
			Exit
		EndIf
		Wait Seconds 0.005
	Next
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_Destinat + 1)) < 0.01
			If Abs(Slot(Slot_M2_Destinat + 1)) < 0.01
				'If Abs(Slot(Slot_M3_Destinat + 1)) < 0.01
				Exit
			    'EndIf
			EndIf
		EndIf
		Wait Seconds 0.001
	Next
Return


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


'BOOKMARKS 378 175 

'EDITEDLINES 344 6 501 3 508 3 518 3 550 4 408 5 375 6 68 7 137 8 312 9 58 10 

