Malfunction of Distance function

PetrB

Hello,

I'm using function Distance to detect current position position of TCP for decision which path to take.




There is Task running in background (type Static), which switching DO bits on/off based on current position. There are around 50 positions.

When PLC issues command Go to home, based on DO states or distance from certain position, controller use correct path to get to home position. Those conditions are in main motion task.



This logic was working fine for few weeks, until today. Basically robot took a wrong path to home and would hit the stand, if I wouldn't stop it. I went through logic numerous times, but can't find any bug in my code and taken in account, that logic was working flawlessly for weeks, I don't think it's bug in my coding.

There are two options what happen. First is that controller set wrong DO bit or result of Distance function returned incorrect number. When issue happened, TCP was about 1000mm away from indicated position.

Any ideas what could be wrong?

Thank you

lemster68

I would be a bit wary of the background task setting bits like so, especially if the robot is moving quickly.  Could be a lag in the bits changing, and where in your code do you turn off bits that were previously on but no longer valid?  I am assuming that you have more ELSEIF for other positions.  I wrote this code some time ago which could be used for a recovery to home function.

MODULE Utility

  VAR string stRobt:=stEmpty;

  VAR string stClosestPoint;

  CONST num nMaxRecoverDist:=2000;

  PERS tooldata tCheck;

  PERS string stToolName;

  TASK PERS wobjdata wobjCheck:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];

  PERS string stWobjName:="wobj0";

  

  PROC FindNearestPoint()

    VAR robtarget pCurrentPosition;

    VAR num n12Distance:=10000;

    VAR robtarget checkrobt;

    VAR datapos block;

    

    GetSysData tCheck\ObjectName:=stToolName;

    GetSysData wobjCheck\ObjectName:=stWobjName;

    pCurrentPosition:=CRobT(\Tool:=tCheck\WObj:=wobjCheck);

    SetDataSearch "robtarget"\InMod:="R1_Data";

    WHILE GetNextSym(stRobt,block\Recursive) DO

      GetDataVal stRobt\TaskName:="T_ROB1", checkrobt;

      ! Compare current robot position with stored robtarget

      ! If it is less than maximum recovery distance store the name

      ! and distance from current position.

      IF (Distance(checkrobt.trans,pCurrentPosition.trans) < nMaxRecoverDist) THEN

        ! Check to see if point is within Maximum recovery

        ! but maybe farther than last compared point.

        ! Should update n12Distance if closer point is found.

        IF Distance(checkrobt.trans,pCurrentPosition.trans) < n12Distance THEN

          n12Distance:=Distance(checkrobt.trans,pCurrentPosition.trans);

          stClosestPoint:=stRobt;

        ELSE

          ! Do nothing, point is farther from last found point

          ! within max recover distance nMaxRecoverDist.

        ENDIF

      ENDIF

    ENDWHILE

    IF n12Distance >= nMaxRecoverDist THEN

      TPWrite "Nearest taught point is Not Found!!!  Caution!!!";

      TPWrite "Not close enough to a known taught point!!!";

      TPWrite "Active tool used to check was " + stToolName;

      TPWrite "Active work object used to check was " + stWobjName;

    ELSE

      TPWrite "Nearest taught point is: " + stClosestPoint;

      TPWrite "Distance from current position is: "\Num:=n12Distance;

      TPWrite "Active tool used to check was " + stToolName;

      TPWrite "Active work object used to check was " + stWobjName;

    ENDIF

  ENDPROC

ENDMODULE

PetrB

Lag would make sense, if robot took closer to TCP. But as I said, Robot took path, which was 1000mm from TCP, so that bit should be off for seconds.

Bits are being turned off right in same IF function. Yes, there are about 50 more IF's.


Thanks for example. I'm operating robot inside steel box and there very limited space for maneuvering, so basically if robot is between two defined positions, he cannot recover, cause from every point there is complex path how to get to home position.

PetrB

I probably figure out the problem.

I have multi move station with 2 robots. According to manual, for CRobT function there is argument [\TaskRef], to identify the robot. I made two separate function with [\TaskRef] refering to right robot (T_ROB1Id and  T_ROB2Id).

Function with all the parameters, if any one need it in future:

    PERS num nDist_R1;

    PERS robtarget pCurrentPosition_R1;

  FUNC num nDistance_R1(

        robtarget pEnd_R1,

      PERS wobjdata WObj_R1)

        pCurrentPosition_R1:=CRobT(\TaskRef:=T_Rob1Id\Tool:=ToxTool_R1\WObj:=WObj_R1);

        nDist_R1:=Distance(pCurrentPosition_R1.trans,pEnd_R1.trans);

        Return nDist_R1;

lemster68

I see that you are turning the bits off, wasn't paying attention closely enough.