' ====================================================================
'     Name:	WXM_DigiClamp_9.txt                   Date: 27 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 =  850
ClampMin =  360

' -------------------------------------- Slots
Slot_ClampMotor = 1
Slot_RotMotor = 3
Slot_EnableMotors = 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  5 Text Sequence
Button  5 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.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_RotMotor
Variable Numeric Slot_EnableMotors
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 EnableMotors
	Slot Slot_RotMotor = Slot(Slot_RotMotor) - 400
	' --------------------------------------------------------
	Gosub WaitRotMotor
	' --------------------------------------------------------
	Slot(Slot_RotMotor) = Reset
	Wait Seconds 0.1
	Slot(Slot_RotMotor) = 5
	Wait Seconds 0.1
	' --------------------------------------------------------
	'Gosub PrintValues
	Gosub DisableMotors
Return

Label Demo
	Gosub EnableMotors
	
	' --------------------------------------------------------
	Slot Slot_RotMotor = Slot(Slot_RotMotor) - 20
	' --------------------------------------------------------
	Gosub WaitRotMotor
	' --------------------------------------------------------
	Slot(Slot_RotMotor) = Reset
	Wait Seconds 0.1
	Slot(Slot_RotMotor) = 5
	Wait Seconds 0.1
	' --------------------------------------------------------
	For v9 = 1 To 300
		Gosub RELEASE
		Slot(Slot_RotMotor) = 90
		Wait Seconds 2
		Gosub CLAMP
		Wait Seconds 2
		Slot(Slot_RotMotor) = 180
		Wait Seconds 1
		Gosub RELEASE
		Wait Seconds 2
		Slot(Slot_RotMotor) = 270
		Wait Seconds 2
		Gosub CLAMP
		Wait Seconds 2
		Slot(Slot_RotMotor) = 10
		Gosub RELEASE
		Wait Seconds 2
	Next
	Gosub DisableMotors
Return


' ====================================================================
'  SEQUENCES
' ====================================================================
Label Sequence
	Button 5 Color Flashing8 Black White
	For v9 = 1 To 2		'9e99
		SequenceFileName = "SEQUENCES\Demo.seq"
		Gosub ExecSequence
	Next
	Button 5 Color LightCyan
Return

Label Stress Test
	Button 6 Color Flashing8 Black White
	Gosub EnableMotors
	Gosub Home(R)
	For v9 = 1 To 50		'9e99
		SequenceFileName = "SEQUENCES\StressTest.seq"
		Gosub ExecSequence
	Next
	Button 6 Color Yellow
Return

Label PARK
	Button 11 Color Flashing8 Yellow Orange
	Gosub EnableMotors
	Gosub Home(R)
	'For v9 = 1 To 2    	'9e99
	SequenceFileName = "SEQUENCES\Park.seq"
	Gosub ExecSequence
	'Next
	Button 11 Color White
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 "ENABLEMOTORS"
				Gosub EnableMotors
			Case "DISABLEMOTORS"
				Gosub DisableMotors
			Case "WAITALLMOTORS"
				'Gosub WaitAllMotor
			Case "CLOSE ALL"
				Gosub CLOSE ALL
			Case "CLAMP"
				Gosub CLAMP
			Case "RELEASE"
				Gosub RELEASE
			' ---------------------------------------------- HOMING
			Case "HOME"
				Gosub Home(R)
			'Case "HOME X"
			'	Gosub Home X
			'Case "HOME Y"
			'	Gosub Home Y
			'Case "HOME Z"
			'	Gosub Home Z
			' ---------------------------------------------- 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 "MOVESLOWZ"
						'MoveSlowZ_Dest = GetSeparatedString(SequenceCmd, 1)
						'MoveSlowZ_Speed = GetSeparatedString(SequenceCmd, 2)
						'Gosub MoveSlowZ
					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

' ====================================================================
'  MOVE SLOW RotationMotor
' ====================================================================
Variable Numeric MSV1
Variable Numeric MSV3
Variable Numeric MSV4
Variable Numeric MoveSlowR_Dest 	' destination in mm
Variable Numeric MoveSlowR_Speed 	' speed in mm/sec

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

' ====================================================================
'   Wait until the rotation motor is stopped
' ====================================================================
Label WaitRotMotor
	Wait Seconds 0.1
	Variable Numeric WaitMot1
	For WaitMot1 = 1 To 9e99
		If Abs(Slot(Slot_RotMotor + 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.1
Goto LoopForever

'Label EventTimer
'	v1 = Slot(2)
'	v1 = Int(v1)
'	Button 2 Text "Pressure=" + v1
'Return

Label EnableMotors
	Slot_EnableMotors = 1000
Return

Label DisableMotors
	Slot_EnableMotors = 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")
		Slot Slot_RotMotor = Limit(Slot(Slot_RotMotor) + JogSpeed, RotMin, RotMax)
	EndIf
	If Key("Down")
		Slot Slot_RotMotor = Limit(Slot(Slot_RotMotor) - 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
	Gosub EnableMotors
	'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_RotMotor) > 550 Or Slot(Slot_RotMotor) < 450
	''	Print "JoyPad Error not center : Slot_RotMotor = " + Int(Slot(Slot_RotMotor))
	''	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_RotMotor = Limit(Slot(Slot_RotMotor) + 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_RotMotor), "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)
