Help with (IME) PID Straight Drive

Hello all. I’ve started experimenting with PIDs (well mostly the P aspect) and wanted some advice on my PID function that I want to use in my autonomous code. I’ve skimmed through George Gillard’s Introduction along with the Aura article. So far, here is what I’ve created without having tested to determine kP. Any and all advice is appreciated :slight_smile:

float kP = 1;

void drivePID (int setPoint) // Sensor-based drive function.
// A setPoint value of 360 is a distance of 12.6 inches.
// To get the distance travelled, use the equation d = PI*(diameter of wheel)*(setPoint value/360).
	resetMotorEncoder (LeftMiddle); // Reset the encoders.
	resetMotorEncoder (RightMiddle);

	while (nMotorEncoder (LeftMiddle) < setPoint && nMotorEncoder (RightMiddle) < setPoint)
		int errorLeft;
		int errorRight;
		errorLeft = setPoint - nMotorEncoder (LeftMiddle); // Calculate the error for each side.
		errorRight = setPoint - nMotorEncoder (RightMiddle);
		setLeftDrive (errorLeft*kP); // Set the power to each side based on the error*kP
		setRightDrive (errorRight*kP);
	setLeftDrive (STOP);
	setRightDrive (STOP);