' ====================================================================
'     Name:	WXM_2DigiClamp_JAKA_2.txt                Date: 26 Jan 202
'    Autor: Leonardo DE PALO Ph.D.

'    Scope:Manage two DigiClamp
'
' Hardware: Standard Theremino Master and two Current Sensor
'    Motor: FeeTech FT846BL
'
' ====================================================================

'Load images\DigiClampUR.jpg
'Load sound\gong.wav
Option Speed 9
Gosub Release_1
Gosub Release_2

' -------------------------------------- Software limits
ClampMaxM1 =  840
ClampMinM1 =  360
ClampMaxM2 =  840
ClampMinM2 =  360

' -------------------------------------- Slots
Slot_DigiClampM1      = 101
Slot_ClampM1Command   = 103
Slot_DigiClampM2      = 201
Slot_ClampM2Command   = 203

Slot_Red   = 7
Slot_Green = 8
Slot_Blue  = 9

' -------------------------------------- BUTTONS
Button  1 Text  CLAMP_1
Button  1 Color Yellow

Button  4 Text  SelfClamp
Button  4 Color LightCyan
Button  7 Text  WHITE
Button  7 Color White
Button  8 Text  RED
Button  8 Color Red
Button  9 Text  GREEN
Button  9 Color Green
Button 10 Text  BLUE
Button 10 Color Blue
Button 11 Text  BLACK
Button 11 Color Black

Button 14 Text  CLOSE ALL
Button 14 Color Red

Button 17 Text  CLAMP_2
Button 17 Color Yellow

Gosub DisableAllButtons

' -------------------------------------- KEYS
KeyJog_Speed = 1000
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 EXTERNAL PROGRAMS
Load "Theremino_HAL\Theremino_HAL.exe"
'Load "Microvideo\Theremino_MicroVideo.exe"
Load "Theremino_HAL\Theremino_SlotViewer.exe"
'Load "Theremino_HAL\Theremino_SignalScope.exe"
Load "DigiClampM1\RoboClamp.exe"
Load "DigiClampM2\RoboClamp.exe"

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

Gosub EnableAllButtons
Wait Seconds 0.5
Load Hide

Stop																'

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_DigiClampM1
Variable Numeric Slot_ClampM1Command
Variable Numeric Slot_DigiClampM2
Variable Numeric Slot_ClampM2Command

Variable Numeric ClampMaxM1
Variable Numeric ClampMinM1
Variable Numeric ClampMaxM2
Variable Numeric ClampMinM2

Variable Numeric Slot_Red
Variable Numeric Slot_Green
Variable Numeric Slot_Blue

' =====================================================================
'  CLOSING EVENTS
' =====================================================================
Label CLOSE ALL
	Load CloseApps
Return

Label EventStop
	Gosub EnableAllButtons
End

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

' ====================================================================
' LED COLORS
' ====================================================================
Label WHITE
	Slot (Slot_Red)   = 1000
	Slot (Slot_Green) = 1000
	Slot (Slot_Blue)  = 1000
Return

Label RED
	Slot (Slot_Red)   = 1000
	Slot (Slot_Green) = 0
	Slot (Slot_Blue)  = 0
Return

Label GREEN
	Slot (Slot_Red)   = 0
	Slot (Slot_Green) = 1000
	Slot (Slot_Blue)  = 0
Return

Label BLUE
	Slot (Slot_Red)   = 0
	Slot (Slot_Green) = 0
	Slot (Slot_Blue)  = 1000
Return

Label BLACK
	Slot (Slot_Red)   = 0
	Slot (Slot_Green) = 0
	Slot (Slot_Blue)  = 0
Return


' ====================================================================
'  SEQUENCES
' ====================================================================
Label SelfClamp
	Button 4 Color Flashing8 Yellow Green
	For v9 = 1 To 2000		'9e99
		SequenceFileName = "SEQUENCES\DemoClamp.seq"
		Gosub ExecSequence
	Next
	Button 4 Color LightCyan
Return

' ====================================================================
'  SEQUENCE LOADING AND EXECUTION
' ====================================================================
Variable String SequenceFileName
Variable String Sequence
Variable String SequenceCmd
Variable Numeric SequenceIndex
Variable Numeric SequenceLength

