PID Turning and Tuning Problem

Hey Forum, I’ve recently discovered what PID is and the benefits of it and wanted to implement it on our worlds robot over the spring break. So, I’ve been researching for the past week how PID works and found @Connor’s tutorial. I implemented his code into our program and have been trying to tune it for the past few days and have made barely any progress. My lateral drive constants seem to work fine alone but when any change or edit is made to the turning portion of the PID, the lateral PID fails. Whenever I test the autonomous, it drives uncontrollably in different directions until I turn the program off. I do not know where I am going wrong or how to fix the code as I am not a very experienced programmer, nor am I the main programmer on my team. I have included the code below, any sort of help is greatly appreciated as I am almost at my breaking point with this code. Thank you.

#include "vex.h"

using namespace vex;

// A global instance of competition
competition Competition;
bool PistonState = true;
bool PistonState2 = false;

// define your global instances of motors and other devices here


   


/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the V5 has been powered on and        */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

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

//Lateral PID constants
double kP = 0.57;
double kI = 0.0;
double kD = 0.15;

//Turn PID constants
double turnkP = 0.5;
double turnkI = 0.0;
double turnkD = 0.0;

//Auton setting
double desiredValue = 0.0;
int desiredTurnValue = 0.0;

int error; //Distance from desired posit. value
int prevError = 0;// Position 20ms ago
int derivative; //error-preverror : speed
int totalError = 0;// totalError= totalError + error

int turnerror; //Distance from desired posit. value
int turnprevError = 0;// Position 20ms ago
int turnderivative; //error-preverror : speed
int turntotalError = 0;// totalError= totalError + error

//Allows to reset the loop
bool resetSensors = false;

//Allows you to turn off PID in usercontrol
bool EnablePID = true;

//Drive PID
int drivePID(){

  while(EnablePID){

    if (resetSensors) {
      resetSensors = false;
      LeftDriveSmart.setPosition(0,degrees);
      RightDriveSmart.setPosition(0,degrees);
      spinny.setHeading(0,degrees);
    }


    int leftdrivepostion = LeftDriveSmart.position(degrees);
    int rightdrivepostion = RightDriveSmart.position(degrees);

///////////////////LATERAL PID////////////////////////////////

    int averagePosition = (leftdrivepostion+rightdrivepostion)/6;

    //Proportional
    error = desiredValue - averagePosition;

    //Derivative
    derivative = error-prevError;

    //Integral
    totalError= totalError + error;

    double latMotorPower = error * kP + derivative * kD + totalError * kI; 

  /////////////////////TURN PID/////////////////////////////////

  //int turnDiff = leftdrivepostion - rightdrivepostion;
    
    //Proportional
    turnerror = desiredTurnValue - spinny.heading(degrees);

    //Derivative
    turnderivative = turnerror-turnprevError;

    //Integral
    turntotalError += turnerror;

    double turnMotorPower = turnerror * turnkP + turnderivative * turnkD + turntotalError * turnkI;
  /////////////////////////////////////////////////////////////
    LeftDriveSmart.spin(forward, latMotorPower + turnMotorPower, voltageUnits::volt);
    RightDriveSmart.spin(forward, latMotorPower + turnMotorPower, voltageUnits::volt);


    prevError = error;
    turnprevError = turnerror;
    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.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {
  vex::task initPID (drivePID);

  resetSensors = true;
  vex::task::sleep(1000);
  desiredValue = 300;
  wait(1,sec);
  Drivetrain.turnToHeading(90,degrees);

Note: some of the code may be incomplete as I am still working on it.

Hey there! Let’s debug this.

first, why is averagePosition equal to the two positions divided by six?? why is it not two? Averaging the positions would literally mean that you take the two positions and divide by 2.

Second, I do NOT recommend having turn and lateral PID in the same method. That is 10x as likely to break then keeping them separate.

Third, error should not be an int!! If desired value is a double, and your position is going to be a double, why are you truncating the result by shoving it into an int? Change error, prevError, totalError ETC into doubles.

Fourth, why are you setting desiredValue AFTER starting up your PID? That just means that the PID sees 0 desiredValue for the first 1000MS (while the autonomous task sleeps)

Lastly, you need an exit condition. You need to say, immediately after finding error, if(error < minimum) { break; } so that once you get close enough to target (aka, once error is within the minimum value set by you) the PID will stop.

3 Likes