Motion profile generator.

[HenrikOlsson]

Hi everyone,
I'm looking for help and advice on implementing a basic motion profile generator in PBP. Ideal would be 3rd order one (linear acceleration, S-curved velocity) but the simpler 2nd order (trapezoidal acceleration, triangular velocity) proved to be hard enough to get right so I'll settle for that to begin with. Here's a crude drawing illustrating what that type of motion profile looks like:

6829

I'm trying to base my version on Microchips AN532 (http://ww1.microchip.com/downloads/en/AppNotes/00532c.pdf), the profile generator is covered in detail in appendix E. But I'm obviously running into trouble or I wouldn't be here seeking advise.

Without looking at any code lets just look at the theory.

I have a timer interrupt running, providing a system tick (the frequency doesn't matter here)
I have a Position variable which tells the feedback loop where the motor is supposed to be at any given time. The feedback loop plays no actual role here.
I have a Target variable which is to where I want the motor to move.
I have an acceleration value (A_Max in the above drawing), lets say this value is 10
I have a Velocity value which is the desired motor velocity in encoder counts per system tick at any given time.
I have a max velocity (V_Max in the above drawing) which is the velocity at which I want the motor to move. Lets say this value is 100


Now lets look at how easy it is to ramp up or down the motor speed. Initially Velocity is 0:
Each tick the value of A_Max gets added to the value Velocity. If Velocity > V_Max THEN Velocity = V_Max. The value of Velocity is added to the Position variable.
This will ramp up the motor from 0 to V_Max and then keep it running at that velocity. Ramping it down is obviosuly equally simple, just subtract A_Max from Velocity and Velocity from Position until Velocity is 0.

What I'm struggling with is to make it stop at the Target position.

In the Microchip implementation they begin by dividing the total distance of the move by 2 and store that value. Then they start to accelerate the motor. Now one of two things can happen:
1) Half the distance is covered before reaching V_Max in which case the motor is decelerated.
2) The velocity reaches and possibly gets clamped at V_Max before half of the distance is covered.

In (2) above they count how many ticks the velocity is at V_Max and when half the distance is covered they start subtracting from that count, one tick at the time. When the count is 0 they decelerate the motor. Pretty simple.

If we do a move of 2000 with A_Max=10 and V_Max=100 then it would look something like this:

Acc Vel Pos
0 0 0
10 10 10
10 20 30
10 30 60
10 40 100
10 50 150
10 60 210
10 70 280
10 80 360
10 90 450
0 100 550 <- At V_Max (1)
0 100 650 <- At V_Max (2)
0 100 750 <- At V_Max (3)
0 100 850 <- At V_Max (4)
0 100 950 <- At V_Max (5)
0 100 1050 <- Half distance
0 100 1150 <- At V_Max (4)
0 100 1250 <- At V_Max (3)
0 100 1350 <- At V_Max (2)
0 100 1450 <- At V_Max (1)
0 100 1550 <- At V_Max (0)
-10 90 1640
-10 80 1720
-10 70 1790
-10 60 1850
-10 50 1900
-10 40 1940
-10 30 1970
-10 20 1990
-10 10 2000


Perfect! This works because everything is dividing nicely into everything else. But what happens when, for example, A_Max doesn't divide evenly into V_Max?



Acc Vel Pos
0 0 0
15 15 15
15 30 45
15 45 90
15 60 150
15 75 225
15 90 315
15 100 415 <- At V_Max (1)
0 100 515 <- At V_Max (2)
0 100 615 <- At V_Max (3)
0 100 715 <- At V_Max (4)
0 100 815 <- At V_Max (5)
0 100 915 <- At V_Max (6)
0 100 1015 <- Half distance
0 100 1115 <- At V_Max (5)
0 100 1215 <- At V_Max (4)
0 100 1315 <- At V_Max (3)
0 100 1415 <- At V_Max (2)
0 100 1515 <- At V_Max (1)
0 100 1615 <- At V_Max (0)
-15 85 1700
-15 70 1770
-15 55 1825
-15 40 1865
-15 25 1890
-15 10 1900
-15 0 1900


Here we end up 100 counts short of the target. I understand WHY it's happening but I can't seem to figure out a good fix for it.

In the Microchip application note they seem to somehow determine which is resulting in the smallest error, one more or one less "tick" at V_Max. They then, obviously, opt for the one resulting in the least amount of error at the end of the profile and then they're simply "bumping" the motor to the CORRECT target position at the end of the move.

I really can't have that - I need it to ramp down and end up exactly AT the target position.

If anyone's got any ideas, pointers, suggestions or codesnippets I'd really appreciate the input, thanks!

/Henrik.

[Darrel Taylor]

