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:

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