PID program

Hello everyone!
In the 2020 2021 game, a lot of accuracy is needed! The risers need to be grabbed in just the right position and the base risers need to be scored just inside the goals and not sitting on the goal perimeter. A very complicated yet very worth it approach is PID turning/driving. My team is still working out the program but we got an “Advanced Drive Control” program from China we’d like to share to help you guys with the coding.

The basic functions are in this document: Note this document does not explixitly call the variables Kp Ki and Kd (and i doubt it has a Kd).
Also the useful functions are at the bottom. Calculate error and Calculate Gap Distance are the error calculating functions.

For those of you who quickly jump to the conclusion that this isnt PID based on the first couple of functions, I would kindly advise you to scroll down to the bottom, under the "advanced drive controls"

This document below is the copy and pasted section of the actual PID function for those of you who don’t scroll:

IMPORTANT: please use this file as a jumping off point for your own code. I hope by sharing this program it will help us all understand PID programs better and be able to implement our own versions of it. the VEX community is all about sharing information and I feel like PID programs is a very guarded secret that beginner teams do not get access to.

4 Likes

Thank you loads. I needed this.

1 Like

This is driving code, but don’t see any PID in it.

1 Like

Yeah, this doesn’t use PID. This video does a good job explaining what PID is:

1 Like

Man really made a PID tutorial without the PID

12 Likes

I swear this thing is PID the error analysis functions are definitely PID. My team has read this code over many times. It just doesnt name the variables Kp, Ki, and Kd. Even if you may claim this isn’t PID, our team has tested it and it drives and works just like a PID program.

1 Like

Milo can you tell me how the conveyor bot works? I made a similar robot but I have trouble keeping the risers straight.

1 Like

The names of the variables don’t matter, it’s the concepts and algorithms. It may work, but it’s not PID.

1 Like

This is not a PID algorithm. Let’s start by taking a look at the actual loop:

       // Calculate error
       float error = adcCalculateError(initialDegree, movedDegree, step);
       writeDebugStreamLine("adcMove(), error = %f", error);
       accError += error;

       distance -= step;
   }
   while (distance > 0);

   accError *= FINE_TUNE_RATIO;
   // Only adjust the error while error > 10mm
   if (abs(accError) > 10)
   {

       switch (moveCommand)
       {
           case FORWARD:
           case BACKWARD:
               horizontalMove(accError, 50);
                       writeDebugStreamLine("adcMove(), horizontalError = %f", accError);
               break;

           case LEFTWARD:
           case RIGHTWARD:
               verticalMove(accError, 50);
                       writeDebugStreamLine("adcMove(), verticalError = %f", accError);
       }
   }

Accumulating the error is in fact done in PID, but not for the purpose demonstrated here. You simply do it to move the drivetrain when the error is significant. Once it is, you call either horizontalMove() or verticalMove().

We’ll first analyze horizontalMove()

bool horizontalMove(float distance, short speed)
{
	resetGyro(gyroSensor);
	resetMotorEncoder(midMotor);
	startTask(keepStraight);
	setMotor(midMotor, speed);
	float motorDegree = distance * CENTER_WHEEL_SPEED;
	waitUntil(abs(getMotorEncoder(midMotor)) >= motorDegree);
	stopTask(keepStraight);
	stopMotor(midMotor);
	return true;
}

This does little more than spin the motor until it reaches a certain number of degrees, which you determine by calculating the error. It runs your algorithm to drive straight during this, but that is not PID.

You actually don’t need to call this function in a while loop since it won’t end until the bot reaches its target. You could simply measure out the distance you need to drive and call the function passing your measurement as a parameter.

Now let’s look at verticalMove()

bool verticalMove(float distance, short speed)
{
    float motorDegree = distance * DRIVETRAIN_SPEED;
    moveMotorTarget(leftMotor, motorDegree, speed);
    // moveMotorTarget(rightMotor, motorDegree, speed * -1);
    moveMotorTarget(rightMotor, motorDegree, speed);
    // Holds program flow until the motors comes to a complete stop.
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    return true;
}

This essentially does the same thing as horizontalMove(), just using moveMotorTarget() instead of starting the motor and stopping it when the encoder reads that it reached its target, and not using your keepStraight algorithm.

Notice how for both of these, the speed of the motors will be the same no matter how large your error is. This is not how PID works.


PID stands for Proportional, Integral, Derivative. It is a feedback loop that uses a proportional measurement of a system’s error (P), the accumulation of the system’s error (I), and the rate of change of that error (D) to accurately drive the error of the system to 0.

Starting with the Proportional (P) term, we simply multiply our error by a constant kp. As the error decreases, so will the Proportional term, meaning in this case that the robot will slow down as it nears its target. The problems with this alone are steady state error and that it can overshoot its target and oscillate.

To solve the overshooting problem, we use the Derivative (D) term. This is found by multiplying the derivative of our error by a constant kd. In this case, the Derivative term allows us to slow our robot down proportionally to its current speed. The faster the robot is moving, the more the Derivative term does to lower its speed, and vice versa. However, we can still run into the problem of steady state error.

Steady state error is when the error of a system is not 0, but remaining constant. For example, if the robot is 20mm from the target, the Proportional term may not be enough to overcome friction, and since the robot isn’t moving the Derivative term will be 0. In a lot of cases, this isn’t enough to matter. However, over the course of a programming skills run, it could add up and cause inconsistency.

