Autonomous: Mixing Drive Distance PID's with Turn Angle PID's.

[Tom Line]

I'm curious how other teams attack their autonomous drivetrain commands.

In the past, 1718 has followed this pattern:
1. Tune a PID loop so the robot can drive a distance to +/-1 inch.
2. Tune a second "Turning" PID loop so the robot can turn to an angle +/- 1 degree.

Then we have used seperate states for each command - drive a distance, then turn, then drive, then turn. Sometimes we add in a 'drive straight' pid that mixes with the "drive a distance" pid so that the robot drives straight. This is usually just a cludged together system like this:

Left Drive = "Drive Distance" PID + "Drive Straight" PID
Right Drive = "Drive Distance" PID - "Drive Straight" PID

This usually results in only one side of the drivetrain helping to 'drive straight' because one side is usually greater than 1 (or less than -1), which gets limited to 1.

I'd like to be able to combine these into a single system where the robot is able to drive 'curves' to it's destination point. However, I really don't know how to start mixing the Drive-distance and Turn PID's except in a silly cludge like I showed above. In addition, I assume we'd have to look at some sort of gain scheduling with the turn PID due to the difference between turning while not moving and turning while moving.

Of course this all assumes tank drive.

How have other teams handled this mathematically? (Please try to keep it simple - system engineering was never my strong point).

[Jared Russell]

