# Generating Position

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?

• @Mast I've run with several different values but my maths impaired mind can't figure out whether the position results are what they should be. I was hoping somebody here could tell me whether the results are correct, and if not, what needs to be changed. – M-R Jul 22 '15 at 22:23
• The thing is, we review working and only working code. So you should at least have a general idea about whether the output is somewhat along the lines of what is expected. – Mast Jul 22 '15 at 22:32
• @Mast, the output is definitely along the lines of what it should be. I'm just unsure about whether the results are exact or approximate. In theory, the code would probably do the required job, but I'd like to know how I can improve accuracy (If I need to). – M-R Jul 22 '15 at 22:36
• Is this along the lines of what you expect? It's not clear what trapezoidal motion would translate into. – Mast Jul 22 '15 at 22:44
• He's right about one thing. As long as your question states as it is, you risk your question being put [on hold]. Please rephrase the question. While you're at it, we like context. Good context gives good answers. – Mast Jul 22 '15 at 22:54

First things first. Pascal is a very readable language, if written clearly. One of the main reasons of writing Pascal is because you want readable code. I'll start with some styling and naming, the actual calculations will be handled in the second part.

# Style

Regardless of which standard you follow, make sure you're consistent. I'm following the GNU Pascal Coding Standards.

### Keywords are lower-case

var while do if then begin end


No capitals. Although Pascal accepts the keywords as valid regardless of capitalisation, it's good style to follow the standard.

### If then

Keep your if and then on the same line.

if Error > 0 then


### Indentation

Your code isn't indented. Indentation is a major step forward in readability.

### Spaces

A single space goes before and after operators. There are also spaces after commas.

### Semi-colons

You are missing a ; here:

Pos := AccDist


### Variable naming

Your variable names could use some love. You may think Pos is a good name for a position, but Pos() is also an already existing function in Pascal. Your variable has nothing to do with the function, so avoid the confusion.

DecelPoint is according to the standard. Max_Vel isn't.

### Result

I've fixed the above (except part of the naming, I'm not too good at that either). The result:

program ideone;
var
Pos : Real;
Vel : Real;
Acc : Real;
Demand : Real;
MaxVel : Real;
AccDist : Real;
DecelPoint : Real;
Error : Real;

begin
Pos := 0;
Vel := 0;
Acc := 5;
Demand := 150;
MaxVel := 10;

AccDist := (MaxVel / Acc * MaxVel) / 2;
DecelPoint := Demand - AccDist;
Writeln('AccelDist ', AccDist:5:2);
Writeln('DecelPoint ', DecelPoint:5:2);
Writeln('ACCEL');

while Vel <> MaxVel do
begin
Pos := Pos + Vel + Acc / 2;
Vel := Vel + Acc;

if Vel >= MaxVel then
begin
Vel := MaxVel;
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 := 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.


# Algorithm

Increasing the accuracy of the graph is a matter of increasing the amount of measurements. If every second isn't good enough, measure every half a second. If that's still not good enough, measure every tenth of a second. Or whatever your time unit may be. Your current set-up does not allow for such modifications.

You'll either need a MaxNumIterations and divide the whole course over that or put in a modifier which does basically the same under water. I'd go with the first.

Then in ACCEL or DECEL, use a shorter cycle, say timestep / N. Adjust the formulaes accordingly. Perhaps only output every N calculations. The smaller timestep gives more accuracy. In your present formulaes timestep is 1, so you are multiplying with 1 and 1 squared. Then using 1/N, you will have to multiply with 1/N and 1/N squared.

• Right now, your post feels incomplete. By starting your first sentence with "Then...", it seems as though you are continuing on from another thought. However, there is no other thought shown. Then, you start mentioning other improvements of yours, but the thoughts again are incomplete because you do not elaborate on what is good about this improvement, why the OP should change their code, and how is this better. – SirPython Jul 23 '15 at 13:33