' ====================================================================
'     Name:	WXM_DigiClamp_7.txt                   Date: 26 August 2020
'    Autor: Leonardo DE PALO Ph.D.
'    Scope: Digital gripper
'
' Hardware: Standard Theremino Master and Theremino Current Sensor
'
' ====================================================================

Load images\DigiClamp.jpg
'Load sound\gong.wav
Option Speed 9

' -------------------------------------- Options
JoyPad_DeadBand = 25
JoyPad_Speed = 100
KeyJog_Speed = 100

' -------------------------------------- Software limits
RotMax =  350
RotMin =   10
ClampMax =  900
ClampMin =  450

' -------------------------------------- Slots
Slot_ClampMotor = 1
Slot_RotationMotor = 3
Slot_EnableMotor = 5
Slot_ClampPushButton = 6
Slot_RoboClampCommand = 13

Slot_JoyPadClamp = 206
Slot_JoyPadRot = 203
Slot_JoyPadStartStop = 220
Slot_JoyPadSpeed = 202
Slot_JoyPadHome6 = 215
Slot_JoyPadTexBox = 212
Slot_JoyPadSaveCoord = 213
Slot_JoyPadRed = 217
Slot_JoyPadGreen = 219
Slot_JoyPadBlu = 221

' -------------------------------------- BUTTONS
Button  1 Text  CLAMP
Button  1 Color Yellow
Button  1 Slot  Slot_ClampPushButton

Button  2 Text  "PRESSURE "
Button  2 Color PaleGoldenrod

Button  3 Text Home (R)
Button  3 Color LightCyan

Button  4 Text Home (R)
Button  4 Color LightCyan

Button  4 Text Demo
Button  4 Color LightCyan

Button 13 Text  JoyPad
Button 13 Color Orange
Button 13 Slot  Slot_JoyPadStartStop

Button 14 Text  CLOSE ALL
Button 14 Color Red

Gosub DisableAllButtons

' -------------------------------------- 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 EXTERNAL PROGRAMS
Load "Theremino_HAL\Theremino_HAL.exe"
'Load "Microvideo\Theremino_MicroVideo.exe"
Load "Theremino_HAL\Theremino_SlotViewer.exe"
Load "RoboClamp\RoboClamp.exe"
Load "Theremino_GamePad\Theremino_GamePad\Theremino_GamePad.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
	Beep
Next
Print ""

Gosub EnableAllButtons
Wait Seconds 0.5
Load Hide

'Goto LoopForever

Stop																'

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_ClampMotor
Variable Numeric Slot_RotationMotor
Variable Numeric Slot_EnableMotor
Variable Numeric Slot_ClampPushButton
Variable Numeric Slot_RoboClampCommand

Variable Numeric RotMax
Variable Numeric RotMin
Variable Numeric ClampMax
Variable Numeric ClampMin

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

' ===================================================================
'  HOMING
' ===================================================================
Label Home (R)
	Gosub EnableMotor
	Slot Slot_RotationMotor = Slot(Slot_RotationMotor) - 400
	' --------------------------------------------------------
	Gosub WaitRotationMotor
	' --------------------------------------------------------
	Slot(Slot_RotationMotor) = Reset
	Wait Seconds 0.1
	Slot(Slot_RotationMotor) = 5
	Wait Seconds 0.1
	' --------------------------------------------------------
	'Gosub PrintValues
	Gosub DisableMotor
Return

Label Demo
	Gosub EnableMotor
	
	' --------------------------------------------------------
	Slot Slot_RotationMotor = Slot(Slot_RotationMotor) - 20
	' --------------------------------------------------------
	Gosub WaitRotationMotor
	' --------------------------------------------------------
	Slot(Slot_RotationMotor) = Reset
	Wait Seconds 0.1
	Slot(Slot_RotationMotor) = 5
	Wait Seconds 0.1
	' --------------------------------------------------------
	
	
	Slot(Slot_RotationMotor) = 90
	Wait Seconds 1
	Slot(Slot_RotationMotor) = 180
	Wait Seconds 1
	Slot(Slot_RotationMotor) = 270
	
	Wait Seconds 2
	
	Slot(Slot_RotationMotor) = 10
	
	
	
