Please check my PID

Programming Support VEXcode Pro V5 Text Tech Support
1

Hi, I’m very new to programming and this is the PID code I’ve tried. The robot has 6 motor drive and so far the code just doesn’t work. Nothing happens when I run it. (The regular auton without PID did work).

Thanks!

bool enabledrivePD = true;


double kp = 45 ;
double kd = 250;


double Tkp = 0.0;
double Tkd = 0.0;

int dist = 0;
int turnDeg = 0;

int error;
int prevError = 0;
int deriv;

int turnError;
int turnPrevError = 0;
int turnDeriv;

bool resetDrive = true;
  int drivePD(){
    
    while(enabledrivePD){
      if(resetDrive){
        resetDrive = false;
        LeftFrontDrive.setPosition(0,degrees);
        LeftMidDrive.setPosition(0,degrees);
        LeftRearDrive.setPosition(0,degrees);
        RightFrontDrive.setPosition(0,degrees);
        RightMidDrive.setPosition(0,degrees);
        RightRearDrive.setPosition(0,degrees);
      }
      int LeftRearPosition = LeftRearDrive.position(degrees);
      int LeftMidPosition = LeftMidDrive.position(degrees);
      int LeftFrontPosition = LeftFrontDrive.position(degrees);

      int RightRearPosition = RightRearDrive.position(degrees);
      int RightMidPosition = RightMidDrive.position(degrees);
      int RightFrontPosition = RightFrontDrive.position(degrees);

      int leftAverage = (LeftFrontPosition+ LeftMidPosition+ LeftRearPosition)/3;
      int rightAverage = (RightFrontPosition+ RightMidPosition+ RightRearPosition)/3;
      int averagePosition = (leftAverage + rightAverage)/2;

      error = averagePosition - dist;
      
      deriv = prevError - error;

    double motorSpeed = kp * error + deriv * kd;
   
      int turnDif = leftAverage - rightAverage;

      turnError = turnDif - dist;
      
      turnDeriv = turnPrevError - turnError;

    double turnSpeed = Tkp * turnError + Tkd * turnDeriv;
    
    
    LeftDrive.spin(forward, motorSpeed + turnSpeed, voltageUnits::volt);
    
    RightDrive.spin(forward, motorSpeed - turnSpeed, voltageUnits::volt);


    turnPrevError = turnError;
    prevError = error;
    vex::task::sleep(20);

    }
    return 1;
  }




/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

