PID crossing 0

[ratdude747]

I am right now working on "unicorn drive" code, and I almost have all of it done.

I have one question: as I understand, PID's never cross 0 and go back to the max. since the crab pods being rotated are coaxial, they can and should cross 0 to making turning faster; I already have a "shortest distance; flip the wheel speed and lose 180 degrees from the setpoint if the distance is over 180 degrees.

Is there an alternative to a PID that will let me cross 0 or a way to make the PID cross 0? or do i need to write my own algorithm to render that?

[RyanCahoon]

I think I see what you're getting at, but if I've interpreted your question incorrectly, please clarify.

In the standard formulation of a PID controller, all three controllers (proportional, integral, derivative) use an error signal defined as

<error> = <set point> - <measured>

Using this with solely positive angle values will, as you say, cause the controller to never command the system past zero (assuming a perfect controller, anyway). To correct this, I would apply some sort of modulus to keep the error signal within -PI<=err<PI (radians, or -180<=err<180 degrees). In C code:

float computeError(float setPoint, float measurement) {
   float err = setPoint - measurement;
   err -= (2*M_PI) * rint(err / (2*M_PI));
   return err;
}

Replace instances of (2*M_PI) with 360.0 for degrees. rint(...) rounds to nearest integer value.

The issue with this is you now have discontinuities in your error function, so you'll want to provide some sort of protection so your derivative term doesn't blow up (e.g. if your error signal jumps from -3.14 to +3.14, derr/dt ~= 6.28 / (0.0333 sec) = 188.4).

--Ryan

P.S. If someone needs helping converting into Java or LabVIEW (or Python or ...), let me know

[EricVanWyk]

By "cross zero" do you mean wrap phase? As in your number line goes 358, 359, 0, 1, 2?

There are wrap/unwrap phase VIs that might help you with this. Just be warned that they aren't psychic.

[Ether]

Originally Posted by ratdude747

Is there an alternative to a PID that will let me cross 0 or a way to make the PID cross 0? or do i need to write my own algorithm to render that?

If you want want to use the WPI library PIDs, you can create "setpoint" and "process_variable" inputs for them as follows (assuming angles in degrees):

angle_error = target_angle - measured_angle;
angle_error -= 360*floor(0.5+angle_error/360);

then:

setpoint = angle_error;
process_variable = 0;

OR

setpoint = 0;
process_variable = -angle_error;


The disadvantage of the first approach is that the Derivative term will be disabled since the WPI Lib PIDs only look at the derivative of the process_variable.

The disadvantage of the second approach is that, although the Derivative term will be active, it will respond to changes in BOTH the measured_angle as well as the target_angle, so be careful how you adjust it.

Notice that the first two lines of code above (calculating the angle_error) calculate the shortest angle distance to the target. You need extra conditional logic only if you want to change the sign of wheel speed. See the discussion here.

[ratdude747]

Originally Posted by Ether

If you want want to use the WPI library PIDs, you can create "setpoint" and "process_variable" inputs for them as follows (assuming angles in degrees):

angle_error = target_angle - measured_angle;
angle_error -= 360*floor(0.5+angle_error/360);

then:

setpoint = angle_error;
process_variable = 0;

OR

setpoint = 0;
process_variable = -angle_error;


The disadvantage of the first approach is that the Derivative term will be disabled since the WPI Lib PIDs only look at the derivative of the process_variable.

The disadvantage of the second approach is that, although the Derivative term will be active, it will respond to changes in BOTH the measured_angle as well as the target_angle, so be careful how you adjust it.

Notice that the first two lines of code above (calculating the angle_error) calculate the shortest angle distance to the target. You need extra conditional logic only if you want to change the sign of wheel speed. See the discussion here.

i already have the sign change code.

my concern is it going all the way around from say 0 to 7pi/4 as opposed to only moving a distance of pi/4.

my only real knowedge of pid is you give it a setpiont and a process variable and it controls a motor within given range of outputs. a lot of the nuts and bolts i have yet to learn.

