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:

Name: Simple_profile.gif Views: 21624 Size: 5.4 KB

I'm trying to base my version on Microchips AN532, 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:
Code:
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?

Code:
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.