// ..........................................................................
// Insert autonomous user code here.
void autonomous(void) {

  vex::task TestingDriver(drivePD);
 resetDrive=true;
  dist = 0;
  turnDeg = 180;
1 Like
2

Do you have anything else after that in your autonoumous() function, or is that it? If there is nothing else, what I believe is happening is that the autonomous is ending as soon as you set your variables and the task is going out of scope and being terminated. You can fix this by either not using a task and just directly calling the function or by adding a sleep command so the autonomous function will wait until your PID is done.

3

Like @xTigr said, you need a sleep command so the code knows to wait until your PID loop is done. This is how my code does it:

void autonomous(void) {
  vex::task CallDrivePID(DrivePID);
  resetDriveSensors = true;
  desiredValue = 300;
  desiredTurnValue = 600;
  

  vex::task::sleep(1000);

  resetDriveSensors = true;
  desiredValue = 300;
  desiredTurnValue = 300;
  //The above lines of code, 146-156, are enabling the PID loop for autonomous
  
}
5

Thanks, I added that in but it still doesn’t seem to work.

6

first, what do you mean “not work”. compile error? not moving?
second, please post your code so others can help you.

7

When I run auton, the robot doesn’t move.

double kd = 0.0;


double Tkp = 0.0;
double Tkd = 0.00;

int dist = 250;
int turnDeg = 45;

int error;
int prevError = 0;
int deriv;

int turnError;
int turnPrevError = 0;
int turnDeriv;

bool resetDrive = false;

bool enabledrivePD = true;
  int drivePD(){
    
    while(enabledrivePD){
      if(resetDrive){
        resetDrive = false;
        LeftFrontDrive.setPosition(0,degrees);
        LeftMidDrive.setPosition(0,degrees);
        LeftRearDrive.setPosition(0,degrees);
        RightFrontDrive.setPosition(0,degrees);
        RightMidDrive.setPosition(0,degrees);
        RightRearDrive.setPosition(0,degrees);
      }
      int LeftRearPosition = LeftRearDrive.position(degrees);
      int LeftMidPosition = LeftMidDrive.position(degrees);
      int LeftFrontPosition = LeftFrontDrive.position(degrees);

      int RightRearPosition = RightRearDrive.position(degrees);
      int RightMidPosition = RightMidDrive.position(degrees);
      int RightFrontPosition = RightFrontDrive.position(degrees);

      int leftAverage = (LeftFrontPosition+ LeftMidPosition+ LeftRearPosition)/3;
      int rightAverage = (RightFrontPosition+ RightMidPosition+ RightRearPosition)/3;
      int averagePosition = (RightFrontPosition + RightMidPosition + RightRearPosition + LeftRearPosition + LeftFrontPosition +LeftMidPosition)/6;

      error = averagePosition - dist;
      
      deriv = prevError - error;

    double motorSpeed = kp * error + deriv * kd;
   
      int turnDif = leftAverage - rightAverage;

      turnError = turnDif - dist;
      
      turnDeriv = turnPrevError - turnError;

    double turnSpeed = Tkp * turnError + Tkd * turnDeriv;
    
    
    LeftDrive.spin(forward, motorSpeed + turnSpeed, voltageUnits::volt);
    
    RightDrive.spin(forward, motorSpeed - turnSpeed, voltageUnits::volt);


    turnPrevError = turnError;
    prevError = error;
    vex::task::sleep(20);

    }
    return 1;
  }




/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

// ..........................................................................
// Insert autonomous user code here.
void autonomous(void) {

  vex::task TestingDriver(drivePD);
 resetDrive=true;
  dist = 0;
  turnDeg = 45;
  
  vex::task::sleep(1000);
  
  resetDrive = true;
8

moving zero distance?

9

Yeah I was trying to get it to turn. Although I will mention that I put in 100 and it still didn’t work.

10

You have your turning constants set to 0. Change Tkp and Tkd to a different value so the robot will start to move.

1 Like
11

I’ve put in constants and it moves now, but it will not stop moving backward. I’ve tried changing the distance value (anything under 100 and it won’t move at all). I’ve tried changing the constants, but oftentimes it either causes the robot to not move at all, or it causes the robot to start weirdly turning. Here’s the code as of right now.


  // Initializing Robot Configuration. DO NOT REMOVE!
  Inertial.calibrate();
  // waits for the Inertial Sensor to calibrate
  while (Inertial.isCalibrating()) {
    wait(100, msec);
    
  }
  vexcodeInit();
  FrontClaw.set(false);
  FrontClaw2.set(false);
  RightDrive.setStopping(coast);
  LeftDrive.setStopping(coast);

}

double kp = 2.0;
double kd = 4.0;

double Tkp = 0.5;
double Tkd = 1.0;

int dist = 0;
int turnDeg = 0;

int error;
int prevError = 0;
int deriv;

int turnError;
int turnPrevError = 0;
int turnDeriv;

bool resetDrive = false;

bool enabledrivePD = true;
  int drivePD(){
    
    while(enabledrivePD){
      if(resetDrive){
        resetDrive = false;
        LeftFrontDrive.setRotation(0,degrees);
        LeftMidDrive.setRotation(0,degrees);
        LeftRearDrive.setRotation(0,degrees);
        RightFrontDrive.setRotation(0,degrees);
        RightMidDrive.setRotation(0,degrees);
        RightRearDrive.setRotation(0,degrees);
      }
      int LeftRearPosition = LeftRearDrive.position(degrees);
      int LeftMidPosition = LeftMidDrive.position(degrees);
      int LeftFrontPosition = LeftFrontDrive.position(degrees);

      int RightRearPosition = RightRearDrive.position(degrees);
      int RightMidPosition = RightMidDrive.position(degrees);
      int RightFrontPosition = RightFrontDrive.position(degrees);

      int leftAverage = (LeftFrontPosition+ LeftMidPosition+ LeftRearPosition)/3;
      int rightAverage = (RightFrontPosition+ RightMidPosition+ RightRearPosition)/3;
      int averagePosition = (RightFrontPosition + RightMidPosition + RightRearPosition + LeftRearPosition + LeftFrontPosition +LeftMidPosition)/6;

      error = averagePosition - dist;
      
      deriv = prevError - error;

    double motorSpeed = kp * error + deriv * kd;
   
      int turnDif = leftAverage - rightAverage;

      turnError = dist-turnDif ;
      
      turnDeriv = turnPrevError - turnError;

    double turnSpeed = Tkp * turnError + Tkd * turnDeriv;
    
    
    LeftDrive.spin(forward, motorSpeed - turnSpeed, rpm);
    
    RightDrive.spin(forward, motorSpeed + turnSpeed, rpm);


    turnPrevError = turnError;
    prevError = error;
    vex::task::sleep(20);

    }
    return 1;
  }




/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

// ..........................................................................
// Insert autonomous user code here.
void autonomous(void) {

  vex::task TestingDriver(drivePD);
 resetDrive=true;
  dist = 100;
  turnDeg = 0;
  
  vex::task::sleep(1000);
  
  resetDrive = true;
12

What do you mean by

Is it just going in the wrong direction? Try swapping the + and - in your motor.spin() command.

13

Oh I meant that it overshoots the distance that I want it to go.

14

Try tuning your p value first before adding derivative control. Overshooting can be a symptom of bad constants

2 Likes
15

I’ll definitely try that out, on a side note, my turning function just doesn’t work at all.

void autonomous(void) {

  vex::task TestingDriver(drivePD);
 resetDrive=true;
  dist = 0;
  turnDeg = 45;
  
  vex::task::sleep(1000);

This just doesn’t move the robot at all.

16

Your tkp may be so small that it’s not overcoming static friction. Try messing with that value and if a really high number doesn’t change anything that would at least rule that out

1 Like
17

im in vex iq and ive also been trying pid. maybe its an issues with ur values of the kp kd and ki. Thats the issue ive been having- finding the right value for the kp

18

I would separate forward PID and turning PID into two functions

1 Like
19

The variable turnDeg is never used in your function unless I miss it.

1 Like