&ACCESS RVP
&REL 117
&PARAM TEMPLATE = C:\KRC\Roboter\Template\ExpertVorgabe
&PARAM EDITMASK = *
DEF bierkasten( )

;--------- Declaration ---------
EXT BAS(BAS_COMMAND:IN, REAL:IN)
DECL AXIS  HOME, HOME_AHEAD
DECL FRAME PositionAboveFirstBottle, PositionFlange
INT I,J

;---------- Initialization ---------
BAS(#INITMOV,0)    ;Initialization of speed, acceleration and coordinate system
$BASE = $WORLD     ;set the base coordinate system (mandatory when using a frame)
$TOOL = $NULLFRAME ;set not to use any tool coordinate system
HOME={AXIS: A1 0,A2 -90,A3 90,A4 0,A5 0,A6 0}  ;set home position
HOME_AHEAD={AXIS: A1 0,A2 -60,A3 60,A4 0,A5 0,A6 0}  ;set position ahead of home
PositionAboveFirstBottle={FRAME: X 1657,Y -502,Z 480,A -90,B 90,C 0}  ;set position above the first bottle

;------------- Main program ------------
PTP HOME ;SAK-Fahrt
PULSE($OUT[15], TRUE,0.5) ;open gripper
FOR J = 1 to 6
  $VEL_AXIS[J]=75 ;x% max. speed
  $ACC_AXIS[J]=75 ;x% max. acceleration
ENDFOR
PTP HOME_AHEAD ;move to a position in front of home
;Positioning of the crate
PTP {FRAME: X 1520,Y -450,Z 485,A -180,B 0,C 180} C_PTP  ;move to position behind the box
PTP {FRAME: X 1640,Y -450,Z 330,A -180,B 0,C 180}        ;move gripper against box
PTP {FRAME: X 1520,Y -450,Z 485,A -180,B 0,C 180} C_PTP  ;move to position behind the box
PTP {FRAME: X 2180,Y -450,Z 485,A -180,B 0,C 180} C_PTP  ;move to position in front of box
PTP {FRAME: X 2060,Y -450,Z 330,A -180,B 0,C 180}        ;move gripper against box
PTP {FRAME: X 2180,Y -450,Z 485,A -180,B 0,C 180} C_PTP  ;move to position in front of box
PTP {FRAME: X 1820,Y -666,Z 485,A -90,B 0,C -180} C_PTP  ;move to position left of the box
PTP {FRAME: X 1820,Y -586,Z 330,A -90,B 0,C -180}        ;move gripper against box
PTP {FRAME: X 1820,Y -666,Z 485,A -90,B 0,C -180} C_PTP  ;move to position left of the box
PTP {FRAME: X 1820,Y -148,Z 485,A -90,B 0,C -180} C_PTP  ;move to position to the right of the box
PTP {FRAME: X 1820,Y -268,Z 330,A -90,B 0,C -180}        ;move gripper against box
PTP {FRAME: X 1820,Y -148,Z 485,A -90,B 0,C -180} C_PTP  ;move to position to the right of the box
PTP {FRAME: X 1540,Y -450,Z 485,A -180,B 0,C 180} C_PTP  ;move to position behind the box

;move to pre-position
PTP PositionAboveFirstBottle    ;move over the first bottle

;move the first three bottles of the first row
FOR I = 0 to 2
  FOR J = 1 to 6
    $VEL_AXIS[J]=50 ;x% max. speed
    $ACC_AXIS[J]=50 ;x% max. acceleration
  ENDFOR
  PositionFlange = PositionAboveFirstBottle
  PositionFlange.Y = PositionAboveFirstBottle.Y + I * 65 ;set Y-position of the bottle to be gripped
  PTP PositionFlange
  PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
  $VEL.CP = 0.1             ;set speed for LIN movements in m/s
  LIN PositionFlange       ;move down to the bottle
  PositionFlange.Y = PositionAboveFirstBottle.Y - 30 + I * 65
  $VEL.CP = 0.02            ;set speed for LIN movements in m/s
  LIN PositionFlange       ;move into the bottle
  PositionFlange.Y = PositionAboveFirstBottle.Y - 27 + I * 65
  PositionFlange.Z = PositionAboveFirstBottle.Z + 150
  $VEL.CP = 0.1             ;set speed for LIN movements in m/s
  LIN PositionFlange       ;lift out the bottle
  FOR J = 1 to 6
    $VEL_AXIS[J]=50 ;% max. speed
    $ACC_AXIS[J]=5 ;% max. acceleration
  ENDFOR
  PositionFlange.X = PositionAboveFirstBottle.X + 3 * 65 
  PTP PositionFlange       ;move to the drop down point
  FOR J = 1 to 6
    $VEL_AXIS[J]=50 ;% max. speed
    $ACC_AXIS[J]=5 ;% max. acceleration
  ENDFOR
  PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
  LIN PositionFlange       ;drop down the bottle
  PositionFlange.Y = PositionAboveFirstBottle.Y + I * 65   
  LIN PositionFlange       ;move out of the bottle
  PositionFlange.Z = PositionAboveFirstBottle.Z
  $VEL.CP = 0.5             ;set speed for LIN movements in m/s
  LIN PositionFlange       ;move up
ENDFOR

;Moving the fourth bottle in the first row
FOR J = 1 to 6
  $VEL_AXIS[J]=50 ;x% max. speed
  $ACC_AXIS[J]=50 ;x% max. acceleration
ENDFOR
PTP {FRAME: X 1900,Y -300,Z 850,A 0,B 90,C 0} C_PTP       ;move to position to change gripping orientation
PositionFlange = PositionAboveFirstBottle
PositionFlange.X = PositionAboveFirstBottle.X + 3
PositionFlange.Y = PositionAboveFirstBottle.Y + 2 * 65 - 42
PositionFlange.A = PositionAboveFirstBottle.A + 180
PTP PositionFlange
PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
$VEL.CP = 0.1             ;set speed for LIN movements in m/s
LIN PositionFlange       ;move down to the bottle
PositionFlange.Y = PositionAboveFirstBottle.Y + 2 * 65 - 42 + 30
$VEL.CP = 0.02            ;set speed for LIN movements in m/s
LIN PositionFlange       ;move into the bottle
PositionFlange.Y = PositionAboveFirstBottle.Y + 2 * 65 - 42 + 27
PositionFlange.Z = PositionAboveFirstBottle.Z + 150
$VEL.CP = 0.1             ;set speed for LIN movements in m/s
LIN PositionFlange       ;lift out the bottle
FOR J = 1 to 6
  $VEL_AXIS[J]=50 ;x% max. speed
  $ACC_AXIS[J]=5 ;x% max. acceleration
ENDFOR
PositionFlange.X = PositionAboveFirstBottle.X + 3 * 65 
PTP PositionFlange       ;move to the drop down point
PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
LIN PositionFlange       ;drop down the bottle
PositionFlange.Y = PositionFlange.Y - 27   
LIN PositionFlange       ;move out of the bottle
PositionFlange.Z = PositionAboveFirstBottle.Z
$VEL.CP = 0.5             ;set speed for LIN movements in m/s
LIN PositionFlange       ;move up
;move to home position
FOR I = 1 to 6
  $VEL_AXIS[I]=100
  $ACC_AXIS[I]=100
ENDFOR
PTP HOME_AHEAD ;move to a position in front of home
PTP HOME

WAIT FOR $IN[16]==TRUE   ;wait for an impulse on input 16 to trigger continuation

PTP HOME_AHEAD ;move to a position in front of home
FOR I = 0 to 2
  FOR J = 1 to 6
    $VEL_AXIS[J]=50 ;x% max. speed
    $ACC_AXIS[J]=50 ;x% max. acceleration
  ENDFOR
  PositionFlange = PositionAboveFirstBottle
  PositionFlange.X = PositionAboveFirstBottle.X + 3 * 65
  PositionFlange.Y = PositionAboveFirstBottle.Y + I * 65 ;set Y-position of the bottle to be gripped
  PTP PositionFlange
  PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
  $VEL.CP = 0.1             ;set speed for LIN movements in m/s
  LIN PositionFlange       ;move down to the bottle
  PositionFlange.Y = PositionAboveFirstBottle.Y - 30 + I * 65
  $VEL.CP = 0.02            ;set speed for LIN movements in m/s
  LIN PositionFlange       ;move into the bottle
  PositionFlange.Y = PositionAboveFirstBottle.Y - 27 + I * 65
  PositionFlange.Z = PositionAboveFirstBottle.Z + 150
  $VEL.CP = 0.1            ;set speed for LIN movements in m/s
  LIN PositionFlange       ;lift out the bottle
  FOR J = 1 to 6
    $VEL_AXIS[J]=50 ;% max. speed
    $ACC_AXIS[J]=5 ;% max. acceleration
  ENDFOR
  PositionFlange.X = PositionAboveFirstBottle.X  
  PTP PositionFlange       ;move to the drop down point
  PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
  LIN PositionFlange       ;drop down the bottle
  PositionFlange.Y = PositionAboveFirstBottle.Y + I * 65   
  LIN PositionFlange       ;move out of the bottle
  PositionFlange.Z = PositionAboveFirstBottle.Z
  $VEL.CP = 0.5             ;set speed for LIN movements in m/s
  LIN PositionFlange       ;move up
ENDFOR

;Reset the fourth bottle of the first row
FOR J = 1 to 6
  $VEL_AXIS[J]=50 ;x% max. speed
  $ACC_AXIS[J]=50 ;x% max. acceleration
ENDFOR
PTP {FRAME: X 1900,Y -300,Z 850,A 0,B 90,C 0} C_PTP       ;move to position to change gripping orientation
PositionFlange = PositionAboveFirstBottle
PositionFlange.X = PositionAboveFirstBottle.X + 3 * 65 + 3
PositionFlange.Y = PositionAboveFirstBottle.Y + 2 * 65 - 42
PositionFlange.A = PositionAboveFirstBottle.A + 180
PTP PositionFlange
PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
$VEL.CP = 0.1             ;set speed for LIN movements in m/s
LIN PositionFlange       ;move down to the bottle
PositionFlange.Y = PositionAboveFirstBottle.Y + 2 * 65 - 42 + 30
$VEL.CP = 0.02             ;set speed for LIN movements in m/s
LIN PositionFlange       ;move into the bottle
PositionFlange.Y = PositionAboveFirstBottle.Y + 2 * 65 - 42 + 27
PositionFlange.Z = PositionAboveFirstBottle.Z + 150
$VEL.CP = 0.1             ;set speed for LIN movements in m/s
LIN PositionFlange       ;lift out the bottle
FOR J = 1 to 6
  $VEL_AXIS[J]=50 ;x% max. speed
  $ACC_AXIS[J]=5 ;x% max. acceleration
ENDFOR
PositionFlange.X = PositionAboveFirstBottle.X + 3 
PTP PositionFlange       ;move to the drop down point
PositionFlange.Z = PositionAboveFirstBottle.Z - 100   
LIN PositionFlange       ;drop down the bottle
PositionFlange.Y = PositionFlange.Y - 27   
LIN PositionFlange       ;move out of the bottle
PositionFlange.Z = PositionAboveFirstBottle.Z
$VEL.CP = 0.5             ;set speed for LIN movements in m/s
LIN PositionFlange       ;move up
;move to home position
FOR I = 1 to 6
  $VEL_AXIS[I]=100
  $ACC_AXIS[I]=100
ENDFOR
PTP HOME_AHEAD ;move to a position in front of home
PTP HOME

END