pascal - Generating Position Set Points for Trapezoidal Motion Profile -

i'm working on prototype motion controller accelerate motor maximum velocity, coast @ maximum velocity , commence deceleration @ correct position motor stop @ target position.

the theoretical position each timestep compared feedback quadrature encoder , resulting error subjected pid loop, result of represented using pwm.

i have following code determine theoretical position 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 begin  pos := pos + vel + acc/2;  vel := vel + acc;  if vel >= max_vel begin vel := max_vel; pos := accdist end;   writeln('position:',pos:5:2);   end;  writeln('flat');     while pos < decelpoint begin  pos := pos + vel;  writeln('position:',pos:5:2);  end;  error := pos - decelpoint;  writeln('decel');  while vel > 0 begin  if error > 0  begin  pos := pos - error; error := 0; end;  pos := pos + vel - acc/2; vel := vel - acc;  if vel <= 0 pos := demand;  writeln('position:',pos:5:2);   end;   end. 

this code seems give approximate results, need exact results. accel , flat section seem yield exact results, when come decel section, things start behaving oddly.

where have gone wrong?

i believe there 2 problems.

problem 1: displaying list of positions not time @ vehicle reaches positions. if assumed accelerations, velocities, positions etc in m/s^2, m/s, , m data consistent 1 second intervals except when vehicle transitions between acceleration , coasting (or coasting , deceleration). if add columns velocity , time output data 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 @ point  flat  position:79.17     velocity:19.00     time:7.33  position:98.17     velocity:19.00     time:8.33 

...

you can see actual time vehicle stops accelerating 0.33 seconds (rather full second) after 6 second measurement point @ position 54.00 m.

the same problem occur when vehicle reaches deceleration point (which estimate should occur @ approximately t = 15.79 seconds (that 9.46 seconds after stops accelerating).

problem 2 second issue write position during flat period before check if vehicle should have started decelerating.

 while pos < decelpoint    begin      pos := pos + vel;      writeln('position:',pos:5:2);    end; 

your code above can increment position past deceleration point , output erroneous data point before 'corrects' later. program output:

 ...  position:231.17  position:250.17  decel  position:257.33  ... 

but last position before decel incorrect vehicle have started decelerating when reaches 239.83 m.