abb irc5 Input/Output Trap Handlers

VAR intnum iRob_Trap;

PROC rWatch_Dog()
IDelete iRob_Trap;
CONNECT iRob_Trap WITH trGrip_Error;
ISignalDI Di01_Grip_Clamp, 0, iRob_Trap;
ISleep iRob_Trap;
RETURN;
EndPROC

TRAP trGrip_Error
PulseDO\PLength := 2.0, Do20_Grip_Clamp_Fault;
STOP;
ISleep iRob_Trap;

ENDTRAP

PROC rST10_Pickup()
rWatch_Dog;
WaitUntil Di30_ST10_Grant = 1;
Reset Do30_ST10_Interference;

MoveJ pST10_Ready, v1000, z50, tool0;
MoveL Offs(pST10_Part, 0, 0, 300), v500, z30, tool0;
MoveL Offs(pST10_Part, 0, 0, 50), v300, z10, tool0;
MoveL pST10_Part, v50, fine, tool0;

rGrip_Clamp;
IWatch iRob_Trap;

MoveL Offs(pST10_Part, 0, 0, 50), v300, z10, tool0;
MoveL Offs(pST10_Part, 0, 0, 300), v500, z30, tool0;
MoveJ pST10_Ready, v1000, z50, tool0;

Set Do30_ST10_Interference;
PulseDO\PLength :=2.0, Do31_ST10_Pickup_End;
RETURN;
EndPROC

PROC rST20_Unload()

WaitUntil Di40_ST20_Grant = 1;
Reset Do40_ST20_Interference;

MoveJ pST20_Ready, v1000, z50, tool0;
MoveL Offs(pST20_Part, 0, 0, 300), v500, z30, tool0;
MoveL Offs(pST20_Part, 0, 0, 50), v300, z10, tool0;
MoveL pST20_Part, v50, fine, tool0;

ISleep iRob_Trap;
IDelete iRob_Trap;
WaitTime 0.5;

rGrip_Open;

MoveL Offs(pST20_Part, 0, 0, 50), v300, z10, tool0;
MoveL Offs(pST20_Part, 0, 0, 300), v500, z30, tool0;
MoveJ pST20_Ready, v1000, z50, tool0;

Set Do30_ST20_Interference;
PulseDO\PLength :=2.0, Do31_ST20_Pickup_End;
RETURN;
EndPROC

PROC rGrip_Clamp()
! Gripper Clamp Operation

Check:
WaitTIme 0.2;
Set Do51_Grip_Clamp;
Reset Do52_Grip_Open;
WaitUntil Di51_Grip_Clamp = 1 AND Di52_Grip_Open = 0\MaxTime:=5\TimeFlag:=bTimeover;

IF bTimeOver = FALSE THEN
WaitTIme 1.0;
GripLoad ldPart;
RETURN;
ElseIF bTimeOver = TRUE THEN
TPErase;
Reset Do51_Grip_Clamp;
Reset Do52_Grip_Open;
ErrWrite " PROC rGrip_Clamp", " Gripper Clamp Operation has Failed!"\RL2:=" 1. Check Di51 or Di52 Input Signal"\RL3:=" 2. Check Air Pressure";
Stop;
GOTO Check;
EndIF

EndPROC

I think you can keep things more simplified by using the error handler, here is one example:

PROC rCloseGripper(
\switch swWithPart)

CONST string astStillOpen1{6}:=[" ","The Gripper is being told","to close, but it is still","showing open."," "," "];
CONST string astNotClosed1{6}:=[" ","The Gripper is being told","to close, but it is not","showing closed."," "," "];
CONST string astPartPres{6}:=[" ","Gripper part present switch","is not showing a part present.","","It should be showing a part present."," "];
VAR bool bOpenFlag1;
VAR bool bClsedFlag1;
VAR bool bNoPartPres;
CONST errnum erStillOpen1:=99;
CONST errnum erNotClsed1:=98;
CONST errnum erNoPartPres:=97;
VAR num nDummy;

! Close Grippers
Reset doUnclampJet;
Set doClampJet;
WaitUntil diJetUnclamped=0\MaxTime:=2\TimeFlag:=bOpenFlag1;
IF bOpenFlag1 RAISE erStillOpen1;
IF Present(swWithPart) AND fiDryCycle=0 THEN
WaitUntil diJetPresent=1\MaxTime:=2\TimeFlag:=bNoPartPres;
IF bNoPartPres RAISE erNoPartPres;
ENDIF
bPartPresent := TRUE;
WaitUntil diJetClamped=1\MaxTime:=2\TimeFlag:=bClsedFlag1;
IF bClsedFlag1 RAISE erNotClsed1;
RETURN;
BACKWARD
! Again it is possible to add
! opengripper here with a move to the
! pick so backward execution behaves
! like a mirror of forward.
Return;
ERROR
IF ERRNO=erStillOpen1 THEN
WHILE diJetUnclamped=1 DO
rAlarmScreen (ERR_UNCLAMP_ON);
ENDWHILE
ELSEIF ERRNO=erNoPartPres THEN
WHILE diJetPresent=0 DO
rAlarmScreen (ERR_NOPART);
ENDWHILE
Reset foOKtoTighten;
RAISE;
ELSEIF ERRNO=erNotClsed1 THEN
IF fiDryCycle = 1 TRYNEXT;
WHILE diJetClamped=0 DO
rAlarmScreen (ERR_CLAMP_OFF);
ENDWHILE
ENDIF
TRYNEXT;
ENDPROC