Motion Error Detection

TimMathias

If a motion error occurs in one arm of the YuMi robot, the motion error is displayed on the Pendant stating which arm caused the motion error. But this discernment between arms is not available in RAPID, only that a motion error occurred. As a workaround, it's possible work out which arm caused the motion error by trapping the motion error in both arms and comparing the time difference between when the trap routines were called because the arm that caused the motion error will call its trap routine before the other arm.

MODULE LeftArm
	PROC main()
		! Initialise the Motion Error Detector.
		MED_Initialise TASK_LEFT_ARM,TASK_RIGHT_ARM;

		!
		! Loop through some movements.
		!
		WHILE bRun DO
		ENDWHILE

		MED_Done;
	ERROR
		TPWriteProc "main.ERROR","@"\n:=ClkRead(clock1\HighRes);
	ENDPROC
ENDMODULE<br>

MODULE RightArm
	PROC main()
		! Initialise the Motion Error Detector, but swap the tasks relative to the other arm.
		MED_Initialise TASK_RIGHT_ARM,TASK_LEFT_ARM;

		!
		! Loop through some movements.
		!
		WHILE bRun DO
		ENDWHILE

		MED_Done;
	ERROR
		TPWriteProc "main.ERROR","@"\n:=ClkRead(clock1\HighRes);
	ENDPROC
ENDMODULE<br>

Place a copy of the following modules in each arm.

MODULE MotionErrorDetector

    RECORD MotionErrorData
        num seconds;
        bool error_detected;
    ENDRECORD

    PERS MotionErrorData MED_Data{2};

    LOCAL VAR TaskID this_arm;
    LOCAL VAR TaskID other_arm;

    LOCAL VAR clock clk;

    LOCAL VAR intnum interrupt;

    VAR bool MED_motion_error:=FALSE;

    TRAP MED_Trap

        MED_Data{this_arm}.seconds:=ClkRead(clk\HighRes);

        TPWriteProc "MED_Trap","BEGIN @"\n:=MED_Data{this_arm}.seconds;

        ! Ensure the time is read in both arms before comparing the time difference.
        SyncTasksPP this_arm,other_arm;

        ! Set a flag to indicate that this arm has a motion error.
        ! This IF statement depends upon our arm interrupt being called before the other arm interrupt.
        IF MED_Data{this_arm}.seconds<MED_Data{other_arm}.seconds THEN

            MED_Data{this_arm}.error_detected:=TRUE;
            MED_Data{other_arm}.error_detected:=FALSE;
            TPWriteProc "MED_Trap","TaskID="+NumToStr(this_arm,0);

        ENDIF

        ! Set a flag to indicate that a motion error occurred.
        MED_motion_error:=TRUE;

        TPWriteProc "MED_Trap","END";

    ERROR

        TPWriteProc "MED_Trap.ERROR","@"\n:=ClkRead(clock1\HighRes);

    ENDTRAP

    PROC MED_Initialise(TaskID this,TaskID other)

        this_arm:=this;
        other_arm:=other;

        MED_ResetSyncStartClock;

        ! Delete any previous assignemnt before assigning it.
        IDelete interrupt;
        CONNECT interrupt WITH MED_Trap;
        IError MOTION_ERR,TYPE_ERR,interrupt;

    ENDPROC

    PROC MED_ResetSyncStartClock()

        ClkReset clk;
        SyncTasksPP this_arm,other_arm;
        ClkStart clk;

    ENDPROC

    PROC MED_Done()

        IDelete interrupt;

    ENDPROC

ENDMODULE<br>

MODULE Synchronisation

    ALIAS bool SyncStep;
    ALIAS num TaskID;

    RECORD R_Sync
        SyncStep SyncStep1;
        SyncStep SyncStep2;
    ENDRECORD

    CONST TaskID TASK_LEFT_ARM:=1;
    CONST TaskID TASK_RIGHT_ARM:=2;

    PERS R_SyncPP SyncPP{2};

    PROC SyncTasksPP(TaskID task_id_this,TaskID task_id_other)

        ! Reset the flag to say we are not at sync step 2.
        SyncPP{task_id_this}.SyncStep2:=FALSE;

        ! Set the flag to say we are at sync step 1.
        SyncPP{task_id_this}.SyncStep1:=TRUE;

        ! Wait for other task to reach step 1.
        WaitUntil SyncPP{task_id_other}.SyncStep1;

        ! Set the flag to say we are at step 2.
        SyncPP{task_id_this}.SyncStep2:=TRUE;

        ! Wait until the other task has computed our step 1 flag before continuing.
        WaitUntil SyncPP{task_id_other}.SyncStep2;

        ! Reset this step 1 flag after step 2 to avoid a race condition.
        SyncPP{task_id_this}.SyncStep1:=FALSE;

    ERROR

        TPWriteProc "SyncTasksPP.ERROR","";

    ENDPROC

ENDMODULE<br>

MODULE Helper
    PROC TPWriteProc(string sProcName,string s,\num n|bool b|pos p|orient o|dnum d)
        VAR string str;
        str:=sProcName+" "+s;
        IF Present(n) THEN
            TPWrite str\num:=n;
        ELSEIF Present(b) THEN
            TPWrite str\bool:=b;
        ELSEIF Present(p) THEN
            TPWrite str\pos:=p;
        ELSEIF Present(o) THEN
            TPWrite str\orient:=o;
        ELSEIF Present(d) THEN
            TPWrite str\dnum:=d;
        ELSE
            TPWrite str;
        ENDIF
    ERROR
        TPWrite "TPWriteProc.ERROR @"\num:=ClkRead(clock1\HighRes);
    ENDPROC
ENDMODULE<br>