How to Implement a PID

Hello, our team decided to implement a PID this season. I have watched Connors tutorial and I think I understand the purpose. Our robot has a four motor tank drive and I just don’t know how to take the PID I have and make it four motor. I have just started in text a few months ago and I have a pretty good understanding. I also don’t understand how to get four motors to drive at once outside of a drivetrain or if that is even necessary. I have looked all over the forum and there seems to be no topics about this specific issue. What are some possible solutions to this problem?

Here is our current program:

void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}


//Tune Here
double kp = 0.0005;
double ki = 0.0;
double kd = 0.0;
double turnkp = 0.0;
double turnki = 0.0;
double turnkd = 0.0;

//Autonomous settings
int desiredValue = 200;
int desiredturnValue = 0;



int error; //sensor value - disired value : positional value
int previouserror = 0; // position 20 milliseconds ago
int derivitive;  // difference between error and previous error : Speed
int totalerror = 0; // totalerror = totalerror + error


int turnerror; //sensor value - disired value : positional value
int turnpreviouserror = 0; // position 20 milliseconds ago
int turnderivitive;  // difference between error and previous error : Speed
int turntotalerror = 0; // totalerror = totalerror + error

bool resetdrivesensor = false;


// variabels motified for use
 bool enabledrivepid = true;

int drivepid(){
  
  while(enabledrivepid){
    

    if (resetdrivesensor) {
      resetdrivesensor = false;
      LeftMotor.setPosition(0, degrees);
      RightMotor.setPosition(0, degrees);
    }




    //get the position of both motors
    int Leftmotorposition = LeftMotor.position(degrees);
    int Rightmotorposition = RightMotor.position(degrees);


    /////////////////////////////////////////////////////
    // lateral movement pid
    ////////////////////////////////////////////////////////////////////////////////////////////
    // mean of the two variables
    int averageposition = Leftmotorposition + Rightmotorposition/2;
   
    error = averageposition - desiredValue;

    // derivitive
    derivitive = error - previouserror;

    //Intigral
    //totalerror = error;

    double lateralmotorPower = (error * kp + derivitive + kd + totalerror + ki) / 12.0;
    //////////////////////////////////////////////////////////////////////////////////


    /////////////////////////////////////////////////////
    // turning movement pid
    ////////////////////////////////////////////////////////////////////////////////////////////
    int turnDifference = Leftmotorposition - Rightmotorposition;
   
    turnerror = turnDifference - desiredturnValue;

    // derivitive
    turnderivitive = turnerror - turnpreviouserror;

    //Intigral
    //turntotalerror = turnerror;

    double turnmotorPower = (turnerror * turnkp + turnderivitive + turnkd + turntotalerror + turnki) / 12.0;
    //////////////////////////////////////////////////////////////////////////////////


    /////////////////////////////////////////////////////////////////////////////////
    LeftMotor.spin(forward, lateralmotorPower + turnmotorPower, voltageUnits::volt);
    RightMotor.spin(forward, lateralmotorPower - turnmotorPower, voltageUnits::volt);



    //code
    previouserror = error;
    turnpreviouserror = turnerror;
    vex::task::sleep(20);

  }
  
  return 1;
}

void autonomous(void) {

vex::task Tea(drivepid);

resetdrivesensor = true;
desiredValue = 300;
desiredturnValue = 600;

vex::task::sleep(100);

resetdrivesensor = true;
desiredValue = 300;
desiredturnValue = 300;

}

Any help would be amazing. I am definitely not an experienced programmer.