Label ExecSequence
	Load Sequence SequenceFileName
	If Len(Sequence) = 0
		Controls OpenTextBox
		Print CRLF + " Sequence file not found: " + SequenceFileName
		Beep
		Stop
	EndIf
	' ------------------------------------------
	Variable Numeric ExecSequenceStartTime
	ExecSequenceStartTime = ElapsedTime
	' ------------------------------------------
	SequenceLength = GetSeparatedStringCount(Sequence, CRLF)
	For SequenceIndex = 0 To SequenceLength-1
		SequenceCmd = GetSeparatedString(Sequence, SequenceIndex, CRLF)
		SequenceCmd = Replace(SequenceCmd, Chr(9), " ") 'TABS to SPACES
		SequenceCmd = RemoveComments(SequenceCmd)
		SequenceCmd = Trim(SequenceCmd)
		'
		Print " Executing command: " + SequenceCmd
		'
		Select UCase(SequenceCmd)
            ' ---------------------------------------------- End sequence
			Case "ENDSEQ"
				Exit
		    ' ---------------------------------------------- ACTIONS
			Case "WAITALLMOTORS"
				'Gosub WaitAllMotor
			Case "CLOSE ALL"
				Gosub CLOSE ALL
			Case "Slot_DigiClampM1"
				Gosub CLAMP_1
			Case "RELEASEM1"
				Gosub Release_1
			Case "CLAMPM2"
				Gosub CLAMP_2
			Case "RELEASEM2"
				Gosub Release_2
			' ---------------------------------------------- LED COLORS
			'Case "REDON"
			'	Gosub RedOn
			'Case "GREENON"
			'	Gosub GreenOn
			'Case "BLUON"
			'	Gosub BluON
			'Case "YELLOWON"
			'	Gosub YellowOn
			'Case "WHITEON"
			'	Gosub WhiteOn
			'Case "RGBOFF"
			'	Gosub RgbOff
			'Case "WHITEFRONTON"
			'	Gosub WhiteFrontOn
			'Case "WHITEFRONTOFF"
			'	Gosub WhiteFrontOFF
			' ---------------------------------------------- VIDEO
			'Case "STARTVIDEO"
			'	Gosub StartVideo
			'Case "STOPVIDEO"
			'	Gosub StopVideo
			'Case "SAVEVIDEO"
			'	Gosub SaveVideo
			'Case "TAKEIMAGE"
			'	Gosub TakeImage
            ' ---------------------------------------------- With parameters
			CaseElse
				s1 = GetSeparatedString(SequenceCmd, 0)
				Select UCase(s1)
					Case "PAUSE"
						'Gosub RedOn
						Wait Seconds GetSeparatedString(SequenceCmd, 1)
						'Gosub GreenOn
					Case "DISPENSER"
						'DispenserTime = GetSeparatedString(SequenceCmd, 1)
						'Gosub Dispenser
					CaseElse
						If s1 <> ""
							Controls OpenTextBox
							Print CRLF + " Unrecognized command: " + SequenceCmd
							Beep
							Stop
						EndIf
				EndSelect
		EndSelect
	Next
	SequenceFileName = Replace(SequenceFileName,".seq","")
	Print SequenceFileName + " executed in " + Format(ElapsedTime - ExecSequenceStartTime, "0.00") + " seconds"
Return


' ====================================================================
'  FUNCTIONS
' ====================================================================
Label CLAMP_1
	Slot (Slot_ClampM1Command) = 1000
	Button 1 Text "Release 1"
	Button 1 Color Flashing7 Orange Yellow
Return

Label Release_1
	Slot (Slot_ClampM1Command) = 0
	Button 1 Color Yellow
	Button 1 Text CLAMP_1
Return

Label CLAMP_2
	Slot (Slot_ClampM2Command) = 1000
	Button 17 Text Release_2
	Button 17 Color Flashing7 Orange Yellow
Return

Label Release_2
	Slot (Slot_ClampM2Command) = 0
	Button 17 Color Yellow
	Button 17 Text CLAMP_2
Return

' ====================================================================
'  JOG with keyboard
' ====================================================================
Label ExecKeys
	Variable Numeric KeyJog_Speed
	Variable Numeric JogSpeed
	JogSpeed = KeyJog_Speed / 100
	Select True
		Case Key("SHIFT")
			JogSpeed = JogSpeed * 0.1
		Case Key("CTRL")
			JogSpeed = JogSpeed * 0.01
		Case Key("ALT")
			JogSpeed = JogSpeed * 0.001
	EndSelect
	'
	If Key("Right")
		Slot Slot_DigiClampM1 = Limit(Slot(Slot_DigiClampM1) + JogSpeed, ClampMinM1, ClampMaxM1)
	EndIf
	If Key("Left")
		Slot Slot_DigiClampM1 = Limit(Slot(Slot_DigiClampM1) - JogSpeed, ClampMinM1, ClampMaxM1)
	EndIf
	'
	If Key("Up")
		Slot Slot_DigiClampM2 = Limit(Slot(Slot_DigiClampM2) + JogSpeed, ClampMinM2, ClampMaxM2)
	EndIf
	If Key("Down")
		Slot Slot_DigiClampM2 = Limit(Slot(Slot_DigiClampM2) - JogSpeed, ClampMinM2, ClampMaxM2)
	EndIf
	'
	'If Key("PageUp")
	'	Slot Slot_Z = Limit(Slot(Slot_Z) + JogSpeed, Zmin, Zmax)
	'EndIf
	'If Key("PageDown")
	'	Slot Slot_Z = Limit(Slot(Slot_Z) - JogSpeed, Zmin, Zmax)
	'EndIf
	'
	Gosub PrintValues
Return



' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	s1 = ""
	s1 = s1 + " Clamp 1 = " + Format(Slot(Slot_DigiClampM1), "0.000")
	S1 = S1 + "      "
	s1 = s1 + " Clamp 2 = " + Format(Slot(Slot_DigiClampM2), "0.000")
	Print s1
Return


'WATCH Slot
'WATCH Slot
'EXEC Sin2 = Sin(33)