I think the obvious answer is to use floating point (http://melabs.com/resources/fp.htm) math ...
But then the frequency of your interrupts may become more important. How fast are they?

Or maybe you could take the remainder of the division and add it in a separate variable on each step, and when that overflows add one to Vel.
When ramping down, subtract 1 on an underflow instead.
It's kind of like floating point, as long as you're just adding and subtracting 1 unit each time.
It's harder to do than the actual floating point, but is probably faster.

[HenrikOlsson]

Thanks Darrel,
The interrupt rate varies depending on application but up to 2.5kHz.

I guess by 'the division' you mean V_Max / A_Max ? Or do you mean Distance/2 (I don't think so since the reminder will either be 0 or 1)?
So, in the previous, non ideal example I'd get 100//15 = 10.

From there I don't quite follow... If I keep adding 10 to a BYTE variable it'll overflow on the 26th tick which is right at the very end of the profile in that previous example, that can't be right. On the other hand, for the acceleration step:

Accumulator = Accumulator + Reminder
If Accumulator >= A_Max THEN
Accumulator = Accumulator - A_Max
Velocity = Velocity + 1
END IF
Is that in line with what you're thinking?

I need to sleep on this I think. Thanks again!

/Henrik.

[Darrel Taylor]

Well, I was thinking more of the decimal remainder than the modulas remainder.

But in trying to recreate the code, I'm seeing some big limitations in the theory.
The only way to end up at the exact Target Position, is to have a Position that is an exact multiple of V_Max.

With V_Max = 100, you can only get target positions of 1800, 1900, 2000, 2100 etc.
I would think you would want more precise targets ...

Currently, I can get +/- 5 counts from those targets by using the decimal remainder idea.
I think I can change it to reach any target position, but then I see more problems down the road.

This only ramps from 0 to a target.
It won't go from target to target.
Unless there's some other steps in the process that I'm missing.

Here's some output from the test program.
If I can figure the rest out, I'll post the code.

1 = Edit Max_Acceleration (10)
2 = Edit Max_Velocity (100)
3 = Edit TargetPosition (2000)
Enter = GO
>

0 0 0
10 10 10
10 20 30
10 30 60
10 40 100
10 50 150
10 60 210
10 70 280
10 80 360
10 90 450
10 100 550
10 100 650
10 100 750
10 100 850
10 100 950
10 100 1050
10 100 1150
10 100 1250
10 100 1350
10 100 1450
10 100 1550
10 90 1640
10 80 1720
10 70 1790
10 60 1850
10 50 1900
10 40 1940
10 30 1970
10 20 1990
10 10 2000
10 0 2000

1 = Edit Max_Acceleration (10)
2 = Edit Max_Velocity (100)
3 = Edit TargetPosition (2000)
Enter = GO
>1
Enter new Max_Acceleration (10):15

Max_Acceleration = 15

1 = Edit Max_Acceleration (15)
2 = Edit Max_Velocity (100)
3 = Edit TargetPosition (2000)
Enter = GO
>

0 0 0
14 14 14
14 28 42
14 42 84
14 57 141
14 71 212
14 85 297
14 99 396
14 100 496
14 100 596
14 100 696
14 100 796
14 100 896
14 100 996
14 100 1096
14 100 1196
14 100 1296
14 100 1396
14 100 1496
14 100 1596
14 100 1696
14 86 1782
14 72 1854
14 58 1912
14 43 1955
14 29 1984
14 15 1999
14 0 1999

1 = Edit Max_Acceleration (15)
2 = Edit Max_Velocity (100)
3 = Edit TargetPosition (2000)
Enter = GO
>3
Enter new TargetPosition (2000):1800

TargetPosition = 1800

1 = Edit Max_Acceleration (15)
2 = Edit Max_Velocity (100)
3 = Edit TargetPosition (1800)
Enter = GO
>

0 0 0
14 14 14
14 28 42
14 42 84
14 57 141
14 71 212
14 85 297
14 99 396
14 100 496
14 100 596
14 100 696
14 100 796
14 100 896
14 100 996
14 100 1096
14 100 1196
14 100 1296
14 100 1396
14 100 1496
14 86 1582
14 72 1654
14 57 1711
14 43 1754
14 29 1783
14 14 1797
14 0 1797

1 = Edit Max_Acceleration (15)
2 = Edit Max_Velocity (100)
3 = Edit TargetPosition (1800)
Enter = GO
>2
Enter new Max_Velocity (100):90

Max_Velocity = 90

1 = Edit Max_Acceleration (15)
2 = Edit Max_Velocity (90)
3 = Edit TargetPosition (1800)
Enter = GO
>

0 0 0
15 15 15
15 30 45
15 45 90
15 60 150
15 75 225
15 90 315
15 90 405
15 90 495
15 90 585
15 90 675
15 90 765
15 90 855
15 90 945
15 90 1035
15 90 1125
15 90 1215
15 90 1305
15 90 1395
15 90 1485
15 90 1575
15 75 1650
15 60 1710
15 45 1755
15 30 1785
15 15 1800
15 0 1800

1 = Edit Max_Acceleration (15)
2 = Edit Max_Velocity (90)
3 = Edit TargetPosition (1800)
Enter = GO
>

I'm just going from your description, I haven't tried to convert the original code.

My questions:

Is it supposed to be able to move to any target position? Not just a multiple of V_Max?
Is it supposed to move from any position to any other position?
Or is that part handled in your routines? And this is just a "profile" that gets mapped onto the movement?

[HenrikOlsson]

Hi Darrel,
Thank you very much for spending time on this, I appreciate it!
I guess +/-5 isn't TOO bad, always ending up short of target would be better than overshooting, then I could "creep" the position the last couple of counts but obviously ending up right on the spot was/is the goal.

Is it supposed to be able to move to any target position? Not just a multiple of V_Max?
Yes, absolutely. Any arbitrary position.

Is it supposed to move from any position to any other position?
Yes, absolutely. From one arbitrary position to another, in both directions. But I guess you can think the profile generator as moving from 0 to any position by simply taking the "output-position" of the profile generator and add that to the actual position variable. Ie the profile generator creates a relative position output which is then added to the real position variable. Hope that makes sense.

Or is that part handled in your routines? And this is just a "profile" that gets mapped onto the movement?
Not sure I understand, or perhaps the answer to the previous question covers it (reading it again I think it did)....
Anyway, the feedback loop moves the motor to whatever position is put in the Position variable. But I need to "move" the target position at a speed that the motor and load can follow in real life. Simply stepping the position variable +/-12345 counts (or whatever) is very hard on the motor and for the PID-loop to handle without overshooting etc. It can be handled by de-tuning the filter to less than optimal response but who wants that.

The best way to move from one position to another is to "move" the target position at a pace that the motor can actually follow. Ie ramp it up, keep at speed, ramp it down. Or, if the move is short, ramp up and at half the distance ramp down. Ideally it should be done with a 3rd order motion profile to reduce the jerk at the beginning and end of the acceleration phase. Ie you're not only ramping up/down the velocity but your also ramping up/down the acceleration creating an S-curved velocity profile instead of one with linear "sloped". But like I said, the 2nd order one proved hard enough.

Again, thanks!

/Henrik.

[Darrel Taylor]

>Thank you very much for spending time on this, I appreciate it!
You help everyone out too, it's nice to be able to do something for you for a change.

Thanks for the explanations, now I have a better understanding of what it needs to do.

Although the idea of just splitting the profile in half and doing the exact same thing in reverse for the second half was interesting ... that's what's causing it to only move to multiples of V_Max. I need to change that.

And I need to make it go both directions, it only goes forward right now.

I hope to have something by the time you wake up in the morning.
Sometimes being on the other side of the planet is helpful. :)

[HenrikOlsson]

Thanks Darrel!

Actually, I don't think you should need to worry about the direction. Perhaps I misunderstood your earlier question and overcomplicated my explaination after all.

How about this:
* The "global position variable" is the expected motor position at any given time. The PID loop servos the motor to this position.
* The "relative position variable" is the output of the profile generator, exactly as you shown earlier, always starting a new move at 0.
* When a new move is commanded it is easy to calculate if the target position is positive or negative relative to the current "global position".
* A profile, starting at 0 and ending at ABS(Target Position - Global Position) is then generated.
* Depending on the direction or "polarity" the output of the profile generator is added or subtracted from the Global Position variable one "tick" at a time.
* The PID loop makes the motor follow the Global Position variable very smoothly and everyone's happy, especially me :-)

Does that make sense?

/Henrik.

EDIT: Put another way... Acceleration is added to Velocity. Velocity is added to RelativePosition. RelativePosition is added to OR subtracted from GlobalPosition.

[amgen]

H,
Ditto on you always helping everyone. If this isn't too elementary relating to where your at.......

on de-celleration when V_Max reaches to 0...zero, recalculate the ACC (will be de-celerate) val.
if you want the same amount of iterations to de-cel as used to acelerate, at A=10 then 100/10 for 10 loops or A=15 for 100/15 for 6? or 7? loops..............
then subtract present pos (@ V_MAX=0) from desired pos. Thats the actual steps to stopping ??
now use sum of # loops...( sum=loops(loops+1)/2) is new ACC val to loop to 0 (zero) over that many loops (should be +- 1 or 2 from the starting ACC val)
now there will be either 0 or some amount less than A as a final step to stop.
should be a few more lines of code to test and calc.
I think Darrel mentioned re-calc something.
don

[Darrel Taylor]

Maaaannnnn ... This is harder than it looks.

Well, I have it going to the exact target position with zero error ... as long as it's able to reach the maximum velocity (the flat part of the profile).

But if the target position isn't far enough away, and it doesn't reach V_Max before it has to decelerate ... it overshoots the center, and with the maximum deceleration (A_Max) ... the velocity can't reach 0 until it's past the target. Arrrrrggg.

So on the shorter distances, I'll have to stop accelerating before it gets half way there, and calculate a new slope for the second half so that it stays under the maximum deceleration and gets the velocity to 0 before or at the target position. This way will make it decelerate slightly slower than it accelerates. Probably not noticable though.

Need more time.

[HenrikOlsson]

Darrel,
What, you need more time? Unacceptable, I need a working solution NOW ;-)
Seriously, take all the time you need, I appreciate any and all help!

