PID for encoders and Gyro

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]));

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

{
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();

{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!