Control Theory - Help me create a "Trajectory Generator".

Hi Guys

i'm writing a high performance motion controller for the ARM based sam3x www.atmel.com/images/doc11057.pdf

I've been reading up on control theory for the past couple of days and have come to the conclusion that i'm not smart enough to do the math and i therefore need to wing it, as usual.

The usual PID stuff has been dealt with and small step performance is incredible. Next up is the acceleraton profiles.


For now im going with a trapezodial Profile with 2 jerk ramps. but i hope to late modify it for a S-curve profile.

1: Acceleration to cruise jerk
2: Deacceleration from cruise to stop jerk.

For now i'm spacing my targets(red dots) by 1millisecond - and running my pid and pwm calculations at 5kz (hardware timed) .
What i want to do is interpolate 5 intermediate (red bars) of positions between the targets as to match my 5khz loop and keep the motion as precise and smooth as possible.



This gives me a encoder count of 12 @ 500rpm. and works as expected- i have ramp up acceleration and cruise speed. But i am not overly happy with the acceleration as doing a acceleration from 0 to 500rpm the acceleration is in stages. This is because it ramps from 1 -> 12 encoder counts offsets from eg:

Code:

1khz loop{
maxSpeed = 12;
currentSpeed = abs(currentPos - lastPos);
if (currentSpeed < maxSpeed)
  {
    speed += acclConst; //ramp speed to 12
    "nextPosition +=  Speed ; // apply next position speedoffset.
  }

use pid to drive us to next position.

}

As you can see the speed increase can only occur as currentposition + 1 - 12 encoder count offset which causes the motor to accelerate in steps. i would like to interpolate between the 12 steps.

Any ideas and especially C-examples is welcome

thanks guys

5KHZ can cause motor squeal, hence we use 20 Khz

Hi Bsft!

Do you mean Pwm frequency or pidloop calculation frequency?

PWM frequency is hardware controlled on a separate timer and can be adjusted independently from the pid-loop timer( 1khz to 80khz)@ 12 bit accuracy .

The pid loop is cpu limited(by my poor coding skills ) to 100Khz - i just chose 5khz because it was recommended by commercial servo manufacturers as a "Fast high performance pid loop for servo drives".



do you run a pid calculation before each pwm pulse ?

Thanks

ah ok, I dont really know.
I use the JRK and I set the PID according to instructions from another member, and I use 20Khz PWM to stop motor squeal.
I will leave this thread as I misunderstood what you are trying to do.

I'm a little further along - i had to look into basic physics equations. Basically SUVAT which is s-distance, u-current velocity, v-velocity, a-acceleration, t-time. As far as i can see, i need to divide it intp 3 parts.

The first acceleration, cruising, decceleration. and calculate the distance for each operation and compare add it all together and compare it to the final destination.

It is a bit more involved than i first thought. And math is not my strong side

Cheers
Daniel

okay so i'm now planning the entire move profile ahead of time, recalculating the movement/trajectory profile 1000 times a second and updating the PID 10000 times a second, and it works beautifully.
Acceleration and and deceleration are now at a controlled rate and it hits the target position perfectly. Before with PID only the motor would snap violently to each new position. Now it accelerates with the values i give it by "snapping" in very small steps generated by the equations below:

These are the equations i'm using:


http://en.wikipedia.org/wiki/Equations_of_motion

Heres a stepper motor running a trapezoidal profile for those who are more visually stimulated (like me)

jump to 3:40 notice the smooth accelerations..