I know, I found it quite a bit harder....

Since I don't know how you're actually aproaching this I'm just thinking out loud:
On the short moves I'm thinking how it would work out if the distance was simply cut in half and then the actual acceleration value tweaked to fit so we'd hit right on the centermark while accelerating. Then start decelerating at the same slope would make us end up at the target, right? I wonder what the worst case deviation from the set acceleration value would be... Code size and speed is obviosuly of interest as well...

I'm looking forward to see what you come up with!

Don,
Perhaps Darrels aproach is exactly what you're trying to explain but I must I admit I don't quite follow what you're trying to explain.
If an error at the end of the move of alwas less than A_Max is the best we can get then so be it but it sounds like Darrel is really onto something.

Thank you both!
/Henrik.

[amgen]

Henrik,
While the professional is working up the best scheme.

Question..... on the last itteration to stopping, can you subtract 'position now' from 'position desired' ?
and if so, can you move exact steps of the difference with the A value?

Don

[HenrikOlsson]

Hi Don,
If it always ends up short of the actual target AND the remaining distance TO the actual target is always less than A_Max then I could probably live with that. It means that it would always reach the actual target within a single "tick" from optimal.
However, I'd have to move it a speed equal to the actual distance to target, if moved at A_Max then it would overshoot in one tick - which in turn gives it a little bit of "S" at the end reducing the jerk. Might actually be good.

Thanks!
/Henrik.

[aratti]

Surely I will say something obvious, but since I did not noticed it in the above post, let me say it.

When the travel distance is less than twice the accelleration the trapezoidal profile become triangular and V max decrease, so that Vmax = tan(accelleration Slope) time the travel distance / 2

Cheers

Al.

[Darrel Taylor]

Here's what I'm up against ...

You can only adjust the acceleration.
I don't see any way to know what acceleration is required to make the half-way point end up exactly at an interrupt period.
It always ends up somewhere in the middle of a period.

If I new what the velocity would be at the halfway point, I could adjust the acceleration so it reaches the half-way point at exactly an interrupt period.
Except that since the acceleration changed, it won't be at that velocity at the half-way point anymore.
So I don't see any way of predicting the half-way point in order to use the same deceleration slope it uses for acceleration (the yellow line).

I can only change the acceleration at specific times, which coincide with the interrupt periods.
So if I recalculate the deceleration slope at period 6 before it reaches the half-way point, it creates the green line, which should end up at the target perfectly.

If I were to recalculate it after passing the half-way point, the deceleration rate would be higher, and violate the A_Max constraint.

The blue line is the overshooting profile, with it's velocity using the left axis.
The red line is the position, using the right axis.
The parameters for that run were ... A_Max=10, V_Max=100, Target=475.

http://support.melabs.com/DT/Decelerate.gif

I'm doing the green line.
But since the green line is actually a slower velocity, it will take longer than shown in the chart to reach the target.
But at least it is calculatable.

[timmers]

It is all relative to time. One thing not concidered yet is what to do if the servo has not achieved the demand. Do you run the servo faster/slower (velocity), harder/softer rate of change (acceleration & deceleration), longer/shorter (time) or forwards/back (position) to achieve the end target?

I assume you are attempting to modulate the velocity to achieve an end target at xx time whilst keeping the rate of change and max velocity within the set limits.

