PID help

I traditionally do drive straight PID with 2 control loops. PID for distance and PID (really just P but ID are coded) for correcting the drive. Code below


straight.error=straight.target -((SensorValue[leftDrive]-initialLeft) + (SensorValue[rightDrive]-initialRight));
angle.error=((SensorValue[leftDrive]-initialLeft)-(SensorValue[rightDrive]-initialRight)); 

int StraightOut = straight.kP*straight.error + straight.kI*straight.integral + straight.kD*straight.derivative;
int AngleOut = usedUltra?0:angle.kP*angle.error + angle.kI*angle.integral + angle.kD*angle.derivative;
StraightOut = limit(minSpeed,maxSpeed,StraightOut);

setLeftWheelSpeed(StraightOut-AngleOut);
setRightWheelSpeed(StraightOut+AngleOut);

The essence is that they both drive at some agreed upon speed and as the robot drifts the fast side slows down and the slow side speeds up. Note the limit is fairly important because if the agreed upon speed is already full speed you will only change the speed of 1 side(127-20,127+20 = 107,147) thus correcting slower and drifting farther away.