PID_Drive.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           frontL,        tmotorVex393TurboSpeed_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port3,           backL,         tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           backR,         tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           frontR,        tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//CONSTANT VALUES USED FOR TUNING
float kp = 0.15;
float ki = 0.005;
float kd = 2;
float ks = 0.75; // ratio of left drive motors to right

//CONSTANT VALUES (INCHES)
float inchesPerTile = 24.25;
float ticksPerInch = 20.00275229;

//PROPORTIONAL, INTEGRATON, DERIVATIVE LOOP VARIABLES
float proportionalL;
float derivativeL;
float integralL;
float integralLimit=40;
float integralActiveZone=100;

//ERROR VARIARBLES
float errorPL;
float errorIL;
float errorDL;
float power;

//CONVERT TILES TO TICKS
float tilesToTicks(float tiles)
{
	return tiles*inchesPerTile*ticksPerInch;
}

void initialize(){
	//ENCODERS SET TO 0
	nMotorEncoder[frontR]=0;
	nMotorEncoder[frontL]=0;
	wait1Msec(500);
}

void moveStraight(float numOfTiles){
	clearTimer(T1);
	initialize();
	int dummyCounter=0;
	float targetTicks = tilesToTicks(numOfTiles);

	while(time1[T1]<4000 && dummyCounter<100){
		//PROPORTIONAL
		errorPL=targetTicks-nMotorEncoder[frontL];
		proportionalL=errorPL*kp;

		//TIMER
		if (abs(errorPL)<25){
			dummyCounter+=1;
		}

		//INTEGRAL
		if(abs(errorPL)<integralActiveZone)
		{
			errorIL+=errorPL;
			} else {
			errorIL = 0;
		}
		integralL=errorIL*ki;
		if(integralL > integralLimit)
		{
			integralL=integralLimit;
		}

		//DERIVATIVE
		derivativeL=(errorPL-errorDL)*kd;
		errorDL=errorPL;
		if(errorPL==0)
		{
			derivativeL=0;
		}

		//POWER FOR MOTORS AND OVERALL POWER
		power=proportionalL+derivativeL+integralL;
		motor[backR]=power;
		motor[backL]=power*ks;
		motor[frontR]=power;
		motor[frontL]=power*ks;

		wait1Msec(20);
	}
}

task main()
{
	moveStraight(2);
	delay(500);
	moveStraight(2);
	delay(500);
	moveStraight(-2);
	delay(500;
	moveStraight(-2);
}