Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
Use PMAC to Predict the Future ?
#1
Need ideas on how to solve a application problem:

We have a output controlling some device that has a delay in its reaction of about 30msec. It needs to be adjusted proportionally with motor XY vector velocity. If we adjust the output based on CURRENT vector velocity the output will be late due to delays.

How can we "predict the future" - know what the vector vel will be in 10-40msec so we can adjust the output BEFORE we actually reach the velocity along the path. This is a basic NC type applicaition using Gcode.

Thanks,
mike
#2
Mike,

There are two ways of doing this:

First approach is using trajectory filter on X and Y axis using Ixx40 and set the filter delay for 30 msec. This will cause the commanded position to get delayed by 30 msec for X and Y axis. However, this approach introduces some deviation from defined path on X and Y which for 30msec may be tolerable, but if it isn't then we have to use the next approach.

The second approach is to use kinematics with a 30 msec lookahead buffer. Doing this, the kinematics routine will run at segmentation time, calculating the vector angle of XY and assigning the value to an M-variable which is directly commanding the slow responding device. Since the lookahead buffer was defined for 30msec, PMAC is always calculating (and feeding the M-variable), 30 msec ahead of actual move commands being sent to XY. This way, instead of predicting the "future", we delay "now" for 30 seconds which essentially has the same effect.
Sina Sattari
Chief Engineer
Delta Tau Data Systems, Inc.
#3
Very cool... I like both.
But to understand the limits...

Setting Seg time to 30 will also create rougher arcs... when cutting circles.

So this approach is pretty simple but is limited by how much delay your need and if the system smoothness needed can be maintained.

I'll discuss this with Mark and see what he thinks... thanks for the great ideas.

mike
#4
Mike,

You keep the segmentation time as low as you need for example 3 msec. But you set the lookahead buffer for 30msec which means

Isx20=30msec/Isx13=30/3=10

So this will not affect the segmentation or the quality of interpolations at all.
Sina Sattari
Chief Engineer
Delta Tau Data Systems, Inc.
#5
Yes... awesome

But don't we set Isx20=10, Isx13=3 and this means we are in the Kin buffer every 10 seg periods x 3 msec per seg time period or every 30 msec ?
#6
Mike,

The inverse kinematic routine is executed for every segmentation time, so you will have 10 updates within 30msec window on your M-variable, but the commanded positions for the motors P1, P2 and ... will be stored in the lookahead buffer and then will be sent to motors as commanded position. This way your M-variable was updated 30-msec ahead of the move happening on the XY axis.
Sina Sattari
Chief Engineer
Delta Tau Data Systems, Inc.
#7
Here I just wanted to post the result of working with Sina on this. I also documented this useage of the Lookahead and Kinematic features of the PMAC in the Thread called "PMAC Kinematics for Dummies". You can download the attached document for more details if needed.

In your inverse kinematics routine:

1. Data for X and Y comes in as Q7 and Q8.
2. Calculate the position for your corresponding motors to axis X and Y and put the count value in Pn.
3. Calculate the vector distance by doing a square root of sum of squares for current X and previous X and current Y and previous Y.


4. Calculate the vector velocity:

5. Store the calculated vector velocity into a P-array and increment the array index. You need to loop the index once you reach the maximum number of lookahead segments (40 in your example). So your code will look like something like this if you want to store the values in lets say P1001 to P1040:

P(counter)=sqrt((Q7-Q17)*(Q7-Q17)+(Q8-Q18)*(Q8-Q18))/I5113
counter=counter+1
if (counter>1040)
counter=1001
endif

Obviously the counter needs to be set to 1001 for the first time in forward kinematics also set the previous X and previous Y (Q17 and Q18) equal to first calculated values for X and Y (Q7 and Q8). In this way you would have a buffer of 40 which you can decide how far back you want to look in to calculated past.

6. Store the current Q7 and Q8 into Q17 and Q18 for next time around.