To solve this, we use the Integral term. This is calculated by taking the integral of the error and multiplying it by a constant ki. Since the integral is an accumulation, this will continue to grow as long as the error of the system isn’t 0, so it will be able to eliminate steady state error. It also runs the risk of getting too large though, because the system may start with a large error. Thus, we put restrictions on the Integral term such as only allowing it to accumulate a certain distance from the target, and putting a hard limit on how high it can get.

Here is an example of a PID loop:

void PID(double target) {    //target is the desired state of the system
double kp = 0.33;     
double ki = 0.0005;
double kd = 0.2;
//you need to tune the constants for yourself;

double proportion;
double totalError;
double integral;
double integralActiveZone = 90;       //you need to tune this with the constants
double integralPowerLimit = 50 / ki;  //this will probably need to be adjusted too, depending on the velocity units
double derivative;
double finalPower;

double error = target;
double lastError = target;
sleepMs(50);
while(abs(error) > 3){    //will stop when the system is within 3 units of target, in this case degrees;
    error = target - motor.rotation(rotationUnits::deg);
    if(error == 0){
        break;
    }
    proportion = kp * error;
    
    if(abs(error) < integralActiveZone && error != 0){
    totalError += error;
    } 
    else totalError = 0.0; //sets the Integral term to 0 if it's too far from the target
    
    //hard limit on the integral term
    if(totalError > integralPowerLimit){
        totalError = integralPowerLimit;
    }
    if(totalError < -integralPowerLimit){
        totalError = -integralPowerLimit; 
    }

    integral = ki * totalError;
    
    derivative = kd * (error - lastError);
    lastError = error;
    
    if(error == 0){
        derivative = 0;
    }

    finalPower = 0.5 * (proportion + integral + derivative); //.5 is the gain; you can adjust that as needed
    motor.spin(fwd, finalPower, velocityUnits::pct);
    sleepMs(20);
}
motor.stop();

Hopefully this helps clear up some of the confusion.

Edit: @Illyana caught a missed negative in the Integral term limit

11 Likes

VEXForum Wiki is a great friend

4 Likes

buddy, please scroll down to the ADCmove and ADCturn functions and analyze them. I hope you can see what i am talking about

why dont you look here? I guess you are too lazy to scroll down to the section VERY CLEARLY labelled “advanced drive controls”
I copy and pasted that section into this document here:


Hope that helped : )

Hm, I believe @Anomalocaris did scroll down and even included that specific portion of code in their response. All necessary explanations of what PID stands for and is are included in their post above. While your control functions are not bad and will most likely produce accurate results it is far from a real PID loop. At best you may be able to call this a modified P loop because of the FINE_TUNE_RATIO error adjustment. But even then this really isn’t a full P loop. Please read the linked posts and materials and I am sure the IQ community would love an updated program implementing real PID control.

5 Likes

Very well. I’ll start with adcTurn()

void adcTurn(float degree, tSensors gyroSensor)
{
    writeDebugStreamLine("adcTurn(), degree = %f", degree);
    float initialDegree = adcGyroDegree(gyroSensor);
    writeDebugStreamLine("adcTurn(), initialDegree = %f", initialDegree);

    short clockwise = degree < initialDegree ? 1 : -1;
    float speed = TURN_SPEED * clockwise;

    repeatUntil(adcWaitUntilTurnStop(clockwise, degree, gyroSensor))
    {
        setMotor(leftMotor, speed);
        setMotor(rightMotor, speed * -1);
    }

    float turnedDegree = adcGyroDegree(gyroSensor);
    writeDebugStreamLine("adcTurn(), turnedDegree = %f", turnedDegree);
}

This function spins the left and right motors in opposite directions at speed speed until adcWaitUntilStop() is true. In this function, you initialize speed as such:

float speed = TURN_SPEED * clockwise;

clockwise is initialized as:

short clockwise = degree < initialDegree ? 1 : -1;

and TURN_SPEED as:

const short TURN_SPEED = 20;

We’ll next look at your adcWaitUntilTurnStop() conditional

bool adcWaitUntilTurnStop(short clockwise, float degree, tSensors gyroSensor)
{
    float turnedDegree = adcGyroDegree(gyroSensor);
    if (clockwise > 0)
    {
        if (turnedDegree <= degree) return true;
    }
    else
    {
        if (turnedDegree >= degree) return true;
    }
    return false;
}

This simply returns true when the turn is finished.

In summary, this will always turn the robot at the same speed until it reaches its target, sometimes clockwise and sometimes counterclockwise depending on the situation. This is not PID. PID would slow the turning down as the robot nears its target.

As for adcMove(), I analyzed this already:

I skipped much of the do part of he while() loop since it only runs once and most of it was not necessary for my explanation.

I’m not trying to criticize your code. It may be very effective for your robot. However, as you stated in your initial post,

While this is not true if you know where to look, it can in fact be hard to find resources for PID as a beginner team. I don’t want these teams to find this and believe that it is PID when it is not. Again, it may be very good at what it does, but it is not PID, so you should not try to pass it off as such lest it cause a team to believe they have learned PID when they have not. Hopefully this helps.

7 Likes

Hmmm… I should probably change the working around a bit but you should analyze the entire adcMove function. For some reason Vex forum is not letting me edit the original post anymore after 4 edits. >: (

also this isn’t my code, we got it from a team in China as reference.