PID for encoders and Gyro

VEX V5 General Discussion Technical Discussion
#1

Greetings to everyone! We are currently in the programming process, and we are trying to use PID for our drive base with encoders, and for our turns with a gyro. Here is what we currently have:

void Base_Forward(float Distance_I, float Seconds)
{
	Distance_I *= 28.66; //360/(4.0*3.14)
	Distance_I = round(Distance_I);
  Seconds*= (1000/15);


	float Power, Error, Integral, Derivative, Prev_Error, Error_E;
	SensorValue[Encoder_R]=0;
	SensorValue[Encoder_L]=0;
	//while(-1*(SensorValue[Encoder_R]) < Distance_I)
	for(int counter =0; counter<Seconds;	counter++)


	{
		Error=(Distance_I - -1*(SensorValue[Encoder_R]));

		//Calculate error to adjust

		Integral=Integral+Error;																	 //Calculate integral part of correction

		if(Error==0)																						 //Adjust integral error
		{
			Integral=0;
		}

		Derivative=Error-Prev_Error;															 //Calculate derivative part of correction
		Prev_Error=Error;

		Power=Error*Kp+Integral*Ki+Derivative*Kd;									 //Set power correction for motors

		if(Power>127)
		{
			Power=127;
		}

		if(-1*(SensorValue[Encoder_R]) > -1*(SensorValue[Encoder_L]))			 //Adjust motor power for encoder difference / Right > Left
		{
			Error_E=(-1*(SensorValue[Encoder_R])- -1*(SensorValue[Encoder_L]));

			motor[downleft] = Power;
			motor[yl] = Power;
			motor[downright] = Power-Error_E;
			motor[yr] = Power-Error_E;
		}

		else if(-1*(SensorValue[Encoder_R]) < -1*(SensorValue[Encoder_L]))			 //Adjust motor power for encoder difference / Left > Right
		{ Error_E= (-1*(SensorValue[Encoder_L])- -1*(SensorValue[Encoder_R]));
			motor[downleft] = Power-Error_E;
			motor[yl] = Power-Error_E;
			motor[downright] = Power;
			motor[yr] = Power;
		}
		else																										 //Set power for motors w/o difference
		{
			motor[downleft] = Power;
			motor[yl] = Power;
			motor[downright] = Power;
			motor[yr] = Power;
		}
		wait1Msec(15);
	}
	motor[downleft] = 0;
	motor[yl] = 0;
	motor[downright] = 0;
	motor[yr] = 0;
}

void Turn_Right(float Degree, float Seconds)//**********************************************************************************************************************
{
 SensorValue[in8]=0;
 Degree= round(Degree*(-10));
 float Power, Integral, Derivative, Prev_Error;
 float Error;
  Seconds*= (1000/15);


 for(int counter =0; counter<Seconds;	counter++)															 //Turn Right
	{
		Error=(Degree - SensorValue[in8]);						           //Calculate error to adjust
		Integral=Integral+Error;	//Calculate integral part of correction
		datalogClear();


			if(Error==0)																						 //Adjust integral error
			{Integral=0;}

		Derivative=Error-Prev_Error;															 //Calculate derivative part of correction
		Prev_Error=Error;

		Power=-1*(Error*Gp)+Integral*Gi+-1*(Derivative*Gd);									 //Set power correction for motors

			if(Power>100)
			{
				Power=100;
				}

			motor[downleft] = Power;
			motor[yl] = Power;
			motor[downright] = -Power;
			motor[yr] = -Power;

			wait1Msec(15);
		}

}

We are trying to make it more accurate, so if anybody can help us with suggestions we would really be thankful. Thanks in advance!

#2