We are using a PID loop that was converted from a ROBOTC project. When used in a test base with 4 motors and 2 encoders, it worked just fine. When applied to a base with the same specs (4 motor, 2 encoder (1 reversed encoder)), it is broken. What happens is the robot moves forward for infinity mo matter how small or big the error is. No matter what we do to the constants, the result is the same as well. Mathematically, the loop checks out so I’m not sure what the problem is. Here is my code included:

void pidDrive(int target)

{

float pid_Kp = 0.1;

float pid_Ki = 0.0;

float pid_Kd = 0.0;

```
int ErrorL;
int ErrorR;
float LastErrorL;
float LastErrorR;
float CurrentSensorValueL;
float CurrentSensorValueR;
float DriveL;
float DriveR;
float IntegralL;
float IntegralR;
float DerivativeL;
float DerivativeR;
```

E1.resetRotation();

E2.resetRotation();

```
LastErrorL = 0;
LastErrorR = 0;
IntegralL = 0;
IntegralR = 0;
while(true)
{
CurrentSensorValueL = -E2.value(); //encoder reads negative
CurrentSensorValueR = E1.value();
//Proportional
ErrorL = target - CurrentSensorValueL;
ErrorR = target - CurrentSensorValueR;
//Integral
if(pid_Ki != 0)
{
if(((abs(ErrorL) + abs(ErrorR)) / 2) < 50)
{
IntegralL = IntegralL + ErrorL;
IntegralR = IntegralR + ErrorR;
if(IntegralR > 10){
IntegralR = 10;
}
else if (IntegralR < -10){
IntegralR = -10;
}
if(IntegralL > 10){
IntegralL = 10;
}
else if (IntegralL < -10){
IntegralL = -10;
}
else
{
IntegralL = 0;
IntegralR = 0;
}
}
}
else
{
IntegralL = 0;
IntegralR = 0;
}
//Derivative
DerivativeL = ErrorL - LastErrorL;
LastErrorL = ErrorL;
DerivativeR = ErrorR - LastErrorR;
LastErrorR = ErrorR;
//Calculating the motor speed
DriveL = (pid_Kp * ErrorL) + (pid_Ki * IntegralL) + (pid_Kd * LastErrorL)/12;
DriveR = (pid_Kp * ErrorR) + (pid_Ki * IntegralR) + (pid_Kd * LastErrorR)/12;
//Limits power to 12 volts
if(DriveL > 12){
DriveL = 12;
}
else if(DriveL < -12){
DriveL = -12;
}
if(DriveR > 12){
DriveR = 12;
}
else if(DriveR < -12){
DriveR = -12;
}
BL.spin(forward,DriveL,voltageUnits::volt);
FL.spin(forward,DriveL,voltageUnits::volt);
BR.spin(forward,DriveR,voltageUnits::volt);
FR.spin(forward,DriveR,voltageUnits::volt);
```

wait(10,msec);

```
}
```

}