Still having PID trouble

#1

For some time I have been trying to develop a PID. I took a look at @Connor’s video on how to make one. I understand some parts of it, but cannot seem to get it working in autonomous. If someone could tell me what’s wrong with the code, and maybe help me a little with the changes, that would be amazing.

double kP = 1; // Error
double kI = 0.001 ; // Last, Increase Precision
double kD = 0.01; // To Fast or Too Slow

double turnkP = 1 ;
double turnkI = 0.001;
double turnkD = 0.1;

// Auton Settings
int targetValue = 200;
int targetTurnValue = 0; 

double error; // SensorValue - DesireValue : Positional Value 
double previouserror = 0; // Position 20 miliseconds ago
double derivative; // error - previouserror : Speed
double totalerror= 0 ; // totalerror = totalerror + error

double turnerror; // SensorValue - DesireValue : Positional Value 
double turnpreviouserror = 0; // Position 20 miliseconds ago
double turnderivative; // error - previouserror : Speed
double turntotalerror= 0; // totalerror = totalerror + error

bool resetDriveSensors = true;

// Variation modified for use
bool enableDrivePID = true;

int drivePID(){

  while(enableDrivePID){

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


    // Get Position of Both Motors
    int leftMotorPosition = LeftMotor.position(degrees);
    int rightMotorPosition = RightMotor.position(degrees);

   

    // Average Of the Motors
    int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

    // Proportional
    error = averagePosition - targetValue;

    // Derivative
    derivative = error - previouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    totalerror += error;

    double lateralMotorPower = (error * kP + derivative * kD + totalerror * kI);
  
    
    // Average Of the Motors
    int turnDifference = leftMotorPosition - rightMotorPosition;

    // Proportional
    turnerror = turnDifference - targetTurnValue;

    // Derivative
    turnderivative = turnerror - turnpreviouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    turntotalerror += turnerror;

    double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI) ;

    ///////////////////////////////////////////////////////////////////////

    LeftMotor.spin(reverse, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);



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

  }

  return 1; 
}


/*---------------------------------------------------------------------------*/

void autonomous(void) {

  vex::task start(drivePID);
  targetValue = 300 ;
Autonomous Consistency
#2

Check here to see if I’ve already answer it.

If you still have specific questions to them post back here.

3 Likes
#3

Hello, I have looked at that thread and I have a few questions.

  1. What does Clamping do exactly?
  2. I don’t really understand much about the derivative value in a PID
  3. I tried copy/pasting some of the sample code from the other forum to see if it would work, but I keep getting errors in my code. The errors are at the int main() at the bottom of the code.
//
// P - Proportional ( Like P-Control )
//
// I - Integral ( )
//
// D - Derivative ( )
//
//


#include "vex.h"

using namespace vex;

// A global instance of competition
competition Competition;

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

extern inertial Inertial20 ;

extern motor LeftMotor;
extern motor RightMotor;
extern motor BLM ;
extern motor BRM ;

extern drivetrain Drivetrain ;

extern controller Controller1 ;

extern inertial InertialS ;



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, ...
}

// Settings
double kP = 12; // Error
double kI = 12 ; // Last, Increase Precision
double kD = 12; // To Fast or Too Slow

double turnkP = 1 ;
double turnkI = 0.00;
double turnkD = 0.1;

// Auton Settings
int targetValue = 200;
int targetTurnValue = 0; 

double error; // SensorValue - DesireValue : Positional Value 
double previouserror = 0; // Position 20 miliseconds ago
double derivative; // error - previouserror : Speed
double totalerror= 0; // totalerror = totalerror + error

double turnerror; // SensorValue - DesireValue : Positional Value 
double turnpreviouserror = 0; // Position 20 miliseconds ago
double turnderivative; // error - previouserror : Speed
double turntotalerror= 0; // totalerror = totalerror + error

bool resetDriveSensors = true;

// Variation modified for use
bool enableDrivePID = true;

bool pidAbort = false;      // added for testing during usercontrol
double deadBand = 5.0; // this is the close enough to target value

