Generating Position Setpoints

[martinrand]

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?