If you concider a velocity/time graph (trapezoid shape), with velocity as the height of the perpendicular at the current time, then the distance to travel is the area under the graph from the current position. (see graph in post #1)
The math to extract the velocity demand (in QC's / time period), at any given time, according to end target, defined limitations and servo error, is what you are trying to calculate.

Am I helping?

[timmers]

You can only adjust the acceleration.
I don't see any way to know what acceleration is required to make the half-way point end up exactly at an interrupt period.
It always ends up somewhere in the middle of a period.

Realistically, there will be thousands of intervals, the error will be tiny. We use an interval of 2mS and get a QC count of 1 at 15 RPM to 680 at 10,000 RPM. (using a 512 line encoded motor)
I would suggest to leave the acceleration at the demand rate, and work out when to stop accelerating and run at a constant speed or to decelerate in time to hit the target.

[Darrel Taylor]

Timmers,

You are talking about the control that Henrik is using. I'm sure the intervals are much smaller, and it includes a PID loop with an encoder feedback.
My task is to generate a "Profile" for that control to follow. A much different situation.

[Darrel Taylor]

Henrik,

I think i have it all working now.
It's really ugly and needs to be cleaned up.

But before I do that tomorrow, I have one last question.
Would adding an "Ease From" and "Ease To" function to the beginning and end of the profile suffice for the 3rd order curve thought?

[HenrikOlsson]

Hi guys,

First of all, for clarity:
The PID-loop (http://www.picbasic.co.uk/forum/showthread.php?t=5874) is already in place. It servos the motor to the position given to it and does whatever it can to bring the motor to the commanded based P,I,D and feedforward parameters. If the error between the target position and actual position gets above a setable limit the loop faults and aborts. This is one of the reasons for needing the profile generator - I can't just ask the PID-loop to move the motor 10000 QCs (quadrature counts or steps or pulses or units or whatever we want to call it) because if I do an error of an equal amount (10000) will instantly develop and the loop will fault. Even if such large errors was allowed the resulting move would be less then optimal.

The profile generators job, being discussed here, is to generate input TO the PID-loop which the motor and load is actually capable of following.
The input to the profile generator can then be "stepped" 10000 QCs. It then will generate a velocity profile with the parameters given to it. Each "tick" the profile generator will give the PID-loop a new position for it to move the motor to during the next time interval. The PID-loop is executed between 500 and 2500 times per second and so will the profile generator. At least that's the idea.

Darrel,
Although I'm having some difficulties figuring out what the worst case scenario could be with the "green line approach" I don't think doing it that way should be any problem in real life. It's better to "stretch" the deceleration phase than to violate A_Max although I would prefer it symetrical but I get what you're saying - I think.

Thanks!
/Henrik.

[HenrikOlsson]

Darrel,
Missed your last post while composing mine.
You mean like a initial "step" (or steps) at a velocity less than A_Max, just to "break the edge" on the trapetzoidal/triangular profile? (If not, could you clarify?)

That could help, ideally it should be at all four "corners" of the profile but having it at the start and end of the move is certainly better than nothing.

Thanks again Darrel!
/Henrik.

[Darrel Taylor]

Ok Henrik, here's what I came up with.

To start a movement, set the three variables and call the ProfileStart subroutine.



MaxAcceleration = 10 ; starting values
MaxVelocity = 100
TargetPosition = 2000
GOSUB ProfileStart


Then for each period, call the ProfileStep subroutine, and the variables ... ThisAccel, Velocity and Position will be updated.
When the TargetPosition has been reached, the ProfileComplete flag will be set.

For the above values, the output of the test program looks like this ...
Like your original example, the first column is Acceleration change, second column Velocity, third column Position.
1 = Edit MaxAcceleration (10)
2 = Edit MaxVelocity (100)
3 = Edit TargetPosition (2000)
Enter = GO
>

0 0 0
10 10 10
10 20 30
10 30 60
10 40 100
10 50 150
10 60 210
10 70 280
10 80 360
10 90 450
10 100 550
0 100 650
0 100 750
0 100 850
0 100 950
0 100 1050
0 100 1150
0 100 1250
0 100 1350
0 100 1450
0 100 1550
-10 90 1640
-10 80 1720
-10 70 1790
-10 60 1850
-10 50 1900
-10 40 1940
-10 30 1970
-10 20 1990
-10 10 2000
-10 0 2000

1 = Edit MaxAcceleration (10)
2 = Edit MaxVelocity (100)
3 = Edit TargetPosition (2000)
Enter = GO
>
I quickly tried the "Ease From" and "Ease To", but it messed up the final position so i pulled it back out.
I'll have to think about it some more.

The MaxAcceleration variable will get changed on small moves.
So you should keep a separate variable and copy it to MaxAcceleration before starting a profile.

Hopefully, this is somewhere close to what you were looking for.
Try the test program, and as many combinations of the settings as you can first.
I'd hate to ruin your CNC. :eek:

If you can follow the code in the include file, and it makes sense ...
You should see a psychiatrist as soon as possible. :)

[timmers]

Henrik, I now understand what you are trying to do.

We use our motor control for velocity regulation because we need smooth moves in our remote cameras. When we need to go to a QC position, we increment/decrement the velocity demand to achieve the target. Running it in a positional regulation tends to be quite "notchy" in our application, but I will try it and let you know how it goes.

Tim.

[HenrikOlsson]

Darrel,
Thanks a lot!
Don't worry about ruining anything, this is for a stand alone application not related to my CNC machines. (For that you need multiple axis interpolating, all with different accelerations and velocity constraints - up for another challenge?).

I'll take a closer look at it and run it thru its paces over the next few days. Really, thank you!

Tim,
Exactly, the PID-loop servos the motor to a position command. Giving it a new target position without any sort of profile doesn't work very well.
To run at a certain velocity I make the motor "chase" an ever changing target position.

Did you develop the code for the motion controllers you mention in PBP? Any links or information, I'd love to see them.

/Henrik.

[timmers]

Henrik,
We work entirely in PBP. For the top end motor control and move profiling we currently use the Maxon EPOS2 controller, but we have better control at the very slow speeds than Maxon. We are moving to do all our own control including the gyro products.

PM message sent.

[Darrel Taylor]

(For that you need multiple axis interpolating, all with different accelerations and velocity constraints - up for another challenge?).

Sure, but it will take several months, many thousands of dollars, and a trip to Sweden. :)
Let me know when you want to start. :D

[HenrikOlsson]

Hi Darrel,
I've played around with the profile generator code and it's very cool. Unfortunately I won't pretend to understand half of it.

There are a couple of issues, one of which I think is a bug.
If you can, do a move with A=7, V=23, T=12345 - what do you get? When I do that it accelerates properly, runs at a velocity of 23 for a while then decides to decelerate to a velocity of 5 and finishes the move at that velocity. If I change to A=7, V=25, T=12345 it's fine but it does do similar things with other settings as well. So, I think something weird is going on there.

The other issue is the WORD size limit on the lenght of move. I've managed to change it to LONGS (changed Position, TargetPosition, RampTotal and Value) and it seems to work that way but it is SO (I mean really) much slower to execute. What seems to be taking the longest time is the step right at the "corners" of the profile, I've seen times above 2500 cycles. I'm not sure there's anything that can be done to make it better but wanted to run it by you in case you may have some ideas.

The last thing is one which I knew would come up from the start but I honestly couldn't imagine the code for this having to be THIS complicated and thought I'd be able to handle it outside of the actual profile generator but now I may need your input on what effects it will have on the profile generator code. The issue is resolution, or rather lack of resolution.

Imagine a 3000rpm motor with a 500 line encoder giving 2000 counts per revolution. The servo-loop needs to run at ~1000Hz which means that lowest speed I can command is 1 count per interrupt (1000 counts per second) or 30rpm - not that great. And, even at the lowest possible acceleration of 1 count/tick/tick it'll still go from 0 to 3000rpm in 100ms.

One possible solution might be to run the profile generator maby one in every ten servo-loop ticks. That would make it better but still pretty crude. Slower than that and it would be noticable I think.

My original idea, which I've used before and thought I could use here as well is to use something like this:


Scratchpad = Scratchpad + VelocityCmd.LowByte 'Accumulate scratchpad...

If ABS (ScratchPad) > 127 then ' If it's time to move 1 count...
Scratchpad = Scratchpad - 256 ' reset the acumulator and...
IF Direction = FWD THEN
Setpoint = Setpoint + 1
ELSE
SetPoint = SetPoint – 1
ENDIF
ENDIF

IF Direction = FWD THEN
SetPoint = Setpoint + VelocityCmd.Highbyte
Else
SetPoint = Setpoint – VelocityCmd.HighByte
Endif

This allows me to set the velocity in steps of 1/256 counts per tick which is good enough and the idea was to use something similar for the acceleration. But, as I said, the profile generator code turned out to be much more complicated than I imagined and now I'm not sure I can do it without messing up the generator completely. I'm thinking I can make the velocity and acceleration variables WORD-sized and then handle the rest outside of the profile generator. What do you think, do you see any major issues with that?

Sorry for the lengthy post and thanks again for all your help!
/Henrik.

[Darrel Taylor]

Yeah, I guess you could call that a bug.
I wasn't planning on such long low speed moves.

I can probably fix it, but it doesn't sound like this is really what you want.
If it has to use Longs, and needs to run faster ... the time it takes my approach to process each step will never meet your requirements.

Maybe a different approach is in order.
But I don't know what that would be.

Sorry.

[HenrikOlsson]

Hi Darrel,
Now I feel really bad.
It's not that it doesn't do what I want. It does exactly what I want just not under all conditions. Conditions which are a given for me but definitely not for you and now you've wasted a lot of time on something just because I didn't manage to explain the application in enough detail. I'm the one who should say I'm sorry.

I keep finding articles and application notes on acceleration profiles and positioning but they all seam to targeting step motor application where you obviously move the motor only one step per "tick" so you can easily "hit" the exact centerpoint and deceleration point of the move. Not so here....

I will keep dwelling on this and trying a couple of things. I keep thinking that perhaps I could live with the acceleration constraint being violated (to some extent) on deceleration, what I can not accept is not overshooting the target.

Thanks!
/Henrik.

[richard]

henrik
did you ever get this working ?
I have been messing with this for years and think I have finally worked it out . my solution is in C code but the idea is probably fairly portable if your interested

[HenrikOlsson]

Hi Richard,
I have been working on it on and off but have yet to actually try it on real hardware. It's been over 6 months since I last looked at the code but the way I left it was that it figured out when it needed to start decelerating (which could be up to VMax past the ideal position) and then recalculating the needed deceleration value each tick untill ending up at the target. Basically this aproach WILL cause the decleration to be "steeper" than the acceleration which I know I said I don't want but in reallity I don't Think it'll matter that much, I mean, in simulation where a "move" is 100 "ticks" in total it looks "brutal" but in reallity I don't Think it'll be that bad.

I have NO idea why you write in C (just kidding) but I'd love to see your code, thanks for offering!

/Henrik.

[richard]

my basic premise is that the input to the pid filter is velocity error (not position error) . the required velocity (setpoint) is found from a lookup tables who's in put is based on position . The move routine has 5 possible modes 4 is start a new move , 3 accelerate ,2 steady speed (cruise) ,1 decelerate, 0 in position , -1 stalled (failure).
The lookup table was developed by - getting motor running at various velocities and throwing the brakes on (l393d style) and measuring the number of ticks travelled till at rest ,
I plotted these in a spread sheet (the motor I'm using is a 171:1 gear motor 564 ticks per rev, target vel is 32 rpm ) interestingly the pos vs vel curve is ver nearly parabolic.
the clever bit is using two lookup tables "the multi_map function" to interpolate the best velocity for the current position using only 9 entries in each table

[richard]

some debug runs

[richard]

beginning to think this may never fly in pbp
I can get various versions to work in C using integer math , but with a real servo motor with 2000 counts /rev the numbers get out of control.
although it seems to be workable using a 16f1825 @32mhz using C with floats.
however using toy servos like the polou gearmotors at modest speeds and a nice faulhaber (probably not a toy ) I found on ebay its probably doable in pbp certainly works with C integer math even limited to 16 bit vars
the pid refresh rate is critical nothing less than 10ms will work for real servos, up to 100ms can work for the others
I have attached a xlsm spread sheet to show lookup table ideas

[richard]

funny I have been wondering why I can get this stuff to work on a atmega328 @16mkz clk without too much trouble , but with a pic18f45k40 @64mhz clk the performance is patchy
then I found this link
http://www.t4f.org/articles/optimization-of-microchip-pic-xc8-compiler-in-free-and-pro-mode/
i'm staggered it's as if microchip don't want hobbyists to use their products
what amateur wants to pay $995 us for a complier ,why even have a free option that's so badly compromised
I'm considering changing camp to atmel

[richard]

henrik

best I have come up with is assuming your time intervals are constant :-
distance is the lessor of (normal full accel ramp ) or travel /2
max velocity is the lessor of sqr( 2*accel*distance/2) or vmax

next position = (current velocity + accel/2)+current position .
while pos+next position<= dist/2 . from then on either constant v or decelerate mode
I'm assuming pid routine can limit vel to vmax

question here is what to do with the error and what is the error is it failure to reach next position or failure to achieve target velocity ?.

it must fit into a lookup table somehow that way you could compensate for errors

still thinking

[HenrikOlsson]

Hi Richard,
Sorry for the late reply, been a bit busy here.
I thought about commenting on the issue with C compilers and the whole AVR vs PIC etc but I don't write code in C and I don't use AVRs so I simply won't.

I don't think the aproach with a lookup table for velocity is the way to go in my particular case. I can see it working in a specific application where you can tweak its value etc but what I want is a general purpose routine. I'm just starting to get back into this, setting up a small testbed with a little servo motor, 500 line encoder and LMD18200 H-bridge driver chip.

My aproach, as it currently stands is based on around a basic state machine with four states - Stopped, accelerating, traversing, decelerating. Timing is very consistent, being interrupt driven, calls to the PID routine is currently at 1000-2500Hz (user selectable) but I think I need to allow a divisor between it and the profile generator.Basically, here's how it works:

I've got:
[Desired velocity] <- Set by user
[Desired position] <- Set by user
[Desired acceleration] <- Set by user

[Output velocity]
[Output position]
[Output acceleration]

On the very first call to the state machine when a new move is about to begin variables are initialized and [Desired position] is recalculated into a relative distance from the current position of the motor.

In the acceleration state [Output velocity] is increased by [Desired acceleration]. Then [Output position] is increased or decreased by [Output velocity].
If [Output velocity] is equal to or larger than [Desired velocity] it's clamped (before manipulating [Output position] ), the traveled distance is stored and the state is changed to traversing.
If [Output position] is equal to or larger than [Desired position] / 2 the state is changed to decelerate.

In the traversing state [Output position] is increased or decreased by the set velocity. If the output position is equal to or less than [Desired position] - "distance traveled during acceleration" state is changed to decelerate.

In the deceleration state the [Output acceleration] is recalculated each tick based on [Output acceleration] = [Output velocity] ^ 2 / 2d where d is the distance left to target. [Output acceleration] is subtracted from [Output velocity] which is added to or subtracted from [Output position].

Now, this will, most of the time, violate [Desired acceleration] when decelerating which I initially said was unacceptable. However, given the fact that the motor position is commanded and sampled at discrete time intervals and is allowed to move an arbitrary amount of counts (any velocity) I simple can't see a way to make it accelerate/decelerate with set rate, run at speed and end up at exactly the desired position - all at once. It's not perfect but just like timmers said early on the deceleration will likely be stretched out over hundreds or even thousands of ticks so in real life it might not even be noticable.

Now I just need to get that little testbed up and running....

/Henrik.

[richard]

I have noticed another problem with my methods ,in that I'm calculating velocity by subtracting pos from old postion in the pid loop. when under acceleration this is actually average velocity not the actual (final)
velocity . I have not been successful in determining velocity without impinging on pid evaluation time.
using floating point routines with a 8bit cpu the highest pid rate I can achieve is 250 hz so average velocity is not good enough. might mean fp pid is not worth pursuing
general purpose might be a tall order . I have a leadshine dcs303 digital dc servo driver , it runs a dcm50202 servo motor / 500line en with no problems (can achieve 3000 rpm and move to pos +- 1 count) . It will not even come close to achieving servo lock on the other motors I'm using. my other motors have a really high inertia this makes them more predictable for my "slow" routines providing you don't try to exceed their accel profiles. the low inertia servo is problematic

[Ioannis]

I am not an expert on this subject but would try to help. Is it necessary to use the PID routine all the time?

Why not just call it just before the target point is close so that it will be used less and everything will be running much faster?

Maybe I am away from what is discussed here... Just a thought.

Ioannis

[HenrikOlsson]

Richard,
Yes, I think floating point is going to take too much resources and I'm not sure it'll actually solve anything. The position is still "set" and sampled at discrete intervals, there's no way to force it to "hit" a specific position at a discrete interval while still allowing "any" velocity and "any" acceleration. If target position is NOT to be violated then something else will have to give. Either change velocity so deceleration can start at exactly the correct spot. Or change deceleration so the position will end up at the correct spot once velocity reaches zero. I'm opting for the second....

I have successfully used my PID code with step and direction input running a motor with a 3600 line encoder at 4000rpm (960kHz step frequency), it'll position to 1 count without problem. The PID is executed at 1500Hz with processing power to spare. Here it is: see it here (http://www.youtube.com/watch?v=U-jrxK7u_34),

for more details see my webpage. (http://www.henriksplace.se/servo/index.html) But it's being commanded by an external motion control software which is what generates the motion profile. The DCS303 manual mentions built in motion controller for self thest with trapezoidal velocity profile which is exactly what I'm trying to achieve. Unfortunately the DCS303 manual doesn't say much about how its profile generator works and/or which set parameters it may "violate" (if any) to achieve its goal.

Ioannis,
YMMV of course but in my case I'd say yes, it's neccesary. It's not just a matter of moving from point A to B as fast as possible, I want the motor to move from point A to point B following the profile generated by the code discussed here as closely as possible. Without the closed loop in there the motor could potentionally stall in the middle of the move.

Fun stuff this....

/Henrik.

[richard]

henrik
will this profile generator acutally "control" the motor or just output pulses/dir to another step /dir controller ?
I'm thinking in the later case you have would heaps of time to do the sums and design a profile and then just output the pulses when prepared.
to accelerate just decrement/increment the pulse interval by a fixed amount , the first and last few pulses and maybe even the transition to constant velocity could all be given special treatment. that treatment would need to be customised for motor/load/inertia as would max velocity and acceleration. bit like mach3 in a pic ?

[HenrikOlsson]

Hi Richard,

The profile generator will generate the moving target setpoint that the PID filter servos the motor to, output of PID filter goes to PCPWM module which drives the H-bridge. Everything running on the same PIC. (Step/dir signals could also provide the moving target setpoint TO the PID filter (bypassing the profile generator) but for this project I'm not interested in generating step/dir signals.)

I think that generating a step/dir based profile would actually be easier (though generating step pulses at any decent frequency isn't easy on a PIC) because then you are basically controlling the motor down to the individual step level. For each "tick" you either issue a step or you don't so it's easy to "hit" the exact spot where deceleration needs to start in order to get to the target position without violating any of the settings.

When doing the profile generation, PID, PWM and feedback in the same PIC it's not as easy since the motor is moving more than a single count for each "tick". You tell the motor to move an arbitrary amount of counts per "tick" which so you're likely to overshoot the "sweetspot" where you need to start decelerating.

I hope that makes sense.

/Henrik.

[richard]

if you were to divide an acceleration move into a number of equally timed blocks,the number of blocks is high enough to ensure that the travel (encoder clicks?) for each block is always < 255 . each block would represent a relative target position based on required acceleration ,it will obviously turn out that the final block(the remainder) would violate the strict acceleration figure but it would always result in slightly lower acceleration ,i doubt that would matter much position being the more inportant criteria.
these blocks could be stored in an byte array , the setpoint for the pid routine would be incremented/decremented by the block travel[] at the block rate , and the preformance could be monitored by checking current position vs start pos+accumulated offsets at each block increment.
as long as the block duration is > than the pid update rate and you have enough memory for a large enough array , at worst the acceleration move could be divided into variable block rate stages to make things more manageable .

[richard]

henrik how does this look ?
can the pid routine take byte sized increments

[HenrikOlsson]

Richard,
Slow down, I can't keep up at the moment! ;-)

The PID routine works with the error between the desired motor position and the actual motor position, the error magnitude it's able to handle depends on how aggressively it's tuned. Ie with low gain it'll handle large errors without overflowing the internal registers, with high gains it won't be able to handle an equally large error. But generally speaking yes, it'll handle an error of 255 without problems.

If the PID is executed 1000 times per second "stepping it" 255 counts each tick would equal a velocity of 255000 encoder counts per second. Using a 500 line encoder that equals a velocity of 7650rpm, pretty decent.

Looking at the code in your latest post I can't seem to see where you specify the distance to be moved.... (?) But that says more about my abillity to read others code than anything else :-)

I appreciate your ideas very much but at this time I don't see how precalculating the move is going to work in real life. One move might last 100ms while another 100s. One move might travel 25 counts while another might travel 100000 counts.

An example:
I'm at position 0.
* I want to go to position 4690
* I want the velocity to be 123 counts per tick
* I want the acceleration to be 8 counts/tick/tick



Tick Accel Vel Pos
1 8 8 0
2 8 16 8
3 8 24 24
4 8 32 48
5 8 40 80
6 8 48 120
7 8 56 168
8 8 64 224
9 8 72 288
10 8 80 360
11 8 88 440
12 8 96 528
13 8 104 624
14 8 112 728
15 8 120 840
16 3 123 960 <---Velocity clamped at 123

At tick 16 increasing the velocity by the set acceleration would violate the set velocity so the velocity is clamped at that point. No problems with that or the code to do it.
At this point I know that it took 16 ticks and a total distance of 960 counts to get up to speed. So all that is needed is to start decelerating 960 counts from the desired target position, ie at position 4690-960=3730.

The problem is that when moving at 123 counts/tick the target position will never be exactly 3730 at a specific tick. It'll be either 3675 at tick 38 and 3798 at tick 39:


Tick Accel Vel Pos
17 0 123 1083
18 0 123 1206
19 0 123 1329
20 0 123 1452
21 0 123 1575
22 0 123 1698
23 0 123 1821
24 0 123 1944
25 0 123 2067
26 0 123 2190
27 0 123 2313
28 0 123 2436
29 0 123 2559
30 0 132 2682
31 0 123 2814
32 0 123 2937
33 0 123 3060
34 0 123 3183
35 0 123 3306
36 0 123 3429
37 0 123 3552
38 0 123 3675 <---Should I start here?
39 123 3798 <---Or should I start here?

So, if I start decelerating (8 counts/tick/tick) at position 3675 I end up short. If start accelerating (8 counts/tick/tick) at position 3798 I end up overshooting - that's the issue with this.

What I'm currently doing in my code is detecting when the distance to target is equal to or less than the distance traveled during the acceleration phase (that would be at tick 39 above) and then recalculate the needed acceleration (deceleration) in order to end up at target. That, however, isn't a one shot kind of thing since the needed acceleration (deceleration) might be 8.324 counts/tick/tick which obviously won't work with PBP. So, next tick I recalculate again and again and again - all the way to the end. The acceleration (deceleration) will toggle between 8 and 9, something like 8,8,8,8,9,8,8,8,9,8,8,8,8,9 - perhaps.

This seems to work in simulation, I have yet to try it on real hardware. The drawback of course is it takes a lot processing power due the division involved in the math.

The above numbers are just an example. The desired acceleration can be "any" number, the desired velocity can be "any" number and distance to move can be anything between 0 and several million counts. And they can change from one move to another.

Thanks Richard!

/Henrik.

[Ioannis]

If you are, as in your example, between tick 38 and 39, you are almost on target position.

Why not change acceleration to 1 count/tick/tick sinc the drop in speed will not be noticed and you will get the exact steps you need?

I am not an expert on this, just droped my idea.

Ioannis

[HenrikOlsson]

Hi,
I'm definitely no expert either, hence this thread ;-)

I'm not sure I follow your reasoning though. Where should I change acceleration to 1 (-1 actually), at step 38?
If I do that the velocity becomes 122 instead of 123 and the position at tick 39 becomes 3797 instead of 3798....then what should I do?

In order to "hit" 3730 at tick 39 the acceleration at step 38 would have to be 68 (8.5 times that of the programmed rate!) so that the velocity becomes 123-68=55 and the position at tick 39 becomes 3675+55=3730.

BUT, even if I could live with that gross violation of the programmed acceleration (which I just can't) the velocity is now 55 so if I now continue to decelerate at the programmed rate of 8 counts/tick/tick I would end up WAY short since the "deceleration sweet spot" of 3730 was "based" on a velocity of 123.... (Velocity will reach zero at tick 45 at which the position will be 3892 when I shooting for 4690).

I hope that makes sense and if there's something I'm missing (which by no means is unlikely) please elaborate!

/Henrik.

[richard]

its mainly the idea's I'm experimenting with at this stage . this one is to increment the setpoint not at the pid update rate but a rate that suits the acceleration / velocity . in the example that would be every 8 mS. it would be fairly simple to clamp the velocity at any rate after the acceleration move (the pid offset is constant), short moves can easily be accommodated by calculatining a lower vmax, the decelerate phase would begin when distance remaining = or less than the total accelerate displacement and then the accelerate parameter blocks are fed into pid setpoint in reverse leaving no doubt at all for the final move. assuming of course that you can sync the real position with where the ramp-generator thinks it is but you have plenty of steps here to adjust out any error.
speed wise after the initial "move" parameters are calculated the array could be filled in on the fly if you have the processing time, I like the array idea because it applies easily to both acceleration phases for the move. biggest problem I can see is that the block rate could vary with each "move " (this keeps the numbers manageable) so you would to be able to provide an accurate but variable timebase for the block clock
its a work in progress

[HenrikOlsson]

Hi Richard,

this one is to increment the setpoint not at the pid update rate but a rate that suits the acceleration / velocity
Yes, I'd probably not run it at the PID rate either but for the purpose of the discussion it makes it easier. I probably wouldn't want to run it slower than say 50 times per second though since that would make the motion jerky. So, with a fixed array size of 64 the longest move possible is 1.28s


the decelerate phase would begin when distance remaining = or less than the total accelerate displacement and then the accelerate parameter blocks are fed into pid setpoint in reverse leaving no doubt at all for the final move
Exactly, but that's the big problem. Because if you don't start "playing back" the accelerate parameter block in reverse at exactly the right position you will reach zero velocity either before or after you reach the target position. And since the motor is moving at a velocity of 240 counts (in your example .pdf) per update the likelyhood of hitting (target position - 7620) (where 7620 is the total displacement during acceleration in your example) is slim.

Accelerating is easy, clamping the velocity is easy, moving at velocity is easy, even decelerating is easy. The hard part is ending up at exactly the desired position.
Your .pdf shows a nice ramp from 0 to 30000 in velocity. Now see if you can keep moving at the velocity and then, at any given "tick", start playing back the acceleration ramp in reverse and get to position 55727. To do that you need to start playing back the acceleration ramp in reverse exactly at position 55727-7620 but since the motor is moving at 240 counts per update you're never going to hit that exact location.

I'm obviously too far into my own thinking and can't see the forest due to the trees.....prove me wrong will you ;-)

/Henrik.

[richard]

I understand what your saying , but the array data is only required for the acceleration phases of each move the constant velocity part if one is required can easily just be a loop terminating when actual_position is less than or equal to target-ramp_length. so moves of any length are possible. I envision the deceleration would need error correction applied as
error = (actual_position - (target-(ramp_length-array[])) , each iteration would be adjusted by that error and the ramp_length decremented by the array [] increment . with potentially 60 odd iterations the final movement should be pretty good. (ramp_length=0 and error=0)=good
I agree about the jerkiness I'm hoping inertia will smooth some out , tell me if you feed a 200 step increment into the pid how many iterations till the error is 0 ? . my guess is that its more the 1 ,this may also do a bit of smoothing.

[HenrikOlsson]

Hi Richard,

tell me if you feed a 200 step increment into the pid how many iterations till the error is 0 ? my guess is that its more the 1 ,this may also do a bit of smoothing.
Yes, of course it's more than one, perhaps 20-200.
Exactly HOW many depends on the inertia of the system, the tuning of the PID parameter, encoder resolution, current limit of the drive etc etc etc. Aggressive tuning will cause overshoot and ringing, "loose" tuning will cause slow risetime and so on - as I'm sure you know.

I think that what you're aiming to do with recalcuating the array is basically what I'm doing as well - except I do it on the fly at each tick. What your method have going for it, provided it works of course, is that you don't use a division in the recalculation phase which will make it run fast. Then, on the other hand, if you need to recalcute the what's left of the array each tick (not sure if you need that or not?) then it might not be more effecient, I don't know.

Please do keep posted and I'll do the same - if I could just find the time to sit down and actually get it running...

Thanks Richard!
/Henrik.

[richard]

the idea was easier to implement than I expected. when the setpoint to target distance is less than the ramp_length + the constant velocity increment , rather than any fancy math I just set the setpoint to the target-ramp_length value. I just let the pid work the mechanics out for its self. after that its just a matter or playing the array back backwards to arrive dead on target .
I have included a poorly annotated sketch of my c routine,
the motor I want to use can only achieve 300 counts/sec (accel 2000 counts /s/s) and needs to not have a count for at least 30mS to guarantee 0 vel , therefore the pid loop is run @40mS intervals and the profile input is applied on every second pid loop. this works pretty well (motor inertia gives smooth motion) and it stops on target _+ 1 count (pid might need better tuning).
seems to me that motor/load characteristics really change the entire nature of the problem.
I'm yet to try this on a real servo motor my pid loops are taking 50-300 uS to run so hopefully a 1mS pid rate is possible , I think I will get some pic18f2431's and have a play with thev qei module

[richard]

I forgot to add that in my application the accel and max v will never change only the direction and distance therefore the ramp array will never need recalculating , for "short" moves i just reduce the number of elements played out. the pid tolerance seems pretty accommodating.
I want no overshoot no stalling and no hunting +- 4 counts is accurate enough . its controlling the really stiff and jerky air slide on the pid controlled wood heater in the lounge (its a matter of principle)

[richard]

this works on the real servo motor. what a difference the qei module makes ,and 14 bit pwm seems much smoother
pid rate now 1ms ramp rate 8ms with 49 steps , no attempt yet to handle short moves but long ones are good . fwd and backwards moves are good .
pos counter 24bit / vmax and accel are fixed constants so far

[HenrikOlsson]

Hi Richard,
I've actually made some progress myself too, just for fun I'm attaching a photo of my little testbed. It was designed a long time ago, at the time I was Little "afraid" of the PCPWM module so it's actually using the normal CCP module for the PWM output, 19.531kHz, 10bit resolution. What PWM frequency are you using to get 14bit with the PCPWM module?

7478

Anyway, my code seems to work pretty good in the real world too. PID-rate is 1220Hz (derived from the PWM time base, 1:16 postscale). Initially I tried to run the profile generator "slower" than the PID but it really didn't want to fly properly. Now it's updating at the same rate as the PID and it's working pretty good. The CPU load is pretty high during deceleration but I'm "only" running it at 20MHz on this board so at 40MHz it'll be more relaxed. Acceleration, velocity and distance can be set arbitrarily.

Time is limited and motivation to work on it comes and go but I'll keep poking at it.

/Henrik.

[richard]

yes noise is an issue for 14 bit its 1.8khz , much quieter at 16khz and 10 bit res fosc is 32mhz (8meg rock *4pll)

I need to make a better test bed 600ma from the l293d is a bit limiting .

got the pid routine under 500us worst case using floats . I find that floats are easier to work with (mentally) for experimenting with but will go back to ints when I have things sussed out.

I'm not sure how I will go about implementing variable velocity or accel yet. I was just really happy to get " pid lock " and good position control to start with.

ps using the microchip c floating point library adds 8k bytes to the pgm its huge

[richard]

I found this article on setting and using the qei /pcpwm modules and incorporated it into my scheme ,its worth a look the qei.h and motor_pwm.h in particular.

http://hades.mech.northwestern.edu/index.php/PIC_Motor_Control_and_Serial_Port_Example