# PID Isn't Working and We're Not Sure Why

``````void pidforward(float x)
{
x=x/(12.56636*(3/2));
initialize;
double error = Fl.rotation(rotationUnits::rev) - Fr.rotation(rotationUnits::rev);
double prevError; //defines a bunch of variables with useless names
double integral;
double derivative;
double speed;
while(((Fl.rotation(rotationUnits::rev)+Fr.rotation(rotationUnits::rev))/2)>x) //while the encoder ticks are less than x, do this
{
prevError = error; //keeps a history of error (you need two points if you want to have a slope)
error = Fl.rotation(rotationUnits::rev) - Fr.rotation(rotationUnits::rev); //calculate the difference between the two sides' encoder ticks
derivative = error-prevError; //calculates the slope of the change in error
integral = integral+error; //sums the integral of the error over time
adjustment = Kp*error + Ki*integral + Kd*derivative; //sets the adjustment to the PID formula, Kp, Ki, and Kd are constants that need to be fine tuned
speed = (100*(x-((Fl.rotation(rotationUnits::rev)+Fr.rotation(rotationUnits::rev))/2))/x)+20; //determines the base speeds of the motors, P loop in terms of distance
Fl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm); //adjusts the base speeds of the motors to keep the robot straight
} //When the encoder ticks reach the desired value, exit while loop
Fl.stop(); //stop
Fr.stop();
Br.stop();
Bl.stop();
}

void pdGyroTurnRight(float x)
{
float currentVal = Gyro1.value(rotationUnits::deg);
float desired = currentVal+x;
double speed;
float error;
float prevError;
float derivative;
while(Gyro1.value(rotationUnits::deg)<desired)
{
prevError = error; //keeps a history of error (you need two points if you want to have a slope)
error = Gyro1.value(rotationUnits::deg) - desired; //calculate the difference between the two sides' encoder ticks
derivative = error-prevError; //calculates the slope of the change in error
adjustment = Kp*error + Kd*derivative; //sets the adjustment to the PD formula, Kp and Kd are constants that need to be fine tuned
speed = 20;
Fl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm); //adjusts the base speeds of the motors to keep the robot straight
}
Fl.stop();
Fr.stop();
Br.stop();
Bl.stop();
}
``````

Our coder’s too lazy to post it, so I will. This is our PID code. Instead of working, it causes our robot to spin endlessly in circles. We’ve tried changing signs on some of the motors’ correction commands but it doesn’t work. Is there something else that’s wrong with it?

Just to specify, it doesn’t bother to turn on the left motors at all. Is there something wrong with our PID formula that might cause that?

I haven’t picked through it carefully, but it looks like you’re sort of doing -pid inside pid. The formula for speed is someone just negative of the formula for error, and error factors into each of the terms of adjustment. That means speed and adjustment are somewhat negatives of each other. If they’re close enough to negatives of each other, the left motors will be passed values of 0 and the right motors will be passed values of double something. Try not doing that dynamic bit for speed at all, just working with adjustment. The idea with pid is that it already handles slowing down when you’re close, so you don’t need to try to include this again.

@callen

I’m using a PID loop to minimize error between the two sides of the robot, but I’m also using a P controller to slow down as it approaches the target encoder value. Is this unnecessary?