int drivePID(){

  while(1) {

    if (resetDriveSensors) {
      resetDriveSensors = false;
      totalerror = 0.0;
      previouserror = 0.0;
      turntotalerror = 0.0;
      turnpreviouserror = 0.0;
      LeftMotor.setPosition(0,degrees);
      RightMotor.setPosition(0,degrees);
    }

    if (enableDrivePID) {
      // PID CODE GOES HERE

      // stop PID from controlling motor when at taget

      if((fabs(error) < deadBand) || pidAbort) {

        enableDrivePID = false;

      }

      task::sleep(20);
    }

    ////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////

    


    ////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////


    // Get Position of Both Motors
    int leftMotorPosition = LeftMotor.position(degrees);
    int rightMotorPosition = RightMotor.position(degrees);

   

    // Average Of the Motors
    int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

    // Proportional
    error = targetValue - averagePosition ;

    // Derivative
    derivative = error - previouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    totalerror += error;

    double lateralMotorPower = (error * kP + derivative * kD + totalerror * kI);
  
    
    // Average Of the Motors
    int turnDifference = leftMotorPosition - rightMotorPosition;

    // Proportional
    turnerror = turnDifference - targetTurnValue;

    // Derivative
    turnderivative = turnerror - turnpreviouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    turntotalerror += turnerror;

    double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI) ;

    ///////////////////////////////////////////////////////////////////////

    LeftMotor.spin(reverse, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);



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

  }

  return 1; 
}

void pidMoveToPosition (double setPoint, double setTurn, bool wait4done=true) {
      resetDriveSensors = true;
      targetValue = setPoint;
      targetTurnValue = setTurn;
      enableDrivePID = true;
      while (enableDrivePID & wait4done) {
        pidAbort = Controller1.ButtonB.pressing();  // abort PID by press button B
        vex::task::sleep(20);
      }
      pidAbort = false;
  return; 
}



/*---------------------------------------------------------------------------*/

void autonomous(void) {

  vex::task start(drivePID);
  targetValue = 300 ;

  task::sleep(1000);

  Inertial20.calibrate(false) ;

  while(Inertial20.isCalibrating()){
    wait(1,msec);
  }

  LeftMotor.spin(directionType::fwd);
  RightMotor.spin(directionType::fwd);
  BLM.spin(directionType::fwd);
  BRM.spin(directionType::fwd);

  waitUntil(Inertial20.rotation(degrees) >= 270 );

    LeftMotor.stop() ;
    RightMotor.stop() ;
    BLM.stop() ;
    BRM.stop() ;
  


 
  




}