[Ether]

Originally Posted by ratdude747

my concern is it going all the way around from say 0 to 7pi/4 as opposed to only moving a distance of pi/4.

Read the rest of the post. It explains how to deal with that. Try it with some example values and you'll see.

my only real knowedge of pid is you give it a setpiont and a process variable and it controls a motor within given range of outputs

If you use the WPI library PID and feed it your desired angle as the "setpoint" and the measured angle as the "process variable" without the computations I showed in the post, then you will get the undesirable behavior that you are concerned about. The computations I posted show how to avert this undesirable behavior.




For example, suppose your steering angle encoder ("measured_angle") reads 0 to 360 degrees clockwise, with zero being "straight ahead". Suppose your calculated desired steering angle ("target_angle") also is 0 to 360 degrees, again with 0 being straight ahead.

Then let's do some example calculations:


Example 1:

measured_angle = 359 degrees

target_angle = 2 degrees

angle_error = target_angle - measured_angle = 2 - 359 = -357 degrees

angle_error -= 360*floor(0.5+angle_error/360) = +3 degrees


setpoint = angle_error = 3 degrees

process_variable = 0 degrees


In other words, the PID will try to rotate the steering clockwise 3 degrees, which is what you want.


Example 2:


measured_angle = 4 degrees

target_angle = 354 degrees

angle_error = target_angle - measured_angle = 354 - 4 = 350 degrees

angle_error -= 360*floor(0.5+angle_error/360) = -10 degrees


setpoint = angle_error = -10 degrees

process_variable = 0 degrees


In other words, the PID will try to rotate the steering counter-clockwise 10 degrees, which is what you want.



Notice how, in each of the above examples, the code "angle_error -= 360*floor(0.5+angle_error/360)" calculates the shortest angle path (and the correct angle direction) from the measured_value to the target_value.

[ratdude747]

if what i am reading is correct, then:
i can use target-measured=setpoint; process =0

everything in the code is in radians.

[Ether]

Originally Posted by ratdude747

if what i am reading is correct, then:
i can use target-measured=setpoint; process =0

No. You left out the line "angle_error -= 360*floor(0.5+angle_error/360)"

If you leave that line out, you will get the following:


Example 1:

measured_angle = 359 degrees

target_angle = 2 degrees

angle_error = target_angle - measured_angle = 2 - 359 = -357 degrees


setpoint = angle_error = -357 degrees

process_variable = 0 degrees


In other words, the PID will try to rotate the steering counter-clockwise 357 degrees, which is NOT what you want.


Example 2:


measured_angle = 4 degrees

target_angle = 354 degrees

angle_error = target_angle - measured_angle = 354 - 4 = 350 degrees


setpoint = angle_error = 350 degrees

process_variable = 0 degrees


In other words, the PID will try to rotate the steering clockwise 350 degrees, which is NOT what you want.



If you are using radians instead of degrees, then replace "angle_error -= 360*floor(0.5+angle_error/360)" with "angle_error -= 2pi*floor(0.5+angle_error/2pi)".

[ratdude747]

i see... but what exactly do you mean by "floor"? never seen that math operation before.

edit- i see now... its the lowest integer function. however the equations doesn't work. did you mean somethign else?

[ratdude747]

Originally Posted by EricVanWyk

By "cross zero" do you mean wrap phase? As in your number line goes 358, 359, 0, 1, 2?

There are wrap/unwrap phase VIs that might help you with this. Just be warned that they aren't psychic.

thats what i meant.

[Ether]

Originally Posted by ratdude747

i see... but what exactly do you mean by "floor"? never seen that math operation before.

edit- i see now... its the lowest integer function.

Yes, it's part of the standard C library.


For LabVIEW users, the calculation "angle_error -= 360*floor(0.5+angle_error/360)" can be coded as shown in the attachments to this post.


Notice that it doesn't matter if your angles are from 0 to 360 or from -180 to +180. All that matters is that they are both zeroed at the same point and that they are both measured in the same direction.


