PID Coding and Tuning Help

Programming Support VEXcode Pro V5 Text Tech Support
,
1

Hey Everyone,
I am a Middle Schooler that is going to Worlds for the first time. We are actually the first team from our program to go to Worlds. So any tips would be greatly appreciated.
Now on to the actual problem. At our previous competition, the autonomous kept being inaccurate. After qualifying to Worlds from State, I wanted to try and make our autonomous actually work and be able to compete at Worlds. After doing some research, I came across PID. I tried learning it. (I had a friend who told me I should learn the calculus equation, and you can guess how that went :grimacing:) I eventually came across this video.
https://www.youtube.com/watch?v=_Itn-0d340g&t
I watched it, put it into my code, and started testing. So the issue is that whenever I run the code. It moves the motors less than millimeters and then doesn’t do anything else. Could I please have some help on how to fix this, and overall, how to code and use PID? Any help is greatly appreciated.

P.S. I am also on a bit of time crunch as I have to code a Match and Skills Autonomous before Worlds, which is in a little over 3 weeks.

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       sagan.karthikeyan                                         */
/*    Created:      3/28/2025, 12:22:07 AM                                    */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

#include "../include/vex.h"

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here
controller controller1 = controller(); //definining controllers

// defining motors
//Drivetrain
motor LeftFront = motor(PORT1, ratio18_1, true);
motor LeftBack = motor(PORT2, ratio18_1, true);
motor RightFront = motor(PORT11, ratio18_1, true);
motor RightBack = motor(PORT12, ratio18_1, true);
motor_group LeftDrive = motor_group(LeftFront, LeftBack);
motor_group RightDrive = motor_group(RightFront, RightBack);
drivetrain Drivetrain = drivetrain(LeftDrive, RightDrive, 259.34, 14, 11.5, inches, 2);


//Mogomech
motor MogoMech = motor(PORT20, ratio6_1, true);

 
//LadyBrown
motor LadyBrown = motor(PORT17, ratio6_1, true);

//Intake
motor Intake = motor(PORT18, ratio36_1, false);
motor Conveyor = motor(PORT16, ratio36_1, false);

motor_group FullIntake = motor_group(Intake, Conveyor); //Motor Group for Intake

/////////////////////WallStake Macros////////////////////////////////////////////////

//3 Wallstake Positions
int WallPOSITION_1 = 0;
int WallPOSITION_2 = 350;
int WallPOSITION_3 = 2400;
int WallPOSITION_4 = 3200;

//Starts at position 1
int Wallposition = 1;


///////////////////////////////////End of WallStake Macros/////////////////////////////////////////////////

///////////////////////////////////MogoMech Macros/////////////////////////////////////////////////////////

//2 MogoMech Positions
int MogoPOSITION_1 = 0;
int MogoPOSITION_2 = 180;

//Starts at position 1
int MogoPosition = 1;

//////////////////////////////////End of MogoMech Macros/////////////////////////////////



////////////////////////////////////////////PRE-AUTONOMOUS/////////////////////////////////////////////////////


/*---------------------------------------------------------------------------*/
/*                          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) {

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

//////////////////////////////////PRE-AUTOONOMOUS END////////////////////////////////////////////////////////////////


///////////////////////////PID VARIABLES START///////////////////////////////////////

//Settings
double kP = 3;
double kI = 0.0;
double kD = 3;

double turnkP = 500;
double turnkI = 0.0;
double turnkD = 3;

/* HOW TO TUNE SETTINGS
  kP: Increase kP until you have steady minor oscillations; This fixes errors. 
  kD: Increase kD until the isolations are no longer isolations anymore; This makes minor corrections. 
  kI: Increase kI until the precision is "good enough for you." This changes speed depending if it is too fast or too slow. DON'T DO FOR DRIVETRIAN AS IT CAN BE INEFFECTIVE.*/

/*EXAMPLE VALUES:
kP = 0.0000004
During autonomous,
desiredValue = 300
desiredTurnValue = 600*/

//https://api.vexcode.cloud/v5/html

//Autotnomous Settings
int desiredValue = 300;
int desiredTurnValue = 0;


int error; //Sensor value - Desired Value - Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //Difference between error and previous error : Speed
int totalError = 0; //totalError = totalError + error

int turnError; //Sensor value - Desired Value - Position
int turnPrevError = 0; //Position 20 milliseconds ago
int turnDerivative; //Difference between error and previous error : Speed
int turnTotalError; //totalError = totalError + error

