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?

[Ari423]

With my lack of C++ knowledge (I think its C++), I can't critique your code. I can tell you, based on my experience with PID controllers, that what you are looking to do is probably better accomplished using only a PID controller, with the sum of all of the encoder values as the process variable and the desired angle as the setpoint. A PID controller will automatically accelerate the motor to maximum (if needed), coast at maximum for however long it takes to get close to the point ('close' is changed by tuning the PID), and then decelerate until it hits the point you want, which seems to be what you are looking to do. If tuned properly, the PID loop should not only be very accurate and fast at getting the motor to the specified point, but it should also hold the motor at the point if something tries to move it.

If you need help writing or tuning a PID controller (and I would recommend writing it yourself), there are plenty of websites that can help with that. Here are a few:

https://en.wikipedia.org/wiki/PID_controller
http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html (helpful for writing your own PID controller)
http://www.csimn.com/CSI_pages/PIDforDummies.html

[martinrand]

Thanks for your reply!

It's actually Pascal, not C++, and the issue isn't really a syntax one, it's a logic/maths/physics one.

From the research I've done, generating a position setpoint seems to be the way to go. PID can come in later to compensate for the resulting error between theoretical position and actual position (from the encoder).

Either way, I'm bent on solving this problem! I may not end up using it as part of the controller, but I'd like to at least be able to solve it before I look for other options.

Thanks.

[Ari423]

...generating a position setpoint seems to be the way to go. PID can come in later to compensate for the resulting error between theoretical position and actual position (from the encoder).

PID will use a position setpoint to figure out where you want the motor to turn to. I guess you could use some other method to get close to your setpoint and then use a PID to get it exact (or at least closer), but I don't see the benefit if you have to make and tune a PID anyway. Maybe you can clarify.

[martinrand]

Right, but my problem seems to be generating the exact setpoint for a trapezoidal move.

[Ari423]

The setpoint is just the value of the process variable that you would like to motor to turn to. If you are measuring motor rotation using an encoder, the encoder gives you degrees per second. You would then numerically integrate this (sum all of the values) to get degrees of displacement. The setpoint should be however many degrees of displacement you want your encoder to measure. (NOTE: this may be different from how many times the motor turns depending on your encoder setup)

[martinrand]

Is it possible my definition of setpoint is incorrect?
When I use the term, I'm referring to where the motor (i.e. the encoder count) should be at a given point in time.

[GeeTwo]

Forcing Pos and Vel to the "known" values without setting a known time is probably much of the issue. You're probably getting a way with it at the end of acceleration, but not at the beginning of deceleration. I would recommend making a table of times and positions and speeds, dropping in extra values at the start/end of each phase.

Also, the use of the variable Error is strange. You are ultimately only using it as a special case flag at the start of deceleration; where it is used to set Pos is more easily given as Pos:=Decelpoint;. This should just be a special print statement that is not part of a loop.

[martinrand]

Forcing Pos and Vel to the "known" values without setting a known time is probably much of the issue. You're probably getting a way with it at the end of acceleration, but not at the beginning of deceleration. I would recommend making a table of times and positions and speeds, dropping in extra values at the start/end of each phase.

Also, the use of the variable Error is strange. You are ultimately only using it as a special case flag at the start of deceleration; where it is used to set Pos is more easily given as Pos:=Decelpoint;. This should just be a special print statement that is not part of a loop.

Thanks for that!

I'm still a bit unsure about all of this :mad:
Any chance you could kindly make your suggested amendments to the code to give me a better idea?

Thanks!

[JamesTerm]

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.Where have I gone wrong?

I'm not going to solve this for you, but I do want you to take a peek at this: (https://www.youtube.com/watch?feature=player_embedded&v=VPkYW2ZVzKE)

d=1/2 a t^2
learn this, and love it... it is probably one of my favorite equations.

What I do is solve for time...
t=sqrt(2.0*(d/a))
If I know how much time I have left given my current velocity and given the desired acceleration rate. I can solve where on the velocity line my velocity should be at that time by the equation v=a*t; //where t is the amount of time I need to come to a complete stop of "a" as my max stopping acceleration. (I factor out mass from force to keep my numbers small). This works for both setpoint (e.g. arm position) kind of problems as well as velocity control (e.g. ball shooter).


On a side note you may find this equation too:
d=1/2at^2+vt
At one time I considered using the vt variable to do a match velocity kind of function in game development (i.e. one object tracking another moving object)... but found it easier in code to treat that as a separate problem and add the velocity back in as a separate part of the function.

(Ether you still around?)

[martinrand]

I had originally thought of using the equations you suggested but a discrete method (such as the one I used in my code) would be more appropriate for the MCU I'll be using. It also seems to bet the preferred way of performing the calculations.

[JamesTerm]

I had originally thought of using the equations you suggested but a discrete method (such as the one I used in my code) would be more appropriate for the MCU I'll be using. It also seems to bet the preferred way of performing the calculations.


Can you elaborate what you mean... when you say discrete method... to be honest I never heard anyone use this term before.

Thanks!

[GeeTwo]

I had originally thought of using the equations you suggested but a discrete method (such as the one I used in my code) would be more appropriate for the MCU I'll be using. It also seems to bet the preferred way of performing the calculations.

If you're going to stick with constant acceleration, the closed form solution suggested above is easier to use. If you just want samples on regular intervals but don't mind using closed form for calculations, I would suggest something like:

1) calculate time and distance to accelerate from stop to Max_Vel
2) calculate time and distance to accelerate from Max_Vel to stop (same, if max_accel is the same)
3) verify that the sum of these distances is less than Demand; adjust times and distances if too large.
4) Calculate time to begin deceleration
5) Start a loop over time
Within the loop, do a case or if/then/elseif structure based on whether time is within the acceleration phase, cruise phase, or deceleration phase.