void usercontrol(void) {

  enableDrivePID = false;

  while (1) {

    vex::task t1(drivePID);

    // ADD THIS CODE - it doesn't mater if it is before or after Driver Control code
    // start PID by pressing button A
    if (Controller1.ButtonA.pressing()) {
     pidMoveToPosition(720,0,true);
     pidMoveToPosition(-720,0,true);

    


    
    

   

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

//
// 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();

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

  1. Clamping prevents the integral term from getting much larger than the max value of the motor voltage. Also if you are going to use the turn part of the PID then you also need to clamp the output of the first drive PID so that it doesn’t get to say 40. And then you are getting a turn output of 3 so your two values are 40+3 and 40-3 both of which are greater than 12 so the motor clamps to 12 and then you don’t turn.

  2. Derivative attempts to limit how fast the change in error affects the output. It tries to smooth large jumps between the last point measured and the current point measured. I kind of think of it as putting a damper on the enthusiasm of the P term early on and then later (if enough iterations are run) possibly the I term. It seems to be the trickiest to get tuned from what I’ve read.

  3. The error is not in main but above it. You are missing a } somewhere. I suggest selecting Tools->Format Document from the menu to make brace alignment easy to see. Then cursor up to each brace } and see where its companion brace { is. It shouldn’t be too many above main.

4 Likes
#5

Okay, so when I started the program the wheels just spun super fast, then changed directions many times very fast.

#6

double kP = 12; // Error
double kI = 12; // Last, Increase Precision
double kD = 12; // To Fast or Too Slow

these values are way to large. start small with kP first slowing increasing it until it gets to your target reasonable fast. for tuning look up Ziegler-Nichols method
double kP = 0.02; // Error
double kI = 0.0 // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow

I do see an issue with the drivePID code.

  1. there are two task::sleep(20);
  2. the PID code is not in the if (enableDrivePID) { code block meaning it will run all the time and not stop driving the motors. This is something you might do if you need hold. However there are better ways to implement that by adding a enableHold variable to that if-statement.
  3. targetValue and targetTurnValue need to be the return type of motor.position()
    https://api.vexcode.cloud/v5/html/classvex_1_1motor.html#a16746eaf0955f4c9c5d6330834288a59
3 Likes
#7

targetValue and targetTurnValue have to be the return types of what?
LeftMotor.position() or RightMotor.position() and where would I put it?
Sorry Im kind of new to text code.

#8

you have them declared as int, they need to be double.
in your posted code

// Auton Settings
int targetValue = 200;
int targetTurnValue = 0;

and

    int leftMotorPosition = LeftMotor.position(degrees);
    int rightMotorPosition = RightMotor.position(degrees);

from the API:
◆ position()
double vex::motor::position ( rotationUnits units )
Gets the current position of the motor’s encoder.

Returns
Returns a double that represents the current position of the motor in the units defined in the parameter.
Parameters
units The measurement unit for the position.

4 Likes
#9

Okay the wheels are still moving fast and changing directions sporadically.
Here is the updated code.

//
// P - Proportional ( Like P-Control )
//
// I - Integral ( )
//
// D - Derivative ( )
//
//

#include "vex.h"

using namespace vex;

// A global instance of competition
competition Competition;

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

extern inertial Inertial20;

extern motor LeftMotor;
extern motor RightMotor;
extern motor BLM;
extern motor BRM;

extern drivetrain Drivetrain;

extern controller Controller1;

extern inertial InertialS;

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, ...
}

// Settings
double kP = 0.1; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow

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

// Auton Settings
double targetValue = 200;
double targetTurnValue = 0;

double error;             // SensorValue - DesireValue : Positional Value
double previouserror = 0; // Position 20 miliseconds ago
double derivative;        // error - previouserror : Speed
double totalerror = 0;    // totalerror = totalerror + error

double turnerror;             // SensorValue - DesireValue : Positional Value
double turnpreviouserror = 0; // Position 20 miliseconds ago
double turnderivative;        // error - previouserror : Speed
double turntotalerror = 0;    // totalerror = totalerror + error

bool resetDriveSensors = true;

// Variation modified for use
bool enableDrivePID = true;

bool pidAbort = false; // added for testing during usercontrol
double deadBand = 5.0; // this is the close enough to target value

int drivePID() {

  if (enableDrivePID) {
      // PID CODE GOES HERE

      // stop PID from controlling motor when at taget

      if ((fabs(error) < deadBand) || pidAbort) {

        enableDrivePID = false;
      }

      task::sleep(20);
    }

  while (1) {

    if (resetDriveSensors) {
      resetDriveSensors = false;
      totalerror = 0.0;
      previouserror = 0.0;
      turntotalerror = 0.0;
      turnpreviouserror = 0.0;
      LeftMotor.setPosition(0, degrees);
      RightMotor.setPosition(0, degrees);
    }

  

    ////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////



    ////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////

    // Get Position of Both Motors
    double leftMotorPosition = LeftMotor.position(degrees);
    double rightMotorPosition = RightMotor.position(degrees);

    // Average Of the Motors
    double averagePosition = (leftMotorPosition + rightMotorPosition) / 2;

    // Proportional
    error = targetValue - averagePosition;

    // Derivative
    derivative = error - previouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    totalerror += error;

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

    // Average Of the Motors
    double turnDifference = leftMotorPosition - rightMotorPosition;

    // Proportional
    turnerror = turnDifference - targetTurnValue;

    // Derivative
    turnderivative = turnerror - turnpreviouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    turntotalerror += turnerror;

    double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD +
                             turntotalerror * turnkI);

    ///////////////////////////////////////////////////////////////////////

    LeftMotor.spin(reverse, lateralMotorPower + turnMotorPower,
                   voltageUnits::volt);
    RightMotor.spin(forward, lateralMotorPower + turnMotorPower,
                    voltageUnits::volt);

    previouserror = error;
    turnpreviouserror = turnerror;
    task::sleep(20);
  }

  return 1;
}

void pidMoveToPosition(double setPoint, double setTurn, bool wait4done = true) {
  resetDriveSensors = true;
  targetValue = setPoint;
  targetTurnValue = setTurn;
  enableDrivePID = true;
  while (enableDrivePID & wait4done) {
    pidAbort = Controller1.ButtonB.pressing(); // abort PID by press button B
    vex::task::sleep(20);
  }
  pidAbort = false;
  return;
}

/*---------------------------------------------------------------------------*/

void autonomous(void) {

  /*vex::task start(drivePID);
  targetValue = 300;

  task::sleep(1000);

  Inertial20.calibrate(false);

  while (Inertial20.isCalibrating()) {
    wait(1, msec);
  }

  LeftMotor.spin(directionType::fwd);
  RightMotor.spin(directionType::fwd);
  BLM.spin(directionType::fwd);
  BRM.spin(directionType::fwd);

  waitUntil(Inertial20.rotation(degrees) >= 270);

  LeftMotor.stop();
  RightMotor.stop();
  BLM.stop();
  BRM.stop();*/
}

void usercontrol(void) {

  enableDrivePID = false;

  while (1) {

    vex::task t1(drivePID);

    // ADD THIS CODE - it doesn't mater if it is before or after Driver Control
    // code start PID by pressing button A
    if (Controller1.ButtonA.pressing()) {
      pidMoveToPosition(720, 0, true);
      pidMoveToPosition(-720, 0, true);

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

  //
  // 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();

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

You still need to fix #1 (task sleep) and #2 put PID code inside if (enableDrivePID) { from my prior post.
I’m not sure what starting a task in a while loop does but I’m pretty sure you should not be doing that. Start the PID task in main(). This way it is available before Autonomous (for when you get it working).

2 Likes
#11

Ok so I put the PID code inside the if enableDrivePID statement. Where do I put the if statement in? A while loop?

// Settings
double kP = 0.01; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow

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

// Auton Settings
double targetValue = 200;
double targetTurnValue = 0;

double error;             // SensorValue - DesireValue : Positional Value
double previouserror = 0; // Position 20 miliseconds ago
double derivative;        // error - previouserror : Speed
double totalerror = 0;    // totalerror = totalerror + error

double turnerror;             // SensorValue - DesireValue : Positional Value
double turnpreviouserror = 0; // Position 20 miliseconds ago
double turnderivative;        // error - previouserror : Speed
double turntotalerror = 0;    // totalerror = totalerror + error

bool resetDriveSensors = true;

// Variation modified for use
bool enableDrivePID = true;

bool pidAbort = false; // added for testing during usercontrol
double deadBand = 5.0; // this is the close enough to target value

if (enableDrivePID) {

  int drivePID() {

    // Get Position of Both Motors
    double leftMotorPosition = LeftMotor.position(degrees);
    double rightMotorPosition = RightMotor.position(degrees);

    // Average Of the Motors
    double averagePosition = (leftMotorPosition + rightMotorPosition) / 2;

    // Proportional
    error = targetValue - averagePosition;

    // Derivative
    derivative = error - previouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    totalerror += error;

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

    // Average Of the Motors
    double turnDifference = leftMotorPosition - rightMotorPosition;

    // Proportional
    turnerror = turnDifference - targetTurnValue;

    // Derivative
    turnderivative = turnerror - turnpreviouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    turntotalerror += turnerror;

    double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD +
                             turntotalerror * turnkI);

    ///////////////////////////////////////////////////////////////////////

    LeftMotor.spin(reverse, lateralMotorPower + turnMotorPower,
                   voltageUnits::volt);
    RightMotor.spin(forward, lateralMotorPower + turnMotorPower,
                    voltageUnits::volt);

    previouserror = error;
    turnpreviouserror = turnerror;
    task::sleep(20);
     
  }

  return 1;

      
      // PID CODE GOES HERE

      // stop PID from controlling motor when at taget

      if ((fabs(error) < deadBand) || pidAbort) {

        enableDrivePID = false;
      }
    
     while (1) {

    if (resetDriveSensors) {
      resetDriveSensors = false;
      totalerror = 0.0;
      previouserror = 0.0;
      turntotalerror = 0.0;
      turnpreviouserror = 0.0;
      LeftMotor.setPosition(0, degrees);
      RightMotor.setPosition(0, degrees);
    }
  }

void pidMoveToPosition(double setPoint, double setTurn, bool wait4done = true) {
  resetDriveSensors = true;
  targetValue = setPoint;
  targetTurnValue = setTurn;
  enableDrivePID = true;
  while (enableDrivePID & wait4done) {
    pidAbort = Controller1.ButtonB.pressing(); // abort PID by press button B
    vex::task::sleep(20);
  }
  pidAbort = false;
  return;
}

/*---------------------------------------------------------------------------*/

void autonomous(void) {

  /*vex::task start(drivePID);
  targetValue = 300;

  task::sleep(1000);

  Inertial20.calibrate(false);

  while (Inertial20.isCalibrating()) {
    wait(1, msec);
  }

  LeftMotor.spin(directionType::fwd);
  RightMotor.spin(directionType::fwd);
  BLM.spin(directionType::fwd);
  BRM.spin(directionType::fwd);

  waitUntil(Inertial20.rotation(degrees) >= 270);

  LeftMotor.stop();
  RightMotor.stop();
  BLM.stop();
  BRM.stop();*/
}

void usercontrol(void) {

  enableDrivePID = false;



    // ADD THIS CODE - it doesn't mater if it is before or after Driver Control
    // code start PID by pressing button A
    if (Controller1.ButtonA.pressing()) {
      pidMoveToPosition(720, 0, true);
      pidMoveToPosition(-720, 0, true);

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

  //
  // 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);

    vex::task t1(drivePID);

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

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

It would be if, then int drive pid, then the while loop

1 Like
#13

if (enableDrivePID) {

int drivePID() {

while(1)
{
//pid code here
}

1 Like
#14

I get an error on the if part. Do i have to put this somewhere else?

#15

what does the error say?

#16

[clang] expected unqualified id

#17

Strange. I’d offer to look through it but I’m really tired and not really a PID expert. I’ll leave this to @rpm. If for some reason rpm doesn’t see it, maybe I can look tomorrow.

1 Like
#18

No Problem! Thanks for the help.

1 Like
#19

The PID code needs a few updates but looks like it will mostly work so you just need to get the order of operation correct. One thing is set turnKp to 0.0 so you are only debugging drviePID.

The order for the PID task is

int drivePID() {                              // this is the task 
  // put local variable definitions here. its ok for your PID vars to be global for now

  while (1) {        // continuously monitor resetDriveSensors and enableDrivePID

    if (resetDriveSensors) { // really this is resetDrivePID not just the sensors
      // reset code goes here
    } 

    if (enableDrivePID) { 

      // Put PID code goes here

      // stop the PID from driving motors when target is reached
      // currently does not support hold 
      if ((fabs(error) < deadBand) || pidAbort) {
        enableDrivePID = false;
      }
    } // if (enableDrivePID)  -- i like to comment the end of long code blocks

    vex::task::sleep(20); 
  } // while (1)
  return 1;
} // drivePID
4 Likes
#20

Ok great its working now. So to increase the accuracy if I use tracking wheels would this work better?