PID crossing 0
 

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?

Re: PID crossing 0
 

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
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:
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

Re: PID crossing 0
 

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.

Re: PID crossing 0
 

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.

Re: PID crossing 0
 

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.

Re: PID crossing 0
 

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


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.

Re: PID crossing 0
 

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

everything in the code is in radians.

Re: PID crossing 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)".

Re: PID crossing 0
 

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?

Re: PID crossing 0
 

thats what i meant.

Re: PID crossing 0
 

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)

Re: PID crossing 0
 

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

Please post an example.


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


Here's the output:

Re: PID crossing 0
 

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.

Re: PID crossing 0
 

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.

Re: PID crossing 0
 

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?

Re: PID crossing 0
 

For Example1,

0.5+(-357)/360 is equal to -0.4917

The floor of -0.4917 is -1.

Are you saying that your calculator returned 0 or 1, instead of -1 ??

Re: PID crossing 0
 

Yes.

1) What encoder are you using, and why did you have to do this:


2) If the (X,Y, Z) joystick values are (0.2, -0.5, 0.25), would you please crank those numbers through your code and post the 4 target wheel speeds and wheel steering angles that your code calculates.


3) What is the wheelbase and trackwidth of your vehicle

Re: PID crossing 0
 

1. US digital MA3 encoders. it is to allow for the calibration to work So anything above 2pi is converted to values in the range of 0 to 2 pi. otherwise you might have negative values and vales not going all the way to 2pi.
2.I will when i have time and my graphing calculator.


3. I am not 100% sure but it is close to the scale of a wb=38 and tw=28 (frame was close to max size, wheels in corners). assume that scale for #2's values.

Re: PID crossing 0
 

If you plan to use vectors, you could actually just use some properties of vectors to find the target angle. Let U be the vector in the direction the wheels are currently facing, and V be the vector in the direction you want the wheels to face. Assuming U and V are unit vectors (that is, they have a length of one), the angle between then (let's call it Q) is found by the equation Q = acos((Ux*Vx)+(Uy*Vy)). Because acos returns a value between 0 and pi, we can subtract Q from pi if Q is greater than pi/2 (because, if Q is greater than pi/2, it is quicker to turn the wheels to face in the opposite direction and rotate them backwards). After this set, Q is between 0 and pi/2, but we don't know which direction to rotate in. Assume a positive value of Q corresponds to a counterclockwise rotation, we can then give Q the same sign as the expression (Ux * Vy - Uy * Vx), which is always positive if the rotation required to move U to V is counterclockwise, and negative if the rotation is clockwise. This will give you the shortest counterclockwise rotation, with negative magnitude benefits.

In psuedo-C:

Re: PID crossing 0
 

Yes, but why are you using sine and arcsine to do this?


I thought you had working code. Can't you just run the code?

Re: PID crossing 0
 

If you don't use vectors, you could do the following:

Let U be the angle (in radians) in the direction the wheels are currently facing,
and V be the angle in the direction you want the wheels to face.

Assuming U and V are both measured in the same direction from the same zero reference,
the angle between them (let's call it Q) is found by the equation:

Q = (V-U) - 2pi*floor(0.5+(V-U)/2pi);

Because the above returns a value for Q between -pi and pi,
it tells you which direction to rotate as well as how much to rotate.

If you want to add logic to reverse direction,
that is straightforward to do at this point.

Be aware that it is not necessarily true that steering angle changes
greater than pi/2 are best handled by reversing wheel speed direction.

It depends on many factors, such as the present value of wheel speed,
and the relative dynamic response of your wheel speed vs steering angle.

Re: PID crossing 0
 

it does use vectors.

I do not know c or c++ for the record.

it *should* work... but I currently do not have access to robot and if i did, it needs its crio re-installed among other control system parts. bottom line is that It may be a while until I can test it.

I used sin and asin since it always puts it in the correct range. you could also use cos and acos.

Re: PID crossing 0
 

Actually, asin will always return a value between -pi/2 and pi/2, while acos will always return a value between 0 and pi. If you want an angle between 0 and 2pi, you have to consider both acos and asin, as alone neither gives enough information to determine the angle*.

If you want to use trig, this method could work:

Let U be the raw angle given by the encoder, and Q be that same angle put in the range [0, 2pi].

If sin(U) >= 0, then Q = acos(cos(U))
Otherwise, Q = 2pi - acos(cos(U))

* In fact, for sin(x) = k, 0 <= x < 2pi, there are two possible values for x (barring x = pi/2 or 3pi/2). Likewise, cos(x) = m also has two values (barring x = 0 or x = pi). The possible value represented in both equations is the true value of x.

Re: PID crossing 0
 

i see the point... perhaps my best bet is to use mechanical calibration instead...

Re: PID crossing 0
 

Yikes! use this:

U = U - 2pi*floor(U/2pi)

Re: PID crossing 0
 

i checked that. looks good. thank you!

I tried using a formula node for that but i guess you have to declare variables in those... the only 2 text languages i really know are pbasic (unrelated: didn't older IFI contollers use that?) and ti-8x code.

although the team's main programmer hates my lack of formula nodes, using discrete math blocks will have to do for now...