[mikets]

Sorry, don't know Java that well.

This year, we have improved using PID by developing a generic PID drive library for C++. The main methods of the PID drive library are:

TrcPIDDrive::TrcPIDDrive(RobotDrive *drive,
                                  TrcPIDCtrl *pidCtrlDrive,
                                  TrcPIDCtrl *pidCtrlTurn,
                                  PIDDriveNotify *notify = NULL);
void TrcPIDDrive::SetTarget(float distSetPt,
                                       float distTolerance,
                                       float angleSetPt,
                                       float angleTolerance,
                                       bool fStopOnTarget);

So when creating the PIDDrive object, you provide two PID controller objects, one for driving straight and one for turning. You also optionally provide a notify object so that you get notified when PID drive is done.
So in autonomous, all we need to do is something like this:

// Drive forward 5 ft to within 1 inch tolerance and then stop.
pidDrive->SetTarget(60.0, 1.0, 0.0, 0.0, true);
WaitForEvents(pidDriveEvent);
 
// Turn right 90 degrees to within 1 degree tolerance and then stop.
pidDrive->SetTarget(0.0, 0.0, 90.0, 1.0, true);
WaitForEvents(pidDriveEvent);
 
...

With this library, once we have an autonomous strategy down, we can usually code it in one day.