bool resetDriveSensors = false;

//Variables modified for use  
bool enabledrivePID = false;

////////////////////////////////////////PID VARIABLES END////////////////////////////////////////////////////////

//////////////////////////////////////////DRIVEPID///////////////////////////////////////////////////////////////

int drivePID(){

  while(enabledrivePID){
    
    if (resetDriveSensors) { 
      resetDriveSensors = false;

      LeftDrive.setPosition(0, degrees);
      RightDrive.setPosition(0, degrees);
    }
    
    
    //Get the position of both motors
    int LeftDriveMotorPosition = LeftFront.position(degrees) ;
    int RightDrivetMotorPosition = RightFront.position(degrees);


    ///////////////////////////////////////////////////////////////////////
    //Lateral Movement PID 
    ///////////////////////////////////////////////////////////////////////

    //Get average of the two motors
    int averagePosition = (LeftDriveMotorPosition + RightDrivetMotorPosition)/2;
    
    //Potential
    error = averagePosition - desiredValue;

    //Derivative
    derivative = error - prevError;

    //Integral
    //totalError += error;

    //DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!

    double lateralMotorPower = (error * kP + derivative * kD);
    //////////////////////////////////////////////////////////// ///////////  



    ///////////////////////////////////////////////////////////////////////
    //Turning Movement PID 
    ///////////////////////////////////////////////////////////////////////
    //Get average of the two motors
    int turnDifference = (LeftDriveMotorPosition - RightDrivetMotorPosition);
    
    //Potential
    turnError = turnDifference - desiredTurnValue;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //Integral
    //turnTotalError += turnError;

    //DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!

    double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD);
    ///////////////////////////////////////////////////////////////////////



    LeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    
    
    //code 
    prevError = error;
    turnPrevError = turnError; 
    vex::task::sleep(20);
  
  
  };

  return 1;
};

//////////////////////////////////////////DRIVEPID END///////////////////////////////////////////////////////////////

///////////////////////////////////////////AUTONOMOUS///////////////////////////////////////////////


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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) {
  // ..........................................................................
  // Insert autonomous user code here.
vex::task billnyethesciencefi(drivePID);
resetDriveSensors = true;
desiredValue = 300;
  
  //This is how you would code Autonomous; Think of the 300's as measurements.
  // ..........................................................................
}

//////////////////////////////////////////AUTONOMOUS END///////////////////////////////////////////////////////////////

/////////////////////////////////////////USER CONTROL////////////////////////////////////////////////////////////////////


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

void usercontrol(void) {
  // User control code here, inside the loop

  enabledrivePID = false;
  while (1) {
    // This is the main execution loop for the user control program.
    // Each time through the loop your program should update motor + servo
    // values based on feedback from the joysticks.

    // ........................................................................
    // Insert user code here. This is where you use the joystick values to
    // update your motors, etc.
 
     //9Motor Gang Code for Split Arcade
     LeftDrive.spin(forward, controller1.Axis1.position(percent) + controller1.Axis3.position(percent)*12/100, volt);
     RightDrive.spin(forward, controller1.Axis1.position(percent) - controller1.Axis3.position(percent)*12/100, volt);
     
    //Ladybrown Controller Code
    LadyBrown.setVelocity(500, rpm);
    LadyBrown.setStopping(brake);
    

    if (Wallposition == 1){
      LadyBrown.spinTo(WallPOSITION_1, degrees, false);
    } else if (Wallposition == 2){
      LadyBrown.spinTo(WallPOSITION_2, degrees, false);
    } else if (Wallposition == 3){
      LadyBrown.spinTo(WallPOSITION_3, degrees, false);
    } else if (Wallposition == 4){
      LadyBrown.spinTo(WallPOSITION_4, degrees, false);
    }

    if (Wallposition > 4) {
      Wallposition = 2;
    }

    if (Wallposition < 1) {
      Wallposition = 2;
    }

    if (controller1.ButtonL1.PRESSED) {
      Wallposition++;
    }
    if (controller1.ButtonL2.PRESSED) {
      Wallposition--;
    }


    //MogoMech Controller Code
    MogoMech.setStopping(hold);
    MogoMech.setMaxTorque(100, percent);
    MogoMech.setVelocity(50, percent);

    if (controller1.ButtonX.pressing())
    {
      MogoMech.spin(forward, 12, volt);
    }
    else if (controller1.ButtonB.pressing())
    {
      MogoMech.spin(reverse, 12, volt);
    }
    else
    {
      MogoMech.setStopping(hold);
      MogoMech.stop();
    }

    //Intake Controlling
    FullIntake.setStopping(brake);
    FullIntake.setVelocity(100, percent);

    if (controller1.ButtonR2.pressing())
    {
      FullIntake.spin(forward);
    }
    else if (controller1.ButtonR1.pressing())
    {
      FullIntake.spin(reverse);
    }
    else
    {
      FullIntake.setStopping(brake);
      FullIntake.stop();
    }



    // ........................................................................

    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }
}

