Drivetrain PID

Programming Support
1

Need some help with my drivetrain PID. I have tried to get this to simply drive forward and turn during autonomous but that’s not happening right now. I don’t know what values to put for kD, kI, kP so I have tested with 0.1 for now.
Comments are from a video I found online relating to PID, added them to keep track of stuff for myself.

Thanks, all.

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;
double kI = 0.1;
double kD = 0.1;
double turnkP = 0.1;
double turnkI = 0.1;
double turnkD = 0.1;

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


int error; //SensorValue - DesiredValue : position
int prevError = 0; //position 20 milliseconds ago
int derivative; //error - prevError : calculates speed
int totalError = 0; //totalError = totalError + error

int turnError; //SensorValue - DesiredValue : position
int turnPrevError = 0; //position 20 milliseconds ago
int turnDerivative; //error - prevError : calculates speed
int turnTotalError = 0; //totalError = totalError + error

bool resetDriveSensors = false;

//Variables modified for use
bool enableDrivePID = true;

int drivePID(){
  while(enableDrivePID){
    if(resetDriveSensors){
      resetDriveSensors = false;

      LeftMotor1.setPosition(0, degrees);
      RightMotor1.setPosition(0, degrees);



    }

    //Get pos of 2 motors
    int leftMotorPosition = LeftMotor1.position(degrees);
    int rightMotorPosition = RightMotor1.position(degrees);

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

    //Get avg of 2 motors
    int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

    //potential
    error = averagePosition - desiredValue;

    //Derivative
    derivative = error - prevError;

    //integral
    totalError += error;

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

    ////////////////////
    //Turning Movement PID
    ////////////////////
    int turnDifference = (leftMotorPosition + rightMotorPosition)/2;

    //potential
    turnError = turnDifference - desiredTurnValue;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //integral
    turnTotalError += turnError;

    double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;


    LeftMotor1.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotor1.spin(forward, lateralMotorPower - turnMotorPower, 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.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {

vex::task pidtest(drivePID);

resetDriveSensors = true;
desiredValue = 20;
desiredTurnValue = 30;

vex::task::sleep(1000);

resetDriveSensors = true;


}

/*---------------------------------------------------------------------------*/
2

I recommend sperating your PID into functions rather than a thread with a seperate one for driving and turning. Besides that, I also recommend testing with much higher PID values to check if there is actually a problem with your code.

4 Likes
3

That is actually not nesessary. There is nothing wrong with driving forward and turning (or correcting to drive straight) at the same time.

If your drive can skid to the side (i.e. all omnis, no central anti-skid tractions) then you just invoke this function separately for turning or driving forward.

The error, I see in your code is that averagePosition and turnDifference are computed the same. Perhaps you want to have '-' sign in the case of the latter?

Also, you always want to start with kD an kI set to zero and work your way up with kP.

There is an excellent explanation of PID at BLRS wiki - highly recommended.

1 Like
4

Also, forgot to mention you have to do something about integral windup. (post edit doesn’t work).

1 Like
5

Set your tuning variables to 1, no? If you set them to 0 then the result after multiplication will just equal 0 I thought.

6

The tuning method for PID is to set your ki and kd and gradually increase your kp to a point of slight oscillation. After this stop increasing your kp and increase your kd and ki until oscillation stops and your are happy with the result.

2 Likes