Pre-executing TriggL interrupt routine

leonardolt

Greetings for all!

 I'm having some headache with TriggL instruction. I have a sequence of movements that must be done continuosly, with fly-by points. The distance between this points may go from 2 up to 10 mm. At the end of each move I must send a message to a PC telling it the actual position of the arm. This message is sent through TPC/IP connection.

 The problem I'm  experiencing is that when the robot is about 2/3 of it's way, it has already sent all messages to the PC, as if it had accomplished the whole path.
The TriggInt configuratin is set to 0.1mm, thus the trigger routine was supposed to be executed only at the right end of the motion. It seems to me that the controller is pre-executing the instructions, but in this situation it's not a good thing to happen.

 Just to antecipate this some questions, I'm using mainly 20mm/s, but this problem remains in lower speeds (5mm/s) and in higher (40mm/s). The only difference is that at higher speeds, messeages arive faster, at a higher rate, but always faster than the robot. I also tried to use stp points, instead of fly-by ones. This aparently solves this problem, but creates another one. If I use stop points, when speed is higer than 40 the movements get granulated. More than that, they become too violent and may cause damages to the structure. Is this application, smoth fly-by moves are very desirable.

Thanks in advance fror any help.
Leo

Torbjorn

Hi!

Can you show me your program?

 

Torbjorn

leonardolt

Hi, Torbjorn

 

 I put here only the code sections that are related to the TriggL issue to avoid posting lots of unnecessary information:

 

CONST num TriggDist := 0.1;

VAR intnum irq_Xscan;

VAR triggdata trg_Xscan_end;

 

! Initializing IRQ service:

 CONNECT irq_XScan WITH TrEndofMove; 

 TriggInt trg_Xscan_end, TriggDist, irq_Xscan;

 

! IRQ Routine:

 TRAP TrEndofMove

! EndofMove is the routine that is supposed to be executed at the end of

! each move. Briefly, it sends through a TCP/IP the actual position and

! updates the variable that controls the current StateMachine status.

 EndofMove;

 ENDTRAP

 

! Typical sequence of moving commands:

 pMirror :=Offs(CurrentPosition, 0, 0, 0);

 TriggL pMirrorID:= 0, ScanVel, trg_Xscan_end, ScanZone, tToolR1 Wobj := wOrgR1;

 

 pMirror :=Offs(CurrentPosition, 5, 0, 0);

 TriggL pMirrorID:= 1, ScanVel, trg_Xscan_end, ScanZone, tToolR1 Wobj := wOrgR1;

 

 pMirror :=Offs(CurrentPosition, 10, 0, 0);

 TriggL pMirrorID:= 2, ScanVel, trg_Xscan_end, ScanZone, tToolR1 Wobj := wOrgR1;

 

 pMirror :=Offs(CurrentPosition, 15, 0, 0);

 TriggL pMirrorID:= 3, ScanVel, trg_Xscan_end, ScanZone, tToolR1 Wobj := wOrgR1;

 

 pMirror :=Offs(CurrentPosition, 20, 0, 0);

 TriggL pMirrorID:= 4, ScanVel, trg_Xscan_end, ScanZone, tToolR1 Wobj := wOrgR1;

 

 So, this is a sample of what the system is doing. Movements are beeing done well. The only thing that is not working as it should is this TriggL execution. This sequence of moves is pretty large, 1000 of moves and more. As I use the trap routine to track the actual position of the arm, it is quite anoying when the arm is in half of the path and all the trap routines have already been executed, as if the robot had already accomplished the whole path.

 

 Nowadays I'm using a different approach. Instead of TriggL I'm using MoveSync instruction, as folows:

MoveLSync pMirrorID:= 1, ScanVel, ScanZone, tToolR1 Wobj := wOrgR1, "EndofMove";

 

 I have not run the full program using MoveSync yet, but the preliminary tests I run suggested that this could be a solution.

 

 Hope this helps you and let me know if you need more information.

 

 Leo

Torbjorn

Hi again!

The MoveLSync is a encapsulation of TriggInt and a TriggL. The distance used in MoveLSync is 0. So if MoveLSync work, your program should also work.

If you still have problems after you have tested with MoveLSync, write an error report on this.

 

