
Option Speed 9

' -------------------------------------- Slots
NumMotors         = 4
Slot_X            = 1
Slot_Y            = 3
Slot_Z            = 5
Slot_A            = 25
Slot_R            = 27

Slot_SensorX      = 19
Slot_SensorY      = 20
Slot_SensorA      = 31
Slot_SensorZ      = 21

Slot_EnableMotors = 11

Slot_Dispenser = -1
Slot_Emergency    = -1

Slot_LedR = 16
Slot_LedG = 17
Slot_LedB = 18

Slot_JoyPadX = 201
Slot_JoyPadY = 202
Slot_JoyPadZ = 207
Slot_JoyPadR = 204
'Slot_JoyPadA = 202
Slot_JoyPadStartStop = 221
Slot_JoyPadDispenser = 211
Slot_JoyPadSpeed = 203
Slot_JoyPadHome6 = 216
Slot_JoyPadTexBox = 213
Slot_JoyPadSaveCoord = 214

Slot_JoyPadFrontWhite = 217
Slot_JoyPadRed = 218
Slot_JoyPadGreen = 220
Slot_JoyPadBlu = 222

Slot_JoyPadPen = 219

Slot_JoyPadTrigger    = 225			' More than 500 to start / Less than 500 to retrigger
Slot_JoyPadDirections = 226 		' 0 = Cartesian  /  1 = Spherical  /  2 = Polar
Slot_JoyPadMagnitude  = 227       	' From 0 to 1000 (negative numbers for negative direction forces)orce effect          Duration    27          Milliseconds (set to zero to retrigger)
Slot_JoyPadDuration   = 228  		' Vibration time in milliseconds

Slot_PEN          = 13

Slot_LASER        = 30
Slot_LockSensor   = 32

' -------------------------------------- OPTIONS
JoyPad_DeadBand  = 25
JoyPad_Speed     = 5000
KeyJog_Speed     = 400

PenDown_Value    = 212
PenUp_Value      = 345

' -------------------------------------- Software limits
Xmax =   35
Xmin =    2
Ymax =  180
Ymin =    2
Zmax =  190
Zmin =   85
Rmax =  350
Rmin =    0
Amax =   30
Amin =    2

' -------------------------------------- BUTTONS
Button 14 Text  JoyPad
Button 14 Color Orange
Button 14 Slot  Slot_JoyPadStartStop





' -------------------------------------- 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
Key A 			Gosub ExecKeys
Key Z 			Gosub ExecKeys




'<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
Stop						 '<<<<<<<<<<<<<< S T O P >>>>>>>>>>>>>>>>>
'<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

' ====================================================================
'  VARIABLES
' ====================================================================
Variable Numeric Slot_X
Variable Numeric Slot_Y
Variable Numeric Slot_A
Variable Numeric Slot_Z
Variable Numeric Slot_R

Variable Numeric Slot_SensorX
Variable Numeric Slot_SensorY
Variable Numeric Slot_SensorZ
Variable Numeric Slot_SensorA

Variable Numeric Slot_LedR
Variable Numeric Slot_LedG
Variable Numeric Slot_LedB

Variable Numeric Slot_Dispenser
Variable Numeric Slot_EnableMotors
Variable Numeric Slot_Emergency
Variable Numeric Slot_LASER
Variable Numeric Slot_LockSensor

Variable Numeric Slot_PEN
Variable Numeric PenDown_Value
Variable Numeric PenUp_Value

Variable Numeric Xmax
Variable Numeric Xmin
Variable Numeric Ymax
Variable Numeric Ymin
Variable Numeric Zmax
Variable Numeric Zmin
Variable Numeric Rmax
Variable Numeric Rmin
Variable Numeric Amax
Variable Numeric Amin



' ====================================================================
'  MOTOR NUMBER TO SLOT CONVERSION
' ====================================================================
Variable Numeric NumMotors
Variable Numeric MotorNumber
Variable Numeric SlotFromMotorNumber

Label MotorNumberToSlot
	Select MotorNumber
		Case 0
			SlotFromMotorNumber = Slot_X
		Case 1
			SlotFromMotorNumber = Slot_Y
		Case 2
			SlotFromMotorNumber = Slot_Z
		Case 3
			SlotFromMotorNumber = Slot_A
	EndSelect
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_X = Limit(Slot(Slot_X) + JogSpeed, Xmin, Xmax)
	EndIf
	If Key("Left")
		Slot Slot_X = Limit(Slot(Slot_X) - JogSpeed, Xmin, Xmax)
	EndIf
	'
	If Key("Up")
		Slot Slot_Y = Limit(Slot(Slot_Y) + JogSpeed, Ymin, Ymax)
	EndIf
	If Key("Down")
		Slot Slot_Y = Limit(Slot(Slot_Y) - JogSpeed, Ymin, Ymax)
	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
	'
	If Key("A")
		Slot Slot_A = Limit(Slot(Slot_A) + JogSpeed, Amin, Amax)
	EndIf
	If Key("Z")
		Slot Slot_A = Limit(Slot(Slot_A) - JogSpeed, Amin, Amax)
	EndIf
	'
	If LimitReached
		Gosub JoyPadVibrate
	EndIf
	Gosub PrintValues
Return

' ====================================================================
'  JoyPad
' ====================================================================
Variable Numeric Slot_JoyPadX
Variable Numeric Slot_JoyPadY
Variable Numeric Slot_JoyPadZ
Variable Numeric Slot_JoyPadR
Variable Numeric Slot_JoyPadStartStop
Variable Numeric Slot_JoyPadSpeed
Variable Numeric Slot_JoyPadTexBox
Variable Numeric Slot_JoyPadSaveCoord
Variable Numeric Slot_JoyPadDispenser
Variable Numeric Slot_JoyPadFrontWhite
Variable Numeric Slot_JoyPadRed
Variable Numeric Slot_JoyPadGreen
Variable Numeric Slot_JoyPadBlu
Variable Numeric Slot_JoyPadPen
Variable Numeric Slot_JoyPadHome6

Variable Numeric JoyPad_DeadBand
Variable Numeric JoyPad_Speed
Variable Numeric JoyPad_SpeedMult

Variable Numeric Slot_JoyPadTrigger
Variable Numeric Slot_JoyPadDirections
Variable Numeric Slot_JoyPadMagnitude
Variable Numeric Slot_JoyPadDuration

Variable Numeric JoyPad_Y0
Variable Numeric JoyPad_V2
Variable Numeric JoyPad_V3

Label JoyPad
	
		' ----------------------------------- 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.00001
		Slot Slot_Y = Limit(Slot(Slot_Y) + JoyPad_V3 * Sign(JoyPad_V2), Ymin, Ymax)
		If LimitReached
			Gosub JoyPadVibrate
		EndIf
		Gosub PrintValues
	EndIf
Return

Label JoyPadVibrate
	Slot Slot_JoyPadDirections = 0
	Slot Slot_JoyPadMagnitude  = 500
	Slot Slot_JoyPadDuration   = 200
	Slot Slot_JoyPadTrigger    = 1000
	Wait Seconds 0.01
	Slot Slot_JoyPadTrigger    = 0
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 + " R=" + Format(Slot(Slot_R), "0.000")
	's1 = s1 + " A=" + Format(Slot(Slot_A), "0.000")
	Print s1 + CRLF
Return


'WATCH s1
'WATCH Rmax
'WATCH Rmin
'WATCH New watch
'WATCH Slot_JoyPadSaveCoord
'EXEC DestX = 12