//////////////////////////////////////////USERCONTROL END///////////////////////////////////////////////////////////////


//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.

  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();
  autonomous();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}

Thank you in advance for any help!

2

Hello Stratlis, I have been in the same time of situation of having to use a PID process control to correct the drift on my robot. I recommend a post called “Correcting drift with a PID process control”. Link: Correcting Drift with a PID process control
From, 12510X MechaManiax

3

After making a few tweaks, it finally works somewhat. Whenever I do run the code, the wheels spin back and forth really fast. I believe I have to tune it. How do I tune PID?

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       sagan.karthikeyan                                         */
/*    Created:      3/28/2025, 12:22:07 AM                                    */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

#include "../include/vex.h"

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here
controller controller1 = controller(); //definining controllers

// defining motors
//Drivetrain
motor LeftFront = motor(PORT1, ratio18_1, true);
motor LeftBack = motor(PORT2, ratio18_1, true);
motor RightFront = motor(PORT11, ratio18_1, true);
motor RightBack = motor(PORT12, ratio18_1, true);
motor_group LeftDrive = motor_group(LeftFront, LeftBack);
motor_group RightDrive = motor_group(RightFront, RightBack);
drivetrain Drivetrain = drivetrain(LeftDrive, RightDrive, 259.34, 14, 11.5, inches, 2);


//Mogomech
motor MogoMech = motor(PORT20, ratio6_1, true);

 
//LadyBrown
motor LadyBrown = motor(PORT17, ratio6_1, true);

//Intake
motor Intake = motor(PORT18, ratio36_1, false);
motor Conveyor = motor(PORT16, ratio36_1, false);

motor_group FullIntake = motor_group(Intake, Conveyor); //Motor Group for Intake

/////////////////////WallStake Macros////////////////////////////////////////////////

//3 Wallstake Positions
int WallPOSITION_1 = 0;
int WallPOSITION_2 = 350;
int WallPOSITION_3 = 2400;
int WallPOSITION_4 = 3200;

//Starts at position 1
int Wallposition = 1;


///////////////////////////////////End of WallStake Macros/////////////////////////////////////////////////

///////////////////////////////////MogoMech Macros/////////////////////////////////////////////////////////

//2 MogoMech Positions
int MogoPOSITION_1 = 0;
int MogoPOSITION_2 = 180;

//Starts at position 1
int MogoPosition = 1;

//////////////////////////////////End of MogoMech Macros/////////////////////////////////



////////////////////////////////////////////PRE-AUTONOMOUS/////////////////////////////////////////////////////


/*---------------------------------------------------------------------------*/
/*                          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) {

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

//////////////////////////////////PRE-AUTOONOMOUS END////////////////////////////////////////////////////////////////


///////////////////////////PID VARIABLES START///////////////////////////////////////

//Settings
double kP = 0.013;
double kI = 0.0;
double kD = 0;

double turnkP = 500;
double turnkI = 0.0;
double turnkD = 3;

/* HOW TO TUNE SETTINGS
  kP: Increase kP until you have steady minor oscillations; This fixes errors. 
  kD: Increase kD until the isolations are no longer isolations anymore; This makes minor corrections. 
  kI: Increase kI until the precision is "good enough for you." This changes speed depending if it is too fast or too slow. DON'T DO FOR DRIVETRIAN AS IT CAN BE INEFFECTIVE.*/

/*EXAMPLE VALUES:
kP = 0.0000004
During autonomous,
desiredValue = 300
desiredTurnValue = 600*/

//https://api.vexcode.cloud/v5/html

//Autotnomous Settings
int desiredValue = 0;
int desiredTurnValue = 0;