'For v9 = 1 To 3
	'	Slot Slot_RotationMotor = 835
 '	'Gosub WaitAllMotors
	'	Wait Seconds 1
	'	Slot Slot_RotationMotor = 900
	'	Wait Seconds 1
	'Next
	'
	Gosub DisableMotor
Return


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

' ====================================================================
'  FUNCTIONS
' ====================================================================
Label CLAMP
	Slot Slot_RoboClampCommand = 1000
	Button 1 Text RELEASE
	Button 1 Color Flashing7 Orange Yellow
Return

Label RELEASE
	Slot Slot_RoboClampCommand = 0
	Button 1 Color Yellow
	Button 1 Text CLAMP
Return

'Label LoopForever
'	v1 = Slot(2)
'	v1 = Int(v1)
'	Button 2 Text "Pressure=" + v1
'	Wait Seconds 0.01
'Goto LoopForever

Label EnableMotor
	Slot_EnableMotor = 1000
Return

Label DisableMotor
	Slot_EnableMotor = 0
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_ClampMotor = Limit(Slot(Slot_ClampMotor) + JogSpeed, ClampMin, ClampMax)
	EndIf
	If Key("Left")
		Slot Slot_ClampMotor = Limit(Slot(Slot_ClampMotor) - JogSpeed, ClampMin, ClampMax)
	EndIf
	'
	If Key("Up")								 'Breakpoint
		Slot Slot_RotationMotor = Limit(Slot(Slot_RotationMotor) + JogSpeed, RotMin, RotMax)
	EndIf
	If Key("Down")
		Slot Slot_RotationMotor = Limit(Slot(Slot_RotationMotor) - JogSpeed, RotMin, RotMax)
	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


' ====================================================================
'  JoyPad
' ====================================================================
Variable Numeric Slot_JoyPadClamp
Variable Numeric Slot_JoyPadRot
Variable Numeric Slot_JoyPadStartStop
Variable Numeric Slot_JoyPadSpeed
Variable Numeric Slot_JoyPadTexBox
Variable Numeric Slot_JoyPadSaveCoord
Variable Numeric Slot_JoyPadRed
Variable Numeric Slot_JoyPadGreen
Variable Numeric Slot_JoyPadBlu
Variable Numeric Slot_JoyPadHome6

Variable Numeric JoyPad_DeadBand
Variable Numeric JoyPad_Speed
Variable Numeric JoyPad_SpeedMult

Variable Numeric JoyPad_Rot0
Variable Numeric JoyPad_V1
Variable Numeric JoyPad_V2
Variable Numeric JoyPad_V3
Variable Numeric JoyPad_Running