The method of adding the two outputs that you propose will work (and work well once the loops are tuned). The limitation of what happens when one drive side is saturated (e.g. can't go any faster) is a problem - as you noted.

The simplest/best strategy for dealing with this problem in my experience is a function which detects this case and adjusts both outputs (left and right) to ensure that the proper ratio between left and right drive power is maintained, while neither output exceeds +/-1. Note that it helps to have a linear response curve before doing this (e.g. use Jaguars, or write a linearization function for your Victors). If you look at the newly-posted 2011 Team 254 code, you will see code to deal with this scenario.

[Joe Ross]

That's exactly the method that we use. We limit the drive distance max to .8, to allow room for the gryo corrections. There are probably better ways, but that was simple.

[Tom Line]

Originally Posted by Jared341

The method of adding the two outputs that you propose will work (and work well once the loops are tuned). The limitation of what happens when one drive side is saturated (e.g. can't go any faster) is a problem - as you noted.

The simplest/best strategy for dealing with this problem in my experience is a function which detects this case and adjusts both outputs (left and right) to ensure that the proper ratio between left and right drive power is maintained, while neither output exceeds +/-1. Note that it helps to have a linear response curve before doing this (e.g. use Jaguars, or write a linearization function for your Victors). If you look at the newly-posted 2011 Team 254 code, you will see code to deal with this scenario.

Now you're just being mean, digging up linearization from my 14 years ago system class .

I remember the basics - create a function that modifies your input so that the output becomes linear. However I'll be darned if I can remember the process, and every online source starts digging into calculus that I haven't touched in 20 years.

So, I'll turn to practical methods.

Step 1: Plot the labview input (x) versus victor output voltage (y) measured with multimeter to define a non-linear curve.
Step 2: Create two columns: "Labview input to victor" and "Required input to linearize output".
3: Populate columns in excel, by estimating values off my graph, then apply a best fit line to get a polynomial equation that relates the required labview inputs to the actual, such that F(Actual labview input) = Required Labview Input.

Now tell me there's some nice clean mathematical way to do this in fewer steps and more accurately (where's Ether when you need him - this is right up his alley)....

[Ether]

Originally Posted by Tom Line

(where's Ether when you need him - this is right up his alley)....

How can I resist? :-)

There are two ways you might want to consider.

Assume you have a set of X,Y points, where Y is the actual Victor output (-12 to 12 volts) for a given X command (-1 to +1). You don't need a whole lot of points, just enough so that a series of line segments connecting the points is a good approximation of the Vic's behavior.

Divide all the Y values by 12^* so that Y is normalized to the range -1 to +1, then proceed as follows:


Method I

Enter the Y values into column A of Excel, and the X values into column B, then plot X versus Y (yes, X versus Y, not Y versus X - you want the inverse function).

Then use Excel's "trendline" function to fit a polynomial, X = f(Y). See attachment.

That polynomial will tell you what X value command you should use to get any desired Y output.

For example, say you want 68% output. Just put the value ".68" into the polynomial and it will tell you what your "X" command should be to get 68% output. In other words, instead of commanding .68 you would command f(.68).


Method II

Use a lookup table. Put the Y,X data into a table. If you want 68% output, search the Y column for the two values bracketing .68. In the example above, that would be .62 and .72. Take the X value corresponding to the .62 Y value (that would be .5) and the X value corresponding to the .72 Y value (that would be .6) and find the required X command as follows:

X = .5 + ((.68-.62)/(.72-.62))*(.6-.5)


^*or whatever the largest abs Y voltage is

[biojae]

Originally Posted by Tom Line

I'm curious how other teams attack their autonomous drivetrain commands.
.
I'd like to be able to combine these into a single system where the robot is able to drive 'curves' to it's destination point. However, I really don't know how to start mixing the Drive-distance and Turn PID's
Of course this all assumes tank drive.

How have other teams handled this mathematically? (Please try to keep it simple - system engineering was never my strong point).

If you have code already written to drive the robot to a point, you can easily make it drive a curve.

Right now I assume that you are just passing the destination point in?
So, the robot will just drive straight towards the destination?

An easy way to make it drive a curve is to make it drive a curve.
What you can do is add some code to [bezier, parabolic, cubic spline, etc] interpolate the points that you give it.
Once you have the curve, you can generate a new list of points to drive to.

Here is a general way that I would do it:

1. User inputted points goes to
2. interpolating function
3. points then trimmed based on curvature, basically remove extra points where the path is not changing that much  (optional)
4. drive to each one of those points.

To drive straight to a point, you can do a simple code like this:

botAngle = the bot's angle;
current position = where the bot thinks it is;
destination position = the destination point;

delta position = current position - destination position;
angle difference = inverse tangent of (delta position) -  botAngle;
distance to point = euclidean magnitude of delta position;

if distance to point less then threshold, go to the next point

//PID calculations to determine power to turn and to drive omitted 

left = drivePower - turnPower;
right = drivePower + turnPower;

// Normalize the values that you send to the drive motors
// this way, you don't have to worry about the bot not turning due to the values being saturated at +/- 1
maxValue = 1
if absolute value of left greater than maxValue, then maxValue =  absolute value of left;
if absolute value of right greater than maxValue, then maxValue = absolute value of right;

left = left / maxValue;
right = right / maxValue;

A little explanation:
That code is a naive implementation of way point navigation, in that it turns the bot to the right angle, and drives straight.
It does not account for anything, other then the driven distance and the current heading.
It will only drive forward, it won't reverse the direction of the drive in any circumstances, even if it is more efficient to turn 30 degrees and drive backwards (this might be preferable in some circumstances).
It doesn't account for the environment, it will ram into anything in its way.

It works like this:
The bot has a current location where it thinks that it is (thinks being the key word, its accuracy in estimated position will be diminished over time and distance) and it knows where it wants to be.
call these BotPos, and DestPos.
These can be treated as vectors, and the same math that works for vectors works for these values.
These values are subtracted to find the distance, and the angle that the bot needs to travel to.
This value is called delta position
delta position.X = (DestPos.X - BotPos.X);
delta position.Y = (DestPos.Y - BotPos.Y);
the angle is found by taking the inverse tangent of these new calculated values.
(this is the angle between the + X axis and the delta position vector)
once this value is found, it is subtracted from the bot's angle to find the Error value which is then fed to the turning PID controller.
the magnitude of the delta position vector is fed into the drive PID controller.

once the values have been calculated from the PID controller, they are combined in the "arcade" style equations.
to get around the problem of saturation, the outputs are normalized (but only if one, or both) of the values are larger than the saturation cut off. (in this case, 1 as that is what C++ and Java WPILib expects (I am not sure what Labview has as the max value))

These values are then sent to the drive motors.

[Mark McLeod]

For driving curves we usually just apply a distance/speed PID and give each side of our tank drive a different target-distance/target-speed to drive.
The inside (shorter distance) target speed is reduced proportional to the difference in distance. The shape of the arc can be affected by applying a +/- constant to the inside speed (if you're into french curves).

I've also applied a drive distance PID to one side, but a drive-straight PID to the other. You can feed the drive-straight PID an encoder reading to match that has been adjusted by the ratio of the two distances. Overshoots, like if another robot is shoving you, can get jumpy though.

[Jared Russell]

Here is the code that 254 (among others) used to linearize their Victors this year, for reference. 5th and 7th order polynomials are mixed together to provide the linearization.

Source: https://github.com/Team254/FRC-2011/...RobotState.cpp

double RobotState::victor_linearize(double goal_speed)
{
const double deadband_value = 0.082;
if (goal_speed > deadband_value)
goal_speed -= deadband_value;
else if (goal_speed < -deadband_value)
goal_speed += deadband_value;
else
goal_speed = 0.0;
goal_speed = goal_speed / (1.0 - deadband_value);

double goal_speed2 = goal_speed * goal_speed;
double goal_speed3 = goal_speed2 * goal_speed;
double goal_speed4 = goal_speed3 * goal_speed;
double goal_speed5 = goal_speed4 * goal_speed;
double goal_speed6 = goal_speed5 * goal_speed;
double goal_speed7 = goal_speed6 * goal_speed;

// Constants for the 5th order polynomial
double victor_fit_e1 = 0.437239;
double victor_fit_c1 = -1.56847;
double victor_fit_a1 = (- (125.0 * victor_fit_e1 + 125.0 * victor_fit_c1 - 116.0) / 125.0);
double answer_5th_order = (victor_fit_a1 * goal_speed5
+ victor_fit_c1 * goal_speed3
+ victor_fit_e1 * goal_speed);

// Constants for the 7th order polynomial
double victor_fit_c2 = -5.46889;
double victor_fit_e2 = 2.24214;
double victor_fit_g2 = -0.042375;
double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2 + victor_fit_g2) - 116.0) / 125.0);
double answer_7th_order = (victor_fit_a2 * goal_speed7
+ victor_fit_c2 * goal_speed5
+ victor_fit_e2 * goal_speed3
+ victor_fit_g2 * goal_speed);


