I'm written the following code to calculate the position of a motor during each timestep, the result of which will be compared with feedback from a quadrature encoder and subjected to a PID algorithm. The trajectory will follow a trapezoidal profile. i.e. Accelerate to a maximum velocity, coast and then decelerate to the target position:
program ideone;
VAR
Pos : Real;
Vel : Real;
Acc : Real;
Demand : Real;
Max_Vel : Real;
AccDist : Real;
DecelPoint : Real;
Error : Real;
begin
Pos:=0;
Vel:=0;
Acc:=5;
Demand:=150;
Max_Vel:=10;
AccDist := (Max_vel/Acc * Max_vel) / 2;
DecelPoint := Demand - AccDist;
Writeln('AccelDist ',AccDist:5:2);
Writeln('DecelPoint ',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;
//If Pos < DecelPoint
//Then
Writeln('Position:',Pos:5:2);
End;
Error := Pos - DecelPoint;
Writeln('DECEL');
While Vel > 0
Do Begin
If Error > 0
Then Begin
Pos := DecelPoint;
Error := 0;
End;
Pos := Pos + Vel - Acc/2;
Vel := Vel - Acc;
If Vel <= 0
Then Pos := Demand;
Writeln('Position:',Pos:5:2);
End;
end.
How can the accuracy of the algorithm be improved?