For example, let's re-do the Example2 calculation in this post, but with the calculated steering angle being from -180 degrees to +180 degrees instead of 0 to 360 degrees, to show that the exact same code can be used to give the proper result


So, your steering angle encoder ("measured_angle") reads 0 to 360 degrees clockwise, with zero being "straight ahead".

And your calculated desired steering angle ("target_angle") is -180 to +180 degrees, again with 0 being straight ahead.



Example 2:


measured_angle = 4 degrees

target_angle = -6 degrees (instead of +354 degrees)

angle_error = target_angle - measured_angle = -6 - 4 = -10 degrees

angle_error -= 360*floor(0.5+angle_error/360) = -10 degrees (you still get -10 degrees, which is what you want)

[Ether]

Originally Posted by ratdude747

edit- i see now... its the lowest integer function. however the equations doesn't work. did you mean somethign else?

What do you mean by "the equations don't work" ?

Please post an example.


Here's a complete C program. Compile and run it:

#include <math.h>
#include <stdio.h>

void test(double target, double measured){
double angle_error;
printf("\n%12.3f target\n%12.3f measured\n",target,measured);
angle_error=target-measured;
angle_error -= 360*floor(0.5+angle_error/360.0);
printf("%12.3f angle_error\n\n",angle_error);
}


int main(void){
   
test(2,359);
test(354,4);

return 0;

}

Here's the output:

       2.000 target
     359.000 measured
       3.000 angle_error


     354.000 target
       4.000 measured
     -10.000 angle_error

[ratdude747]

I found another way to make it work.

I added a bit of code that subtracts current from target, and then adds or subtracts 2pi. it then comapres the absolute values of the initial subtraction and the adjusted and selects the lesser of the two

a way of psudo-wrapping i guess.

i will attach a .zip of the code for anybody who wants to see what is going on in the next post.

[Ether]

Originally Posted by ratdude747

the equations doesn't work.

Originally Posted by ratdude747

I found another way to make it work.

Please post a counter-example showing what doesn't work with the code I posted. If there's a problem with it, I want to know.

[ratdude747]

Originally Posted by Ether

Please post a counter-example showing what doesn't work with the code I posted. If there's a problem with it, I want to know.

I crunched both examples and got 360. maybe my calculator got it wrong, but I think the floor is the issue since it will gives either 0 or 1 and thus either 0 or 360.

I attached my code.

how my code works:

1. each wheel is its own vector, with right being 0 radians like a coordinate plane. therefore, forward = pi/2, left= pi, and backward = 3pi/2

2. every angle is in radians.

3. the encoder values are divided by 5 and multiplied by 2pi to make them radian encoders. I then subtract a calibration value and then use sin and asin to make all values between 0 and 2pi.

4. x and y are already broken down components. z (rotate) is not. z is rendered by taking the optimal angle for each wheel to turn the robot clockwise (can be found with trig, my values are for approx what my unibot has), lets call it Q for now, and making Xz= cos(Q)*z and Yz = sin(Q)*z

5. to generate one wheel's final target vector, x+Xz=Xf ; y+Yz=Yf. then, to find the vector's magnitude, we use the distance formula on Xf and Yf. The direction is found using atan2 on Xf and Yf.

6. the vector is then adjusted for negative magnitude benefits. I figured if the distance between target postion (TP) and current position (CP) is between pi/2 and 3pi/2, then subtracting (+ distance) or adding (- distance) pi from the direction and changing the sign on the magnitude will increase agility.

7. Using the modified vector, the new TP distance from CP is calcualted and then put though a conditional section that takes the original distance (OD) and then adds/subtracts 2pi (same sign rules as #6) to make modified distance (MD). then, it finds which one is closer to zero and sets it as the new PID setpoint. the PID's process variable is always 0. the magnitude is sent as a drive motor %vbus.

8. It does #3-#7 for each wheel independently.

it probably could be simplified in the steps in #6 and #7... the reverse magnitude bit was added first... this is just to be modular enough so the next time i can test the code, I can follow each step.

questions?