Motion profile generator.

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.

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 .

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

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

Code:

---

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:

Code:

---


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.

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

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.

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

Hi Richard,

Quote:

---

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

Quote:

---

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.

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.

Hi Richard,

Quote:

---

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.

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

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)

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

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?

Attachment 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.

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

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/i...l_Port_Example