Improvised PID Function

Programming Support
1

I’m not sure if this PID works properly, mainly because I changed it from a task to a void function. My question is

  1. Will my PID will work if I call the function in auton? (cuz i improvised from the connor 1814D video)
  2. Would a PID task be better than just a void function?
  3. I know I made a mistake in exiting the while loop, so any fixes to that?
  4. The drive and turn variables are inside the loop, making them non-global. Would that cause the code to lag since it is recreating the variables every time I run the function instead of just replacing the value that is stored?

Heres my code:

//PID Settings (when testing constants, make a table of all the numbers tried)
double kP = 0.0; //distance from current to desired; error
double kI = 0.0; //increase precision for steady state error accumluated over time
double kD = 0.0; //speed up or slow down depending on how close you are to the desired position

double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;

double threshold = 0.5;
double turnThreshold = 0.5;

//PID Loop
void doPID(int distance, int angle) {
  leftDrive.setPosition(0, degrees); //Resets encoders so the robot makes desired position the current position so it doesn't keep going back
  rightDrive.setPosition(0, degrees); //if not, then the degrees on the encoders will be affected by the previous turns, messing up the new desired positions

  //Drive
  int currentPosition;
  int desiredPosition = distance;
  int error;
  int previousError = 0;
  int totalError = 0;
  int derivative;

  //Turn
  int currentTurnPosition;
  int desiredTurnPosition = angle;
  int turnError;
  int turnPreviousError = 0;
  int turnTotalError = 0;
  int turnDerivative;

  while(true) {
    int leftPosition = leftDrive.position(degrees);
    int rightPosition = rightDrive.position(degrees);

    //Proportional
    currentPosition = (leftPosition + rightPosition)/2; //average position in degrees of both sides of drivetrain
    error = currentPosition - desiredPosition;

    //Derivative
    derivative = error - previousError; // basically change in distance over change in time

    //Integral
    totalError = totalError + error; //sums up all the tiny errors over time(the space left over or underestimated from LRAM/RRAM) and takes that into account

    //Total
    double drivePower = (error * kP) + (derivative * kD) + (totalError * kI);

    //Turn Proportional
    currentTurnPosition = leftPosition - rightPosition;
    turnError = currentTurnPosition - desiredTurnPosition;

    //Turn Derivative
    turnDerivative = turnError - turnPreviousError;

    //Turn Integral
    turnTotalError = turnTotalError + turnError;

    //Turn Total
    double turnPower = (turnError * turnkP) + (turnDerivative * turnkD) + (turnTotalError * turnkI);

    //Execute
    leftDrive.spin(forward, drivePower + turnPower, voltageUnits::volt); 
    rightDrive.spin(forward, drivePower - turnPower, voltageUnits::volt); 

    previousError = error;
    turnPreviousError = turnError;
    vex::task::sleep(20); //20 milliseconds

    if(abs(error) < threshold && abs(turnError) < turnThreshold) { //if the error is less than a specific measurement, break the loop or else the pid will keep trying to correct itself
      break;
    }
  }
}


//--------------------------------------------------------------------//


void preAutonomous(void) {
  // actions to do when the program starts
  Brain.Screen.clearScreen();
  Brain.Screen.print("pre auton code");
  wait(1, seconds);
}

void autonomous(void) {
  Brain.Screen.clearScreen();
  Brain.Screen.print("autonomous code");
  // place automonous code here
  // 1 mat = 2 feet
  // positive degrees = turn left
  
  doPID(360, 0);
  wait(1, sec);

  doPID(0, 90);
  wait(1, sec);

  doPID(360, 0);
  wait(1, sec);
  
}

You can also ignore the comments and the fact that I still need to test constants. And as context, treat the auton code as if I am trying to make the robot go forward one tile and then turn left 90 degrees and then go forward one more tile. So like what would go wrong if you used my current code and tried to do the context situation.

Thanks.

2
  1. Yes, if it’s a regular function it should work as a function.
  2. Unless you want to return a value from the function it can stay void.
  3. Use return; instead of break; (Also use some feedback system to see if the if statement is triggered in the first place. You may need to change the exit conditions.)
  4. The loop delay is 20 ms so the tiny amount of time it takes to make a variable is comparatively negligible.

Also maybe think about using an inertial sensor for turning as using the encoders on the drive train could be subject to error if the robot slips its wheels.
Can’t really use an IMU to work out distance.

Some of this may be inaccurate as I’m new to C++ but it is what worked in my PID loop and it’s pretty good.

My best advice is just try these things and see if they work.
Then come back for help if you have a specific problem.

Also, do you know know the loops work?
Kinda hard to tune a PID loop if you don’t know how PID loops work.

Hope this helps. :slight_smile:

1 Like
3

Does the PID only output motor power or vsn it also output other values? Like would telling the motor to spin a certain amount of degrees be better?

4

Telling the motor to spin a certain degree gives the PID loop no control over the speed of the motor. Which defeats the points of having a PID loop.

If you tune a PID loop correctly it should find the fastest speed the motors can be at at a given time without overshooting the target.
This moves as fast as possible while minimising overshoot.

It’s like braking for a corner in a race. The driver has to use just the right amount of brake at the right time to make sure they don’t brake too late or lock up then overshoot the corner, or brake too early and spend less time at max speed.

The PID loop is just a simple and effective way of doing that.

1 Like