int error; //Sensor value - Desired Value - Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //Difference between error and previous error : Speed
int totalError = 0; //totalError = totalError + error

int turnError; //Sensor value - Desired Value - Position
int turnPrevError = 0; //Position 20 milliseconds ago
int turnDerivative; //Difference between error and previous error : Speed
int turnTotalError; //totalError = totalError + error

bool resetDriveSensors = false;

//Variables modified for use  
bool enabledrivePID = false;

////////////////////////////////////////PID VARIABLES END////////////////////////////////////////////////////////

//////////////////////////////////////////DRIVEPID///////////////////////////////////////////////////////////////

int drivePID(){

  while(enabledrivePID = true){
    
    if (resetDriveSensors) { 
      resetDriveSensors = false;

      LeftFront.setPosition(0, degrees);
      LeftBack.setPosition(0, degrees);
      RightFront.setPosition(0, degrees);
      RightBack.setPosition(0, degrees);
    }
    
    
    //Get the position of both motors
    int lfmp = LeftFront.position(degrees) ;
    int lbmp = LeftBack.position(degrees);
    int rfmp = RightFront.position(degrees) ;
    int rbmp = RightBack.position(degrees);


    ///////////////////////////////////////////////////////////////////////
    //Lateral Movement PID 
    ///////////////////////////////////////////////////////////////////////

    //Get average of the two motors
    int averagePosition = (lfmp+lbmp+rfmp+rbmp)/4;
    
    //Potential
    error = desiredValue - averagePosition;

    //Derivative
    derivative = error - prevError;

    //Integral
    //totalError += error;

    //DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!

    double lateralMotorPower = (error * kP + derivative * kD);
    //////////////////////////////////////////////////////////// ///////////  



    ///////////////////////////////////////////////////////////////////////
    //Turning Movement PID 
    ///////////////////////////////////////////////////////////////////////
    //Get average of the two motors
    int turnDifference = (lfmp+lbmp) - (rfmp+rbmp);
    
    //Potential
    turnError = desiredTurnValue - turnDifference;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //Integral
    //turnTotalError += turnError;

    //DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!

    double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD);
    ///////////////////////////////////////////////////////////////////////


    //start motors
    LeftFront.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    LeftBack.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightFront.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    RightBack.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    
    
    //code 
    prevError = error;
    turnPrevError = turnError; 
    vex::task::sleep(20);
  
  
  };

  return 1;
};

//////////////////////////////////////////DRIVEPID END///////////////////////////////////////////////////////////////

///////////////////////////////////////////AUTONOMOUS///////////////////////////////////////////////


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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) {
  // ..........................................................................
  // Insert autonomous user code here.
vex::task start(drivePID);
LeftFront.setStopping(brake);
RightFront.setStopping(brake);
LeftBack.setStopping(brake);
RightBack.setStopping(brake);
int errorMargin = 20;
resetDriveSensors = true;
desiredValue = 20;
waitUntil(error < errorMargin);
vex::task::sleep(500);
  
  //This is how you would code Autonomous; Think of the 300's as measurements.
  // ..........................................................................
}

//////////////////////////////////////////AUTONOMOUS END///////////////////////////////////////////////////////////////

/////////////////////////////////////////USER CONTROL////////////////////////////////////////////////////////////////////


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

