auton.c

static const float kp = 10;

void move(int leftPower, int rightPower) {
	motor[left1] = leftPower;
	motor[left2] = leftPower;
	motor[right1] = rightPower;
	motor[right2] = rightPower;
}

float driveForward(int power, float distance) {

	SensorValue[leftEncoder] = 0;
  SensorValue[rightEncoder] = 0;

	int error = 0;
	int slavePower = power; // subtract based upon observations

	float ticksNeeded = (360 / CIRC) * distance;
	float totalTicks = 0.0;


	while(abs(totalTicks) < ticksNeeded) {
		move(slavePower, power);
		
		error = SensorValue[rightEncoder] - SensorValue[leftEncoder];
		slavePower += (int) (error / kp);
		writeDebugStreamLine("Moved: %f", (totalTicks * CIRC)/360);
		
		SensorValue[leftEncoder] = 0;
    SensorValue[rightEncoder] = 0;
		wait1Msec(20);
    totalTicks += (SensorValue[rightEncoder] + SensorValue[leftEncoder])/2;

 }
 	writeDebugStreamLine("Moved: %f", (totalTicks * CIRC)/360);
 	move(0, 0);
	return totalTicks;
}

task autonomous()
{
 	float distance = driveForward(70, 15);
	wait1Msec(500);
 	writeDebugStreamLine("Total Moved: %f", (distance * CIRC)/360);
}

debugStream.txt

Moved: 0.000000
Moved: 0.000000
Moved: 0.000000
Moved: 0.108778
Moved: 0.362592
Moved: 0.688925
Moved: 1.087776
Moved: 1.450368
Moved: 1.849220
Moved: 2.284330
Moved: 2.755700
Moved: 3.190810
Moved: 3.734699
Moved: 4.278586
Moved: 4.822475
Moved: 5.402622
Moved: 5.982770
Moved: 6.562917
Moved: 7.179323
Moved: 7.831989
Moved: 8.484655
Moved: 9.137321
Moved: 9.753727
Moved: 10.333875
Moved: 10.914021
Moved: 11.421651
Moved: 12.038057
Moved: 12.654464
Moved: 13.234611
Moved: 13.887278
Moved: 14.467424
Moved: 15.083831
Total Moved: 15.083831