When posting code, I recommend that you use [code] and the beginning and [/code] at the end, since, while using ``` works just fine, it does format a bit better if you use the code blocks, so it looks like this:

void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();


  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}


//Tune Here
double kp = 0.0005;
double ki = 0.0;
double kd = 0.0;
double turnkp = 0.0;
double turnki = 0.0;
double turnkd = 0.0;

//Autonomous settings
int desiredValue = 200;
int desiredturnValue = 0;



int error; //sensor value - disired value : positional value
int previouserror = 0; // position 20 milliseconds ago
int derivitive;  // difference between error and previous error : Speed
int totalerror = 0; // totalerror = totalerror + error


int turnerror; //sensor value - disired value : positional value
int turnpreviouserror = 0; // position 20 milliseconds ago
int turnderivitive;  // difference between error and previous error : Speed
int turntotalerror = 0; // totalerror = totalerror + error

bool resetdrivesensor = false;


// variabels motified for use
 bool enabledrivepid = true;

int drivepid(){
  
  while(enabledrivepid){
    

   if (resetdrivesensor) {
      resetdrivesensor = false;
      LeftMotor.setPosition(0, degrees);
      RightMotor.setPosition(0, degrees);
    }




    //get the position of both motors
    int Leftmotorposition = LeftMotor.position(degrees);
    int Rightmotorposition = RightMotor.position(degrees);


    /////////////////////////////////////////////////////
    // lateral movement pid
    ////////////////////////////////////////////////////////////////////////////////////////////
    // mean of the two variables
    int averageposition = Leftmotorposition + Rightmotorposition/2;
   
    error = averageposition - desiredValue;

    // derivitive
    derivitive = error - previouserror;

    //Intigral
    //totalerror = error;

    double lateralmotorPower = (error * kp + derivitive + kd + totalerror + ki) / 12.0;
    //////////////////////////////////////////////////////////////////////////////////


    /////////////////////////////////////////////////////
    // turning movement pid
    ////////////////////////////////////////////////////////////////////////////////////////////
    int turnDifference = Leftmotorposition - Rightmotorposition;
   
    turnerror = turnDifference - desiredturnValue;

    // derivitive
    turnderivitive = turnerror - turnpreviouserror;

    //Intigral
    //turntotalerror = turnerror;

    double turnmotorPower = (turnerror * turnkp + turnderivitive + turnkd + turntotalerror + turnki) / 12.0;
    //////////////////////////////////////////////////////////////////////////////////


    /////////////////////////////////////////////////////////////////////////////////
    LeftMotor.spin(forward, lateralmotorPower + turnmotorPower, voltageUnits::volt);
    RightMotor.spin(forward, lateralmotorPower - turnmotorPower, voltageUnits::volt);



    //code
    previouserror = error;
    turnpreviouserror = turnerror;
    vex::task::sleep(20);

  }
  
  return 1;
}
void autonomous(void) {

vex::task Tea(drivepid);

resetdrivesensor = true;
desiredValue = 300;
desiredturnValue = 600;

vex::task::sleep(100);

resetdrivesensor = true;
desiredValue = 300;
desiredturnValue = 300;

}

Personally, I’ve never actually used drivetrains, so I can’t really speak to the effectiveness of them, but if you want multiple to spin outside of a drivetrain you can just assign seperate motor commands and have them run at the same time, such as spinning for a certain amount of time, like this:

motor_1.spin(fwd);
motor_2.spin(fwd);
motor_3.spin(fwd);
motor_4.spin(fwd);
sleep(1);
motor_1.stop();
motor_2.stop();
motor_3.stop();
motor_4.stop();

There’s other ways to do it with spinFor and whatnot, but this is a good starting point.

I would suggest looking at this thread for some additional PID resources that might be able to help you now, and in the future.

3 Likes

This helps so much. Thank you.

2 Likes

You already have the code there to spin “left” and “right” motors, all you need to do to add two more for a 4 motor drive is copy in two more commands, identical to the two you already have but with different motor names.

frontLeftMotor.spin(forward, lateralmotorPower + turnmotorPower, voltageUnits::volt);
frontRightMotor.spin(forward, lateralmotorPower - turnmotorPower, voltageUnits::volt);
backLeftMotor.spin(forward, lateralmotorPower + turnmotorPower, voltageUnits::volt);
backRightMotor.spin(forward, lateralmotorPower - turnmotorPower, voltageUnits::volt);
2 Likes

Ok that makes sense.

The first step into learning PID, is to not copy Connor’s code

6 Likes

You are one hundred percet right. I know better than to copy someone’s work. I now am going to start over and do my own work. Thank you for helping me see this. Also I am apologize for doing this. It was completely irresponsible of me. Good luck this season.

1 Like

As with all threads on PID, I would strongly recommend “separating the concerns” of the PID logic from the application. What I mean by this is to create a general purpose PID class or function which can be re-used for each of the places you want to use PID to do something. This way you don’t copy-pasta code from your PID Turn into your code for Drive Forward PID. This helps with any number of things:

  1. A single place to test that your PID logic is correct. If a bug is found in your PID implementation, you have a single place to fix it.
  2. Code re-use - no copy-pasta needed
  3. More read-able code. You’re not interspersing motor control with PID logic
  4. More extensible - say you change from a 2 motor drive to a 4 motor drive and then a 6 motor drive. You won’t need to chase down multiple locations as your robot evolves.

It takes more time up front, but there are huge benefits.

5 Likes

As a professional software developer (and not a teacher), I (personally) don’t have any problem with teams using other’s code PROVIDED the team understands the code they are incorporating. Software is much like science - built on the shoulders of others.

Implementing PID to learn how to program can be a valid exercise. But it can also reinforce (or introduce) bad habits.

There are any number of PID implementations available on the Interwebs from which to draw inspiration. I’ve never used any of these and they may have bugs, etc, but on a basic inspection of them, they are well-written and well thought out.

4 Likes

This is a very good point. Additionally, separating code into separate files can help tremendously as well. I had thousands of lines in a single cpp file when I first started and looking for things was an absolute mess.

1 Like

Code does 2 things:

  • Tells the silicon what to do
  • Tells other people reading the code what the silicon should do

The second is arguably more important than the first - when the silicon doesn’t do what was intended or the requirements change, being able to read and understand the code is critical.

5 Likes