void usercontrol(void) {
  // User control code here, inside the loop

  enabledrivePID = false;
  while (1) {
    // This is the main execution loop for the user control program.
    // Each time through the loop your program should update motor + servo
    // values based on feedback from the joysticks.

    // ........................................................................
    // Insert user code here. This is where you use the joystick values to
    // update your motors, etc.
 
     //9Motor Gang Code for Split Arcade
     LeftDrive.setStopping(coast);
     RightDrive.setStopping(coast);
     LeftDrive.setVelocity(70, percent);
     RightDrive.setVelocity(70, percent);
 
     LeftDrive.spin(forward, controller1.Axis1.position(percent) + controller1.Axis3.position(percent)*12/100, volt);
     RightDrive.spin(forward, controller1.Axis1.position(percent) - controller1.Axis3.position(percent)*12/100, volt);
     
     
 
     //Ladybrown Controller Code
     LadyBrown.setVelocity(500, rpm);
     LadyBrown.setStopping(hold);
     
 
     if (Wallposition == 1){
       LadyBrown.spinTo(WallPOSITION_1, degrees, false);
     } 
     
     if (Wallposition == 2){
       LadyBrown.spinTo(WallPOSITION_2, degrees, false);
     } 
 
     if (Wallposition == 3){
       LadyBrown.spinTo(WallPOSITION_3, degrees, false);
     } 
     
     if (Wallposition == 4){
       LadyBrown.spinTo(WallPOSITION_4, degrees, false);
     }
 
     if (Wallposition > 4) {
       Wallposition = 4;
     }
 
     if (Wallposition < 1) {
       Wallposition = 1;
     }
 
     if (controller1.ButtonL1.PRESSED) {
       Wallposition++;
     }
     if (controller1.ButtonL2.PRESSED) {
       Wallposition--;
     }
 
 
     //MogoMech Controller Code
     MogoMech.setStopping(hold);
     MogoMech.setMaxTorque(100, percent);
 
     if (controller1.ButtonY.pressing())
     {
       MogoMech.spin(forward, 4, volt);
     }
     else if (controller1.ButtonB.pressing())
     {
       MogoMech.spin(reverse, 4, volt);
     }
     else
     {
       MogoMech.setStopping(hold);
       MogoMech.stop();
     }
 
     //Intake Controlling
     FullIntake.setStopping(brake);
     FullIntake.setVelocity(100, percent);
 
     if (controller1.ButtonR2.pressing())
     {
       FullIntake.spin(forward);
     }
     else if (controller1.ButtonR1.pressing())
     {
       FullIntake.spin(reverse);
     }
     else
     {
       FullIntake.setStopping(brake);
       FullIntake.stop();
     }



    // ........................................................................

    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }
}

//////////////////////////////////////////USERCONTROL END///////////////////////////////////////////////////////////////


//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.

  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();
  autonomous();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}
4

Your turn_kP is too high, I usually don’t exceed 1 for my turn_kP. As for how to tune it, choose a start kP and increase/decrease it until you have a good response & accuracy time with minimal oscillations, then select a starting kD and increase it until the oscillations disappear. kI can stay at 0 unless you have accuracy issues or want to be fully accurate, as it will slow down the response time. If you are using kI, you should probably have an integral clamp. ex: if (integral > integralMax) integral = integralMax; if (integral < integralMin) integral = integralMin; This will remove integral windup.

5

Hello Statlis, to tune your PID, you can go over to the PID class in your code. To determine your coefficients, I recommend using the trial and error method where you manually adjust your coefficients. I always start by adjusting the Kp coefficent until the robot is underdamped. Once the robot is underdamped, I usually keep adjusting the Ki and Kd coefficients until the robot is critically damped.
Under Damped: Robot makes too many corrections resulting in a zigzag movement.
Over Damped: The robot starts correcting itself at the start, but slows down and does not return the robot’s heading back to 0.
Critically Damped: Robot corrects itself the entire run, and successfully returns the robot’s heading back to 0.
Proportional Component Equation: Set Point (Desired Heading) - Process Variable (Actual heading).
Integral Component Equation: Summation of all errors throughout a run.
Derivative Component Equation: Previous Error - New Error
I hope that helped. Let me know if you have any more questions!

6

It seems that you can tune the PID on lines 48-53. Try playing around with the values.

7

Your turn_kP is too high, I usually don’t exceed 1 for my turn_kP.

Yeah, I realized that and fixed it.

kI can stay at 0 unless you have accuracy issues or want to be fully accurate, as it will slow down the response time.

This means I don’t have to use it, correct?

I recommend using the trial and error method where you manually adjust your coefficients.

So I tuned the Kp value until it was a quick singular oscillation with little bouncing. After that, I tuned Kd until there was no bouncing. Is that the correct method for tuning.

How would I tune turn constants, such as turnkP and turnkI?

Thank you for all the help by the way. I am really grateful.

8

This is my current PID control by the way.

double inchesToDegrees(double inches) {
  double wheeldiameter = 3.25;
  double PI = 3.141592653589793;
  return (inches / (PI * wheeldiameter)) * 360.0;
}

////////////////////////////////////////////PRE-AUTONOMOUS/////////////////////////////////////////////////////


