Generating Position Set Points for Trapezoidal Motion Profile - pascal

Question

I'm working on a prototype for a motion controller which will accelerate a motor to a maximum velocity, coast at the maximum velocity and then commence deceleration at the correct position for the motor to stop at the target position.
The theoretical position for each timestep will be compared with the feedback from a quadrature encoder and the resulting error will be subjected to a PID loop, the result of which will be represented using PWM.
I currently have the following code to determine the theoretical position for each timestep:
Pos:=0;
Vel:=0;
Acc:=3;
Demand:=300;
Max_Vel:=19;
AccDist := (Max_vel/Acc * Max_vel) / 2;
DecelPoint := Demand - AccDist;
Writeln(AccDist:5:2);
Writeln(DecelPoint:5:2);
Writeln('ACCEL');
While Vel <> Max_vel
Do Begin
Pos := Pos + Vel + Acc/2;
Vel := Vel + Acc;
If Vel >= Max_Vel
Then Begin
Vel := Max_Vel;
Pos := AccDist
End;
Writeln('Position:',Pos:5:2);
End;
Writeln('FLAT');
While Pos < DecelPoint
Do Begin
Pos := Pos + Vel;
Writeln('Position:',Pos:5:2);
End;
Error := Pos - DecelPoint;
Writeln('DECEL');
While Vel > 0
Do Begin
If Error > 0
Then Begin
Pos := Pos - Error;
Error := 0;
End;
Pos := Pos + Vel - Acc/2;
Vel := Vel - Acc;
If Vel <= 0
Then Pos := Demand;
Writeln('Position:',Pos:5:2);
End;
end.
This code seems to give approximate results, but I really need exact results. The accel and flat section seem to yield exact results, but when we come to the decel section, things start behaving oddly.
Where have I gone wrong?

Answer 1

I believe that there are two problems.
Problem 1: You are displaying a list of positions but not the time at which the vehicle reaches those positions. If it is assumed that the accelerations, velocities, positions etc are all in m/s^2, m/s, and m then the data is consistent with 1 second intervals except when the vehicle transitions between acceleration and coasting (or coasting and deceleration). If I add columns for velocity and time then your output data would look like this:
60,17
239.83
ACCEL
Position: 1.50 Velocity: 3.00 Time:1.00
Position: 6.00 Velocity: 6.00 Time:2.00
Position:13.50 Velocity: 9.00 Time:3.00
Position:24.00 Velocity:12.00 Time:4.00
Position:37.50 Velocity:15.00 Time:5.00
Position:54.00 Velocity:18.00 Time:6.00
Position:60.17 Velocity:19.00 Time:6.33 ...vehicle stops accelerating at this point
FLAT
Position:79.17 Velocity:19.00 Time:7.33
Position:98.17 Velocity:19.00 Time:8.33
...
You can see that the actual time the vehicle stops accelerating is only 0.33 seconds (rather than a full second) after the 6 second measurement point at position 54.00 m.
The same problem will occur when the vehicle reaches the deceleration point (which I estimate should occur at approximately T = 15.79 seconds (that is 9.46 seconds after it stops accelerating).
Problem 2 The second issue is that you write the position during the FLAT period before you check if the vehicle should have started decelerating.
While Pos < DecelPoint
Do Begin
Pos := Pos + Vel;
Writeln('Position:',Pos:5:2);
End;
Your code above can increment the position past the deceleration point and output an erroneous data point before it 'corrects' it later. Your program would output:
...
Position:231.17
Position:250.17
DECEL
Position:257.33
...
but the last position before the DECEL is incorrect as the vehicle will already have started decelerating when it reaches 239.83 m.