% Configure serial port and move to Hard Home (0,99.22,321.47,0,0)
A=exist('s');
if A==0
    s = initXR4 ; 
    gripper(0,s) ;
    moveXYZAT(s,x,y,z,0,0) ;
    gripper(1,s) ;
    moveXYZAT(s,x,y,z+20,0,0) ;
    moveXYZAT(s,x,y,z,0,0) ;
    gripper(0,s) ;
    moveXYZAT(s,0,90,310,0,0);
    gripper(1,s) ;
    i=input('Continue? y/n\n','s');
    while i=='y'
        gripper(0,s) 
        moveXYZAT(s,x,y,z,0,0) ;
        gripper(1,s) ;
        moveXYZAT(s,x,y,z+20,0,0) ;
        moveXYZAT(s,x,y,z,0,0) ;
        gripper(0,s) ;
        moveXYZAT(s,0,90,310,0,0)
        gripper(1,s) ;
        i=input('Continue? y/n\n','s');
    end
    gripper(0,s) ;
    sendCmd('HH',s) ; % Hard Home
    closeXR4(s) ;
    clear s
elseif A==1
    disp('XR4 is already initiated\n')
    gripper(0,s) ;
    moveXYZAT(s,x,y,z,0,0) ;
    gripper(1,s) ;
    moveXYZAT(s,x,y,z+20,0,0) ;
    moveXYZAT(s,x,y,z,0,0) ;
    gripper(0,s) ;
    moveXYZAT(s,0,90,310,0,0);
    gripper(1,s) ;
    i=input('Continue? y/n\n','s');
    while i=='y'
        gripper(0,s) 
        moveXYZAT(s,x,y,z,0,0) ;
        gripper(1,s) ;
        moveXYZAT(s,x,y,z+20,0,0) ;
        moveXYZAT(s,x,y,z,0,0) ;
        gripper(0,s) ;
        moveXYZAT(s,0,90,310,0,0)
        gripper(1,s) ;
        i=input('Continue? y/n\n','s');
    end
    gripper(0,s) ;
    sendCmd('HH',s) ; % Hard Home
    closeXR4(s) ;
    clear s
end
    

    

