Module Demo() 
VAR errnum ERRCodeGripper := -1;  

!Example of program using the previously defined procedures 
PROC Main()  

     BookErrNo ERRCodeGripper; 
     !Reset/Activate the Gripper to cancel previous config.   
     gripperReset;  
     gripperActivate;  
      
     !Full speed, full force, pinch mode  
     gripperSpeed 255;  
     gripperForce 255;
     gripperChangeMode 1;  

     !Partial close of  the Gripper (can reduce cycle  
     !time if multiple objects are to be picked up)  
     gripperMove 100;  
 
     !Approach the object  
     moveRobotToApproachPosition;   

     !Move to the pickup position  
     moveRobotToPickupPosition; 

     !Gripper full close   
     gripperMove 255; 
 
     !Verify if the object has been picked up  
     !(Does not work for very small objects)  
     IF(NOT gripperHasObject()) THEN   
        !No object! Do special procedure.   
        ErrorObjectNotFound;  
     ENDIF  

     !Move the robot to the release location  
     moveRobotToReleasePosition; 
  
     !Fully open the Gripper  
     gripperMove 0;  

     !Move the robot to its home position  
     moveRobotToHomePosition;   

ENDPROC  

!Reset the Gripper command 
PROC gripperReset()   
     SetGO O_rMOD 0;  
     SetGO O_rPRA 0;  
     SetGO O_rSPA 0;  
     SetGO O_rFRA 0;  
     reset o_rGLV;  
     reset O_rAAC;  
     reset O_rICF;  
     reset O_rICS;  
     reset O_rGTO;  
     reset O_rATR;  
     reset O_rACT;  
     waitUntil(I_gIMC = 0); 
ENDPROC  

!Activate the Gripper and wait until initialization is over 
PROC gripperActivate()   
     Set O_rACT;  
     waitUntil(I_gIMC = 3); 
ENDPROC  

!Move the gripper to the specified position 
!and wait until motion is over 
!posRequest = 0   for fully opened 
!posRequest = 255 for fully closed 
PROC gripperMove(num posRequest)  
     !The limits are validated  
     posRequest:= round(posRequest);  
     IF (posRequest > 255) posRequest:=255; 
     IF (posRequest < 0) posRequest:=0; 
 
     SetGO O_rPRA posRequest;  
     Set O_rGTO; 

     !Wait until the position request echo is OK  
     !and the motion is over.  
     waitUntil (I_gPRA = posRequest and I_gSTA > 0);  
ENDPROC  

!Operation mode change with the Gripper fully opened 
!mode = {0:Basic, 1:Pinch, 2:Wide, 3:Scissor} 
PROC gripperChangeMode(num mode)  
     IF ((mode <0) or (mode >3) RAISE ERRCodeGripper;  
     gripperMove 0;  
     SetGO O_rMOD mode;  
     !Wait until the mode request echo is OK  
     !and the motion is over.  
     waitUntil(I_gMOD = mode and I_gIMC = 3); 
ENDPROC  

!Change Gripper Speed (0-255) 
PROC gripperSpeed(num speed)  
     !Validate the limits  
     Speed:= round(speed);  
     If (speed > 255) speed:=255;  
     If (speed < 0) speed:=0;  
     SetGO O_rSPA,speed;
ENDPROC
 
!Change Gripper Force (0-255) 
PROC gripperForce(num force) 
     !Validate the limits  force:= round(force);  
     If (force > 255) force:=255;  
     If (force < 0) force:=0;  
     SetGO O_rFRA, force; 
ENDPROC  
 
!Verify if Gripper has an object by verifying if the
!motion as stopped before requested position 
!Small objects might not be detectable with this approach 
FUNC bool gripperHasObject()  
     RETURN ((I_gSTA = 1) OR (I_gSTA = 2)); 
ENDFUNC  

ENDModule