Label JoyPad
	'Load images\JOYPAD_MEANING_OF_BUTTONS.jpg
	' --------------------------------------- JoyPad Start/Stop
	If JoyPad_Running = 1
		JoyPad_Running = 0
		'Gosub WhiteFrontOn
		'Gosub RgbOff
		Load Hide
		Return
	Else
		JoyPad_Running = 1
		'Gosub WhiteOn
	EndIf
	' --------------------------------------- JoyPad Initializations
	Button 13 Color Green9
	Button 13 Text JoyPad RUNNING
	JoyPad_Rot0 = Slot(Slot_JoyPadRot)
	' --------------------------------------- JoyPad Zero-Test
	''If Slot(Slot_RotationMotor) > 550 Or Slot(Slot_RotationMotor) < 450
	''	Print "JoyPad Error not center : Slot_RotationMotor = " + Int(Slot(Slot_RotationMotor))
	''	JoyPad_Running = 0
	''EndIf
	' --------------------------------------- JoyPad Execution
	For JoyPad_V1 = 1 To 9e99
		
		' -----------------------------------
		Wait Seconds 0.01
		If JoyPad_Running = 0
			Exit
		EndIf
		' ----------------------------------- Logaritmic speed adjustment
		JoyPad_SpeedMult = Slot(Slot_JoyPadSpeed) / 1000 - 0.5
		JoyPad_SpeedMult = Pow(10, JoyPad_SpeedMult)
		' ----------------------------------- JoyPad Rotation
		JoyPad_V2 = Slot(Slot_JoyPadRot) - JoyPad_Rot0
		JoyPad_V3 = Abs(JoyPad_V2) - JoyPad_DeadBand
		If JoyPad_V3 > 0
			JoyPad_V3 = JoyPad_V3 * JoyPad_Speed * JoyPad_SpeedMult * 0.00001  '0.00001
			Slot Slot_RotationMotor = Limit(Slot(Slot_RotationMotor) + JoyPad_V3 * Sign(JoyPad_V2), RotMin, RotMax)
			Gosub PrintValues
		EndIf
		' ----------------------------------- JoyPad Clamp
		Select Slot(Slot_JoyPadClamp)
			Case 270
				Slot Slot_ClampMotor = Limit(Slot(Slot_ClampMotor) + JoyPad_Speed * JoyPad_SpeedMult * 0.01, ClampMin, ClampMax)
				Gosub PrintValues
			Case 90
				Slot Slot_ClampMotor = Limit(Slot(Slot_ClampMotor) - JoyPad_Speed * JoyPad_SpeedMult * 0.01, ClampMin, ClampMax)
				Gosub PrintValues
		EndSelect
		
		' ----------------------------------- 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 Save coordinate on a file
		If Slot(Slot_JoyPadSaveCoord) > 500
			's1 =        "X  = " + Format(Slot(Slot_JoyPadRot), "0.00")
			'Save StringToFile s1 FileName
			'Wait Slot(Slot_JoyPadSaveCoord) < 500
		EndIf
		'
		' ----------------------------------- JoyPad Red button
		If Slot(Slot_JoyPadRed) > 500
			'Slot(Slot_LedR) = 1000 - Slot(Slot_LedR)
			Wait Slot(Slot_JoyPadRed) < 500
		EndIf
		' ----------------------------------- JoyPad Green button
		If Slot(Slot_JoyPadGreen) > 500
			'Slot(Slot_LedG) = 1000 - Slot(Slot_LedG)
			Wait Slot(Slot_JoyPadGreen) < 500
		EndIf
		' ----------------------------------- JoyPad Blu button
		If Slot(Slot_JoyPadBlu) > 500
			'Slot(Slot_LedB) = 1000 - Slot(Slot_LedB)
			Wait Slot(Slot_JoyPadBlu) < 500
		EndIf
		' ----------------------------------- JoyPad Home button
		If Slot(Slot_JoyPadHome6) > 500
			Wait Slot(Slot_JoyPadHome6) < 500
			Gosub Home (R)
		EndIf
		' ----------------------------------- JoyPad Dispenser Button
		'Slot(Slot_Dispenser) = Slot(Slot_JoyPadDispenser)
	Next
	' --------------------------------------- JoyPad Finalizations
	Button 13 Text JoyPad
	Button 13 Color Orange
Return

' ====================================================================
'  PRINT COORDINATES
' ====================================================================
Label PrintValues
	s1 =       "Rotation=" + Format(Slot(Slot_RotationMotor), "0.000")
	s1 = s1 + " Clamp=" + Format(Slot(Slot_ClampMotor), "0.000")
	Print CRLF + s1
Return


'WATCH SlotMotor
'WATCH Abs(Slot(SlotMotor))
'WATCH v1
'WATCH Load
'EXEC Sin2 = Sin(33)
