PID driving too far then reversing

Programming Support
1

Hi,

I recently wrote a pid code. An issue I am encountering is that it will consistently drive to far and then reverse when the integral value increases. Here is a look at the code that makes the robot drive forward, turn around, and then drive back. Thanks for the help.

// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"

using namespace vex;

motor leftFront = motor(PORT1, ratio18_1,false);
motor leftBack = motor(PORT11, ratio18_1,false); 
motor rightFront = motor(PORT10, ratio18_1,true);
motor rightBack = motor(PORT20, ratio18_1,true);


   

int desiredValue = 0;

int Kp = 100;
int Ki = 10000;
int Kd = 100;

int totalError = 0;
int prevError = 0;

bool enableYPID = false;
bool enableTurnPID = false;







int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  leftFront.setStopping(hold); //tell the drivebase to not let the wheels spin unless told too.
    rightFront.setStopping(hold);
    leftBack.setStopping(hold);
    rightBack.setStopping(hold);


  enableYPID = true;

  int desiredValue = 1800;

  

  while(enableYPID) {
  int averagePosition = (leftFront.position(degrees) + leftBack.position(degrees ) + rightFront.position(degrees) + rightBack.position(degrees ))/2;
  int error = averagePosition - desiredValue;
  totalError += error;
  int diffrenceError = error - prevError;

  int Speed = error / Kp + totalError / Ki + diffrenceError / Kd;

  leftFront.spin(reverse, Speed, voltageUnits::volt);
  rightFront.spin(reverse, Speed, voltageUnits::volt);
  leftBack.spin(reverse, Speed, voltageUnits::volt);
  rightBack.spin(reverse, Speed, voltageUnits::volt);

  prevError = error;

  wait(0.02, sec);

  if( error < 3 and error > -3 ){

    enableYPID = false; 
  }

  }
  
  enableYPID = false;
  leftFront.spin(reverse, 0, voltageUnits::volt);
  rightFront.spin(reverse, 0, voltageUnits::volt);
  leftBack.spin(reverse, 0, voltageUnits::volt);
  rightBack.spin(reverse, 0, voltageUnits::volt);
  Brain.Screen.print("stuff");

  wait(1, sec);


 enableTurnPID = true;

  int desiredTurnValue = 1800;

  

  while(enableTurnPID) {
  int averagePosition = (leftFront.position(degrees) + leftBack.position(degrees ) - rightFront.position(degrees) - rightBack.position(degrees ))/2;
  int error = averagePosition - desiredTurnValue;
  totalError += error;
  int diffrenceError = error - prevError;

  int Speed = error / Kp + totalError / Ki + diffrenceError / Kd;

  leftFront.spin(reverse, Speed, voltageUnits::volt);
  rightFront.spin(forward, Speed, voltageUnits::volt);
  leftBack.spin(reverse, Speed, voltageUnits::volt);
  rightBack.spin(forward, Speed, voltageUnits::volt);

  prevError = error;

  wait(0.02, sec);

  if( error < 3 and error > -3 ){

    enableTurnPID = false; 
  }

  }

 
 enableTurnPID = false; 

  leftFront.spin(reverse, 0, voltageUnits::volt);
  rightFront.spin(reverse, 0, voltageUnits::volt);
  leftBack.spin(reverse, 0, voltageUnits::volt);
  rightBack.spin(reverse, 0, voltageUnits::volt);
  Brain.Screen.print("  more  stuff");

  wait(1, sec);

  enableYPID = true;

   desiredValue = 3600;

  

  while(enableYPID) {
  int averagePosition = (leftFront.position(degrees) + leftBack.position(degrees ) + rightFront.position(degrees) + rightBack.position(degrees ))/2;
  int error = averagePosition - desiredValue;
  totalError += error;
  int diffrenceError = error - prevError;

  int Speed = error / Kp + totalError / Ki + diffrenceError / Kd;

  leftFront.spin(reverse, Speed, voltageUnits::volt);
  rightFront.spin(reverse, Speed, voltageUnits::volt);
  leftBack.spin(reverse, Speed, voltageUnits::volt);
  rightBack.spin(reverse, Speed, voltageUnits::volt);

  prevError = error;

  wait(0.02, sec);

  if( error < 3 and error > -3 ){

    enableYPID = false; 
  }

  }
  
  enableYPID = false;
  leftFront.spin(reverse, 0, voltageUnits::volt);
  rightFront.spin(reverse, 0, voltageUnits::volt);
  leftBack.spin(reverse, 0, voltageUnits::volt);
  rightBack.spin(reverse, 0, voltageUnits::volt);
  Brain.Screen.print("  even  more stuff");

  
  
}
3

this might help.

1 Like
4

Sounds like you just haven’t tuned it enough.

This is a great resource for PID. You can check out the tuning section to see how to better tune your constants.

6 Likes
5

@9MotorGang
I recently put together a collection of resources that provide more insight into PID. They range from a beginner to advanced level so it should be easy to follow through no matter your skill level. Additionally, there are both theoretical and concrete examples provided. I highly recommend reading through each resource as each one has unique strengths. You can find it here.

What you are currently experiencing is known as overshoot. The materials I provided should help you understand why this happens and how you can fix this.

1 Like