/*---------------------------------------------------------------------------*/
/*                          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) {

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

//////////////////////////////////PRE-AUTOONOMOUS END////////////////////////////////////////////////////////////////


///////////////////////////PID VARIABLES START///////////////////////////////////////

//Settings
double kP = 0.36;
double kI = 0.0;
double kD = 0.05;

double turnkP = 0.1;
double turnkI = 0.0;
double turnkD = 0;

/* HOW TO TUNE SETTINGS
  kP: Increase kP until you have steady minor oscillations; This fixes errors. 
  kD: Increase kD until the isolations are no longer isolations anymore; This makes minor corrections. 
  kI: Increase kI until the precision is "good enough for you." This changes speed depending if it is too fast or too slow. DON'T DO FOR DRIVETRIAN AS IT CAN BE INEFFECTIVE.*/

/*EXAMPLE VALUES:
kP = 0.0000004
During autonomous,
desiredValue = 300
desiredTurnValue = 600*/

//https://api.vexcode.cloud/v5/html

//Autotnomous Settings
int desiredValue = 0;
int desiredTurnValue = 0;


int error; //Sensor value - Desired Value - Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //Difference between error and previous error : Speed
int totalError = 0; //totalError = totalError + error

int turnError; //Sensor value - Desired Value - Position
int turnPrevError = 0; //Position 20 milliseconds ago
int turnDerivative; //Difference between error and previous error : Speed
int turnTotalError; //totalError = totalError + error

bool resetDriveSensors = false;

//Variables modified for use  
bool enabledrivePID = false;

////////////////////////////////////////PID VARIABLES END////////////////////////////////////////////////////////

//////////////////////////////////////////DRIVEPID///////////////////////////////////////////////////////////////

int drivePID(){

  while(enabledrivePID = true){
    
    if (resetDriveSensors) { 
      resetDriveSensors = false;

      LeftFront.setPosition(0, degrees);
      LeftBack.setPosition(0, degrees);
      RightFront.setPosition(0, degrees);
      RightBack.setPosition(0, degrees);
    }
    
    
    //Get the position of both motors
    int lfmp = LeftFront.position(degrees) ;
    int lbmp = LeftBack.position(degrees);
    int rfmp = RightFront.position(degrees) ;
    int rbmp = RightBack.position(degrees);


    ///////////////////////////////////////////////////////////////////////
    //Lateral Movement PID 
    ///////////////////////////////////////////////////////////////////////

    //Get average of the two motors
    int averagePosition = (lfmp+lbmp+rfmp+rbmp)/4;
    
    //Potential
    error = desiredValue - averagePosition;

    //Derivative
    derivative = error - prevError;

    //Integral
    //totalError += error;

    //DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!

    double lateralMotorPower = (error * kP + derivative * kD);
    //////////////////////////////////////////////////////////// ///////////  



    ///////////////////////////////////////////////////////////////////////
    //Turning Movement PID 
    ///////////////////////////////////////////////////////////////////////
    //Get average of the two motors
    int turnDifference = (lfmp+lbmp) - (rfmp+rbmp);
    
    //Potential
    turnError = desiredTurnValue - turnDifference;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //Integral
    //turnTotalError += turnError;

    //DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!

    double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD);
    ///////////////////////////////////////////////////////////////////////


    //start motors
    LeftFront.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    LeftBack.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightFront.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    RightBack.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    
    
    //code 
    prevError = error;
    turnPrevError = turnError; 
    vex::task::sleep(20);

  };

  return 1;
};
9

Sorry for the continuous posts, but I actually just ran into another “issue.”

Whenever I change the “desired value” variable, the robot turns. When I change the “desired turn value” variable, the robot goes straight.

Yes, I could just use the program like that, but if that could be changed, it would save a lot of time, as I forget stuff somewhat easily.

10

Yes, you do not have to use kI if you don’t want to. They way you described your tuning process is correct, and you can use the same method to tune turn constants.

11

Oh ok Thanks! Do you know how to solve that lssue with desired value and desired turn value being swapped.

12

For your turn difference, you are not correctly calculating the average position. I recommend having separate variables for the left and right side averages so you can replace int averagePosition = (lfmp+lbmp+rfmp+rbmp)/4; with int averagePosition = (leftAvg + rightAvg)/2 and replace int turnDifference = (lfmp+lbmp) - (rfmp+rbmp); with int turnDifference = leftAvg - rightAvg; You could also just use a inertial sensor for turning instead. If you are still having problems, it may be a hardware problem or that your desired value is interfering with your desired turn value. Unrelated, but also your code has a small problem at while(enabledrivePID = true) , it should be while(enabledrivePID == true) .