'================================================================================
'******SmartMotor 4-Axis Gantry Demo******
'================================================================================
'This assumes 4 axis gantry with motors set up as:
' Motor	 Address
'	X 		1
'	Y		2
'	Z		3
'	X'		4
'This code is for example only.  
'Homing and error handling code should be added.
END
'================================================================================
'================================================================================
C1	'Make sync move of 4 motors
 VTS=v				'Set path velocity
 ADTS=a				'Set path accel/decel
 PTS(x;1;4,y;2,z;3)	'Set position to move to
 IF PTSD!=0 GS ENDIF	'Go
 TSWAIT				'Wait for moves to complete
RETURN
'================================================================================
'================================================================================
C500 ' Build-A-Pallet Subroutine
GOSUB(501)			'Get sample pallet values
GOSUB(502)			'Z up
x=g					'Set X pick position
y=h					'Set Y pick position
GOSUB(1)			'Go and linear interpolate the move
GOSUB(503)			'Z down
WAIT=w
GOSUB(502)			'Z up
'	Go to pick subroutine HERE 
c=0					'Zero X loop counter
WHILE c<n			'X row counter loop
 	cc=0			'Zero Y loop counter
	WHILE cc<nn		'Y row counter loop
        s=o*c  		'Find X row position offset
		x=q+s 		'Set X pick place to that offset
 		t=p*cc   	'Find Y row position offset
	    y=r+t		'Set Y pick place to that offset
		GOSUB(1)	'Go and linear interpolate the move
		GOSUB(503)	'Z down
		WAIT=w
		GOSUB(502)	'Z up
		'	Go to place subroutine HERE
        x=g			'Set X position to pick location
		y=h			'Set Y position to pick location
    	GOSUB(1)	'Go and linear interpolate the move
		GOSUB(503)	'Z down
		WAIT=w
		GOSUB(502)	'Z up
		'	Go to pick subroutine
		cc=cc+1		'Update Y loop counter
	LOOP			'Y loop end
	c=c+1			'Update X loop counter
LOOP				'X loop end
RETURN
'================================================================================
'================================================================================
C501
'Example Build-A-Pallet variables
'Note: The following data or an equivalent of same variable names 
'      must be called prior to running subfunction C500
	v=100000		'Speed
	a=100			'Accel/decel

	u=60000			'Z up   position
	d=80000 		'Z down position

	n=3				'Number of X rows
	o=175000		'X row width

	nn=3			'Number of Y rows
	p=130000		'Y row width

	q=16000			'First X row position
	r=25000			'First Y row position

	g=200			'X Pick position
	h=200			'Y Pick position

RETURN
'================================================================================
'================================================================================
C502	'Z up Subroutine
 PT:4=u  					'Set Target Up position
 VT:4=v						'Set Velocity
 ADT:4=a					'Set Accel/Decel	
 G:4						'Go
 WHILE B(0,2):3==1 LOOP		'Wait for Z axis move to complete
 z=u						'Set commanded Z axis (in case C1 is called after this)
RETURN
'================================================================================
'================================================================================
C503	'Z down Subroutine
 PT:4=d  					'Set Target Down position
 VT:4=v						'Set Velocity
 ADT:4=a					'Set Accel/Decel	
 G:4						'Go
 WHILE B(0,2):3==1 LOOP		'Wait for Z axis move to complete
 z=d						'Set commanded Z axis (in case C1 is called after this)
RETURN
'================================================================================
'================================================================================
C504	'Command Z Axis to "z"
 PT:4=z  					'Set Target position
 VT:4=v						'Set Velocity
 ADT:4=a					'Set Accel/Decel	
 G:4						'Go
 WHILE B(0,2):3==1 LOOP		'Wait for Z axis move to complete
RETURN
'================================================================================