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

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).

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.

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.

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

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)…

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

*

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.

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.

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.

``````double RobotState::victor_linearize(double goal_speed)
{
const double deadband_value = 0.082;
if (goal_speed > deadband_value)
else if (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)
else if (answer < -0.001)

}
``````

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.

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.

Here’s some data you can play with. http://forums.usfirst.org/showthread.php?t=10182&highlight=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
``````

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!

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.

I got a pretty nice fit using just 2 cubics by splitting the curve into 2 sections.

Cells A2-A21 and C2-C21 are pulse and volts data from the link you provided. I normalized that data to the range +/-1 for command and output in cells B2-B21 and D2-D21.

I copied the positive half of the curve to cells A27-B38, reversing the cmd & out to get the inverse function, and divided it into 2 sections: cells E27-F33 and E35-F41.

Then I fit a cubic to each section. The equations are in the two graphs at the bottom.

I spliced the 2 cubics together and you can see the net result at the top. It’s a pretty nice fit, and for not much math.

To use it, test your desired (positive) command. If it’s less than 0.842 use the first cubic, otherwise use the second. If it’s zero, use zero.

You could use the same cubics for the negative half of the curve, or create new curves for that half.

*

Thanks guys. It’s been a long time since I’ve had one of those AH HA! moments…

I guess I never realized how far from linear the Victors can be. We’ve used joystick input modification VI’s in the past to make the bots easier to drive, but I never put 2 and 2 together and realized a huge portion of that issue is caused by the Victor’s response.

We noticed last year that the logitech dual action game pad (at least one of them) had a hokey joystick that wasn’t even close to linear. We threw that one away, but after modeling the cim response like this, I’m almost tempted to do the same to the game pad joysticks so I can linearize them too.

Here’s my final result using a single 6th order polynomial.

Darn do I love learning.

(Note: The numbers between .08 and -.08 are not representative in this graph - those are our dead band and will be modified to zero output).

Pertinent response RE: 5th and 7th order polynomials here: http://www.chiefdelphi.com/forums/showthread.php?t=98267&page=2

The Victor data from the link Joe provided:

``````pulse		volts
1.04		-12
1.07		-11.99
1.12		-11.7
1.27		-11.27
1.36		-10.12
1.41		-8.52
1.46		-3.8
1.48		-0.8
1.49		0
1.55		0
1.56		0.8
1.58		2.8
1.6		5.4
1.65		8.9
1.7		10.1
1.75		10.87
1.8		11.25
1.85		11.55
1.9		11.8
2		12
``````

…normalized to +/-1:

``````cmd		out
-1		-1
-0.9375		-0.999166667
-0.833333333	-0.975
-0.520833333	-0.939166667
-0.333333333	-0.843333333
-0.229166667	-0.71
-0.125		-0.316666667
-0.083333333	-0.066666667
-0.0625		0
0.0625		0
0.083333333	0.066666667
0.125		0.233333333
0.166666667	0.45
0.270833333	0.741666667
0.375		0.841666667
0.479166667	0.905833333
0.583333333	0.9375
0.6875		0.9625
0.791666667	0.983333333
1		1
``````

Swap the command and the output
(to get inverse function for linearization)
and select only the positive commands:

``````out		cmd
0		0.0625
0.066666667	0.083333333
0.233333333	0.125
0.45		0.166666667
0.741666667	0.270833333
0.841666667	0.375
0.905833333	0.479166667
0.9375		0.583333333
0.9625		0.6875
0.983333333	0.791666667
1		1
``````

Fit a rational function model to the data:

``````(p1*x^4+p2*x^3+p3*x^2+p4*x+p5) / (x^2+p6*x+p7)
``````

Here are the p1…p7 model parameters:

``````-0.43231217703332, 1.136975493080563, -1.043577770662604, 0.26894582879701, 0.069994693624, -2.151432791900253,  1.151458861241842
``````

Attached graph shows the actual data (blue), the model (red), and the error (green). The error is multiplied by 10 to make it visible. The maximum error is 0.01 (1% of full scale).

**

Here is an interesting post from the old IFI Robotics forum that describes what is going on “under the hood” of a Victor.

Note that there are only 94 discrete output states in each direction. This would make a look-up table a pretty attractive solution. Also, “Be aware that it may take 3 or 4 input steps before the Victor changes its Output.”

EDIT: Although this post seems to contradict the last bit about latency: http://www.ifirobotics.com/forum/viewtopic.php?t=375