// Average the 5th and 7th order polynomials
double answer = 0.85 * 0.5 * (answer_7th_order + answer_5th_order)
+ .15 * goal_speed * (1.0 - deadband_value);

if (answer > 0.001)
answer += deadband_value;
else if (answer < -0.001)
answer -= deadband_value;

return answer;
}

[Ether]

Originally Posted by Jared341

5th and 7th order polynomials are mixed together to provide the linearization.

What is the point of creating two separate polynomials and then adding them together?

[Jared Russell]

Originally Posted by Ether

What is the point of creating two separate polynomials and then adding them together?

Not sure. I will try to hunt down the author of the function and ask this question.

[Thad House]

One thing i have found is that in the RobotDrive class for all 3 languages there is a ratio curve function. Into this you input a speed and a curve, and it does all of the ratio functions for both drive sides. It actually works fairly well. In your case, you could do the distance PID to get the speed and the turn PID to get the curve. then just enter those numbers in every time.

[Joe Ross]

Originally Posted by Ether

What is the point of creating two separate polynomials and then adding them together?

Here's some data you can play with. http://forums.usfirst.org/showthread...ghlight=jaguar.

When I looked at it years ago, I wasn't happy with a 5th order polynomial approximation. We used the following lookup table below with the IFI system (input range 0-254, output range 0-254). It wasn't perfect, but greatly improved over nothing.

0	10	10	10	10	10	10	10	10	10	10	10	10	10	10	11	11	11	11	11	11	11	11	11	12	12	12	12	12	13	13	13	13	13	13	13	13	13	14	14	14	14	14	14	14	14	14	14	15	15	15	15	15	15	15	16	16	17	17	17	17	17	18	18	18	18	18	18	19	19	19	19	19	19	20	20	20	20	20	20	20	21	21	22	22	23	23	23	23	24	24	24	25	26	26	27	27	28	29	29	30	30	31	32	33	33	34	35	36	37	38	39	41	43	45	47	48	51	52	55	59	66	68	73	79	84	92	127	127	143	144	144	144	144	144	145	145	145	145	145	145	145	145	145	145	146	146	146	146	146	146	146	146	146	146	146	146	147	147	147	147	147	147	147	148	148	148	148	149	149	149	149	149	149	149	149	150	150	150	150	150	150	150	150	151	151	151	151	151	151	151	151	152	152	152	152	154	154	154	154	155	155	155	156	156	155	157	157	157	157	157	157	157	159	159	159	159	160	160	160	161	162	163	163	164	164	164	165	166	167	168	169	169	170	171	172	173	174	175	176	177	178	179	182	183	188	191	192	193	196	203	207	211	220	229	254

[Tom Line]

Thanks guys.

In the end, I kind of expected some derivation-based way of doing this as opposed to brute force best-fit-line equations, but this makes it easier for me.

By removing the 'tail' of the victor response down at the intermediate pulse widths that generally fall within the deadband of robot motion due to friction, I was able to get a very nice fit on a couple of 6th order polynomials. I'll poke through the rest of it tonight.

I WISH I trusted the jaguars enough to use them on our drivetrain. Perhaps I'm being paranoid, but I still worry about them burning out on fast full-reverse to full-forward transitions. The voltage mode and their linearity looks wonderful!

[Jared Russell]

Originally Posted by Tom Line

I WISH I trusted the jaguars enough to use them on our drivetrain. Perhaps I'm being paranoid, but I still worry about them burning out on fast full-reverse to full-forward transitions. The voltage mode and their linearity looks wonderful!

You and me both. The 2011 hardware beta test teams have been experimenting with a new version of the Jaguar firmware that implements an "output ramp" function in both PWM and CAN modes to help deal with this case, so hopefully that will help.

[Tom Line]

Originally Posted by Jared341

You and me both. The 2011 hardware beta test teams have been experimenting with a new version of the Jaguar firmware that implements an "output ramp" function in both PWM and CAN modes to help deal with this case, so hopefully that will help.

Yeah, we've been playing with it, but the practical side of me screams that I don't want that tiny delay between full back and full forward. The delay is infintesimal and practically we wouldn't ever see it..... it's just the engineer in me crying out to fix the problem and not the symptom.