'***************************************************************************
'Galil Motion Control Sample DMC Code
'***************************************************************************
'Description:
'Pick and Place example
'***************************************************************************
#initial             ;'label
MG "HOME AXES"
FI AB                ;'drive x and y to home
BG AB                ;'start motion
AM AB                ;'wait until completion

MG "INITIALIZE VARIABLES"
x0 = _RPA            ;'initialize start position
y0 = _RPB
x1 = 10000           ;'initialize first pick position
y1 = 10000
x2 = 20000           ;'initialize first place position
y2 = 20000

#pick                ;'loop label
MG "MOVE TO PICK POSITION"
dx = x1 - x0         ;'find x difference
DY = y1 - y0         ;'find y difference
VP dx, dy            ;'command position
VS 40000             ;'vector speed
VA 200000            ;'vector acceleration
VD 200000            ;'vector decceleration
VE                   ;'end of vector move definition
BG S                  ;'start xy motion
AM S                  ;'wait for motion completion

MG "PICK UP PART"
PR ,,-50000          ;'move head down (z-axis)
SP ,,20000           ;'z speed
AC ,,80000           ;'z acceleration
DC ,,80000           ;'z deceleration
BG C                 ;'start z motion
AM C                 ;'wait for z motion completion
SB 1                 ;'set output bit: activate holding solenoid
PR ,,50000           ;'raise head
BG C                 ;'start z motion
AM C                 ;'wait for z motion complete

MG "MOVE TO PLACE POSITION"
dx = x2 - x1         ;'compute x difference
DY = y2 - y1         ;'compute y difference
VP dx, dy            ;'command motion
VE                   ;'end of vector move definition
BG S                 ;'start xy motion
AM S                 ;'wait for xy completion

MG "PLACE PART"
PR ,,-50000          ;'lower head
BG C                 ;'start head motion
AM C                 ;'wait for z motion complete
CB 1                 ;'clear output bit: release holding solenoid
PR ,,50000           ;'raise head
BG C                 ;'start z motion
AM C                 ;'wait for z motion complete

MG "COMPUTE NEW POSITIONS"
x0 = x2              ;'new start position is where we last placed
y0 = y2
x1 = x1 + 20000      ;'compute new pick position
y1 = y1 + 20000
x2 = x2 + 20000      ;'compute new place position
y2 = y2 + 20000
JP #pick             ;'repeat indefinitely

MG "END"
EN                   ;'end of program