driftMoveStraight.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    gyro,           sensorGyro)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           leftD,         tmotorVex393HighSpeed_MC29, openLoop, driveLeft, encoderPort, I2C_1)
#pragma config(Motor,  port3,           rightD,        tmotorVex393HighSpeed_MC29, openLoop, driveRight, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//WORLD VARIABLES
float targetHeading;
float ticksPerRev=362;
float circ=4*PI;
float inchesPerTile=24.25;

//INITIALIZE
void initialize(){
	//ENCODERS SET TO 0
	wait1Msec(100);
	nMotorEncoder[leftD]=0;
	nMotorEncoder[rightD]=0;
	wait1Msec(100);
}
void gyroInitialize(){

}
float tilesToTicks(int tiles){
	return tiles*inchesPerTile*(1/circ)*ticksPerRev;
}
//PID DRIVE
void moveStraight(float currentAngle, int tiles){
	//ANGLE
	float startAngle=SensorValue[in1]; //ASSUME ROTATING CLOCKWISE IS POSITIVE
	//COUNTERS
	int dummyCounter;
	//RIGHTSIDE
	float proportionalR;
	float integralR;
	float derivativeR;
	//LEFTSIDE
	float proportionalL;
	float integralL;
	float derivativeL;
	//LIMITERS
	float integralActiveZone=100;
	float integralLimit=40;
	//PROPORTIONAL ERROR
	float errorGyro;
	float errorPR;
	float errorPL;
	//INTEGRAR ERROR
	float errorIR;
	float errorIL;
	//DERIVATIVE ERROR
	float errorDR;
	float errorDL;
	//CONSTANTS
	float kp=0.15;
	float ki=0;
	float kd=2;
	//GYRO CONSTANTS
	float gyrokp=0.001;
	//POWER VALUES
	float powerR;
	float powerL;
	float avePower;
	float gyroPower;
	clearTimer(T1);
	while(time1(T1)<1500&&dummyCounter<60){
		//PROPORTIONAL
		errorPR=tilesToTicks(tiles)-nMotorEncoder[rightD];
		errorPL=tilesToTicks(tiles)-nMotorEncoder[leftD];
		proportionalR=errorPR*kp;
		proportionalL=errorPL*kp;
		//GYRO CONSTANTS WITH VALUE
		errorGyro = ((SensorValue(in1)/10)-currentAngle); //VARIED BASED OFF OF THE CLOCKWISE/COUNTERCLOCKWISE
		//COUNTER
		if(errorPL<40 || errorPR<40){
			dummyCounter++;
		}
		//INTEGRAL
		if(abs(errorPR)<integralActiveZone || abs(errorPL)<integralActiveZone){
			errorIR+=errorPR;
			errorIL+=errorIR;
		}
		else{
			errorIR=0;
			errorIL=0;
		}
		integralR=errorIR*ki;
		integralL=errorIL*ki;
		if(integralR>integralLimit && integralL>integralLimit){
			integralR=integralLimit;
			integralL=integralLimit;
		}
		//DERIVATIVE
		derivativeR=(errorPR-errorDR)*kd;
		derivativeL=(errorPL-errorDL)*kd;
		errorDR=errorPR;
		errorDL=errorPL;
		if(errorPR==0 && errorPL==0){
			derivativeR=0;
			derivativeL=0;
		}
		//DRIVE AND GYRO POWER
		powerR=proportionalR+integralR+derivativeR;
		powerL=proportionalL+integralL+derivativeL;
		avePower=(powerR+powerL)/2;
		gyroPower=errorGyro*gyrokp;
		motor[leftD]=avePower-gyroPower;
		motor[rightD]=avePower+gyroPower;
	}
}
task main()
{



}