Torbjorn

leonardolt

Hi, Torbjorn! Thanks for your reply

 Yesterday I had an intensive test session on this system. I realized that the MoveLSync instruction did not solve my problem. Like the TriggL with non fine zone, the MoveLSync performs a continuous path, but the messages received by PC are still out of sync to the robot moving.
 I noticed that if I use fine zone, both TriggL and MoveLSync work perfectly, in therms of syncronization of robot moving and TCP/IP message processing. When I attempt to use non fine zones, both aproaches fail in syncronization.

 Later I was thinking if this sync problem wouldn't be a matter of the ethernet packet timing. So today I'll  try using other notification method, such as RS 232.


 Thanks
 Leo

Torbjorn

Hi!

What zonedata do you use when you are running with "fly-by-points".

I have tested with z1 and there are little difference between the ordered position and the read. Havent tried z0.

What do you do in your TRAP? Using CRobT and then transfer the position? I think you can count on that if using "fly-by-points", you will have values that are not exact. That function will take time, and will affect the values. But it should not be by much.

Another question is, what robotware are you using and what type of robot?

 

Torbjorn

leonardolt

Hi, Torbjorn!
 I use the following zonedata configuration:
PERS zonedata ScanZone := [ FALSE, 0.5, 1, 1, 0.1, 1, 0.1 ];

 When I want to try stop points instead of fly-by, I just change from "FALSE" to "TRUE" the declaration.
 In the trap routine I don't use CRobt. As you can see in the instructions above, there is one variable called pMirror. This var stores the target position, so I dont't need to read it from the controller every time.
I'm using a IRC5  Robotware 5.12.1021.01. The robot is an IRB 1600
 This is my trap routine:

TRAP TrEndofMove

! Store the robtarget in a rawbytes variable and then send it using TCP/IP connection
SendRobT DT_P000, pMirror;

! Check if this was a single moving to a specific point.
    IF FMoveToP000 = 1 THEN
! If it was, clear the falg and update the state machine
        FMoveToP000 := 0;
        FStatus := ST_Idle;
    ENDIF


! Check the state machine to discover how to proceed at the end of the move
    TEST FStatus
   
    CASE ST_Moving:
! Finished one step of the whole step. Shoud keep on moving
! Go updating control variables and StateMachine status through TCP/IP
         Moving;
! For debug purpouses only
! Send a TCP/IP text with the current robtarget (not used in the run-mode version)
!        WriteDebugString RobTToStr (pMirror);

    CASE ST_Idle:
! Finished the whole path or a single moving.
! Sign to the PC software that the move is over.
        WriteDebugString "End of move: ST_Idle" + "ad";

    DEFAULT:
! This sould not occur. If it does, rest the system.
        FStatus := ST_Stopped;
        WriteDebugString "ST_STOPPED - Reset" + "ad";
    ENDTEST
! Use the TCP/IP connection to send the PC the current StateMachine status.
    SendInt8 DT_FStatus, FStatus;
ENDTRAP

Untill now I could not test the RS232. I'll report you back when I get it.

Best regads
Leo

Torbjorn