Now in an RTI PLC (PLC0 or PLCC0), you will read the counter and decide how far back in time you want to go and figure out the array index that you have to read. Lets say you want to look back in time for 30 msec, which at a segmentation time of 2 msec, you are looking back 15 segments from current segment and assign the value to an output M-variable (Lets say a DAC). So your PLC0 logic should be:

timeindex = counter – 15
if (timeindex<1001)
timeindex=timeindex+40
endif
DACoutMvar=P(timeindex)


And this way every RTI your DAC gets updated to 15 lookahead segments from what the commanded position to the motor will be.
#8
Mike,

Actually a better solution:

In order not to deal with synchronizing the PLC/open servo with lookahead buffer and let kinematics take care of everything we can do this:

General idea: in the previous solution, we were buffering the output command to DAC. In this approach we will buffer the desired X and Y!!!


Here is how to do it:

1. In forward kinematics, you simply calculate the X and Y (Q7 and Q8) based upon your motor positions (P1 and P2) and assign the same calculated values to Q17 and Q18 as previous X and Y.

2. In inverse kinematics, you will buffer the desired X and Y (Q7 and Q8) for length of time shift you need on your slow responding axis. (if you need 30 msec advanced command on your DAC output with a segmentation time of 2msec, you need a buffer of 15 for X and a buffer of 15 for Y).

Code:
TimeShiftSegmentCounts=15

OPEN FORWARD CLEAR
Q7=P1/Mtr1ScaleFactor;
Q8=P2/Mtr2ScaleFactor;
Q17=Q7;
Q18=Q8;
IndexCounter=0;
CLOSE

OPEN INVERSE CLEAR
IndexCounter=IndexCounter+1;
If (IndexCounter > TimeShiftSegmentCounts)
      IndexCounter=1;
EndIf
Q(700+IndexCounter)=Q7;       //using Q700s for buffering X
Q(800+IndexCounter)=Q8;       //using Q800s for buffering Y
VectorVelocity= sqrt((Q7-Q17)*(Q7-Q17)+(Q8-Q18)*(Q8-Q18))/I5113;
Q17=Q7;
Q18=Q8;

TimeShiftIndex = IndexCounter + 1;  //oldest buffer is always in the next slot
If (TimeShiftIndex > TimeShiftSegmentCounts)    //wrap-around
      TimeShiftIndex = 1;
Endif

// Calculating the commanded position for the motors

P1=Q(700+ TimeShiftIndex)* Mtr1ScaleFactor;
P2=Q(800+ TimeShiftIndex)* Mtr2ScaleFactor;
P3= VectorVelocity * DACScaleFactor;

CLOSE

3. Now you setup motor 3, such that the servo command (Ix02) is pointing to your DAC with the following as your PID gains. All you need to do is to issue a J/ on this motor before running the program and define all of the motors, including the DAC motor, as kinematics motors in your coordinate system.

Code:
I300=1
I302=$78012
I324=$120001
I303=$35C0        //pointing to empty space
I304=$35C0
I311=0

I330=5600
I331..339=0

#1->I
#2->I
#3->I

I5150=1
I5120=80    // as mush as needed
I5113=2
Sina Sattari
Chief Engineer
Delta Tau Data Systems, Inc.
#9
Yes... Brillant ! Much cleaner. Wow the Kin and Lookahead features are way cool to solve such problems. I updated by Kin Doc with your solution. Thanks....

12/14/12 Updated the PDF with some extra notes in section of PMAC Laser example concerning "Pmac Predict Future" logic.


Attached Files
.zip   KinematicsWithPmac.zip (Size: 31.32 KB / Downloads: 73)
.pdf   KinematicsWithPmac.pdf (Size: 277.26 KB / Downloads: 58)
#10
Sina, Mike,

This works perfectly in the test I've so far.

Mark
#11
(02-15-2010, 09:26 AM)kramrellim Wrote: Sina, Mike,

This works perfectly in the test I've so far.

Mark

Mark,

This is great. Please keep us updated on your progress.

Sina
Sina Sattari
Chief Engineer
Delta Tau Data Systems, Inc.


Forum Jump:


Users browsing this thread: 1 Guest(s)