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

VEX V5 Technical Support
#1 
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 adjustment;
    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
        Fr.spin(directionType::fwd, speed - adjustment, velocityUnits::rpm);
        Br.spin(directionType::fwd, speed - adjustment, velocityUnits::rpm);
        Bl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm);
        task::sleep(5); //don't hog the cpu
    } //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;
    float adjustment;
    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
        Fr.spin(directionType::fwd, -(speed + adjustment), velocityUnits::rpm);
        Br.spin(directionType::fwd, -(speed + adjustment), velocityUnits::rpm);
        Bl.spin(directionType::fwd, speed + adjustment, velocityUnits::rpm);
        task::sleep(5); //don't hog the cpu
    }
    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?

#2

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?

#3

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.

#4

@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?

#5