Hi again!
You know that when you use fly-by-points, you can get this behavior.
MP = motion pointer
PP = program pointer
RP = robot position

    pMirror :=Offs(CurrentPosition, ...
    TriggL p1
    pMirror :=Offs(CurrentPosition, ...
MP->TriggL p2
    pMirror :=Offs(CurrentPosition, ...
    TriggL p3
    pMirror :=Offs(CurrentPosition, ...
    TriggL p4
    pMirror :=Offs(CurrentPosition, ...
PP->TriggL p5
    pMirror :=Offs(CurrentPosition, ...

X---------X--------X--------X-----X
p1     |  p2      p3        p4    p5
      RP  |                       |
          MP                      PP

The program pointer is a couple of instructions before the motion pointer.
It walks ahead. The program needs to feed the system with new positions, so the
new positions can be calculated by the motion system.
Just one TRAP has executed (the one that says that you have been it p1
position). The TRAP executes when the robot is in the position.
But your assignments says that you are in position p5.
The pMirror values will be incorrect in this case.
You will get this behavior when using short movements and high speed.
If using longer movements, you can see that the program pointer is one instruction
before the motion pointer for a short time.
 
Use a counter in your program that is increased between each TriggL. Then you can
have another counter in the TRAP. Execute the program and do TPWrite on the counters after
you increase them. You will probably see a difference between them.

The program I used to check that the TRAP is executed right is the one below.
I run it on an IRB2400 Type A. Then I compared the values logged to file with the onces
that I had in the program.

MODULE MainMoves
 
 TASK PERS tooldata tool1:=[TRUE,[[0,0,0],[1,0,0,0]],[10,[103,0,108],[1,0,0,0],0,0,0]];
 TASK PERS tooldata L10tip:=[TRUE,[[138.695,150.023,98.9783],[0.709396,-0.704707,-0.00856676,0.00851007]],[10,[103.6,-0.7,108.5],[1,0,0,0],0,0,0.12]];
 CONST jointtarget jpos10:=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST jointtarget jpos20:=[[-10.4956,10.1942,43.8338,-15.7201,-46.1733,13.6386],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget p10:=[[933.43,-156.03,885.18],[0.714349,0.00968503,0.699448,0.0196065],[-1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget p20:=[[869.38,-650.65,553.29],[0.00200615,0.711693,-0.702145,0.0219609],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget p30:=[[869.38,335.71,553.30],[0.00200546,0.711693,-0.702145,0.0219654],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget p40:=[[1200.65,335.71,553.30],[0.00200207,0.711693,-0.702145,0.0219671],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget p50:=[[1200.65,-470.31,553.29],[0.00200949,0.711694,-0.702144,0.0219611],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

 
 VAR iodev logfile;
 VAR num cnt:=1;
 
 PROC main()
  
  Open "LOGFILE.txt", logfile Write;
  MoveJSync p10, vmax, fine, tool1, "myproc";
  MoveJSync p20, vmax, z0, L10tip, "myproc";
  MoveLSync p30, vmax, z0, L10tip,  "myproc";
  MoveLSync p40, vmax, z0, L10tip,  "myproc";
  MoveLSync p50, vmax, fine, L10tip,  "myproc";

  Close logfile;
  Stop;

 ENDPROC
 
 
 PROC myproc()
  VAR robtarget myp1;
  
  myp1 := CRobT();
  Write logfile, "Pos"+ValToStr(cnt)+":"NoNewLine;
  Write logfile," [[" +ValToStr(myp1.trans)+"]"NoNewLine;
  Write logfile,"[" +ValToStr(myp1.rot)+"]"NoNewLine;
  Write logfile,"[" +ValToStr(myp1.robconf)+"]"NoNewLine;
  Write logfile,"[" +ValToStr(myp1.extax)+"]";
  cnt:=cnt+1;
  
    ENDPROC
   
ENDMODULE

leonardolt

Thank you for your reply!

 This you say about pMirror beeing pre-fetched and then corrupting the "on trap" value quite makes sense. This may be solved by storing the points in a queue and thes picking them out in the trap in a kind of FIFO. Unfortunatly I dont't know if it will be easily programmed in Rapid, I miss some memory manipulation functions in there.

 I'll try this and post here the results. By the way, I took 2 pictures of the results I has having with Stop and Fly-by points. They are from a scanning process. The robot is used to position the sensor. The robtarget feedback is used to build a map of the sensor's value in a certain area.

This first one was obtained with fine zone, and represents the correct image of the process.



This second one was obtained with fly-by points, z0. One can see that there are some lacks of points in the image and also that the spatial position is wrong.



Thanks
Leo

leonardolt

Dear Torbjorn,

 It has been some time since my last post, but now I have good news. Considering all what you said about pre-fetching the moving instructions I decided to change somethings in the program.
 I kept the TrigL instructions and the pMirror  positioning structure. The main change was in the trap roitine. Istead of sending pMirror, now I'm sendig an index. After sendig the current move index, the index is incremented inside the trap routine. Doing so I assure that this incremet calculation will only be processed at the really end of moving.
 Once all my moves are predefined in a separed module, there is no problem in recovering which position a certain index belongs to.

 Well, all this is just to say that things are doing fine now. Thanks so much.

 Leo