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