PID programming bugs

Programming Support VEXcode V5 Tech Support
, ,
1

Recently, my team has been looking into more accurate forms of motor control for skills autonomous, and stumbled across PID motor control. When attempting to implement our own controller, however, the program does several unexpected things. The lateral PID works well enough when driving forward and backwards, after tuning constants, but the turning fails to perform as expected, and I am failing to find error in my logic.

Our 6-motor drivetrain config

brain Thinky;

motor lm1(0, false);
motor lm2(1, false);
motor lm3(2, false);
motor rm1(7, true);
motor rm2(8, true);
motor rm3(9, true);

motor_group leftMotor(lm1, lm2, lm3);
motor_group rightMotor(rm1, rm2, rm3);

controller sticks;

encoder lquad = encoder(Thinky.ThreeWirePort.C);
encoder rquad = encoder(Thinky.ThreeWirePort.A);

our helper functions:

void reset(){
  lquad.resetRotation();
  rquad.resetRotation();
  curDeg=0;
}
double inchtodegrees(double val){
  double rotations = val/(4.125*PI); // calculations with wheel size
  double degrees = rotations*360;
  return degrees;
}
double degreestorad(double val){
  return val*PI/180;
}
double radtodegrees(double val){
  return val*180/PI;
}

our threads/tasks:

int getheading(){
  while(true){
    curDeg += ((lquad.position(degrees)-prevL) - (rquad.position(degrees)-prevR)) / (lWheelDist + rWheelDist);
    prevL = lquad.position(degrees);
    prevR = rquad.position(degrees);
    task::sleep(20);
  }
  return 1;
}

int headingPID() {
  while(enableDrivePID) {
    // Heading correction logic
    double headingError = targetDeg - curDeg;

    // Basic PID for heading correction
    turnTotalError += headingError;
    if (headingError == 0 || abs(headingError) > 20) turnTotalError = 0;

    double turnDerivative = headingError - turnPrevError;
    turnPrevError = headingError;

    // Adjust heading with PID terms
    double turnPower = headingError * tkP + turnTotalError * tkI + turnDerivative * tkD;

    // Clamp the turn power
    if (turnPower < -12.0) turnPower = -12.0;
    if (turnPower > 12.0) turnPower = 12.0;

    // Use turn power to correct heading while driving
    leftMotor.spin(forward, latpower + turnPower, voltageUnits::volt);
    rightMotor.spin(forward, latpower - turnPower, voltageUnits::volt);

    task::sleep(20);
  }
  return 1;
}

int drivePID(){
  while(enableDrivePID){
    double avgPos = (lquad.position(degrees)+rquad.position(degrees))/2;
    error = avgPos-inchtodegrees(driveDist);

    totalError += error;
    if(error==0 || abs(error) > 20) totalError=0;

    derivative = error-prevError;
    prevError=error;

    // Calculate drive power (forward movement)
    latpower = error * kP + totalError * kI + derivative * kD;

    if (latpower < -12.0) latpower = -12.0;
    if (latpower > 12.0) latpower = 12.0;

    // Let headingPID function handle heading correction
    task::sleep(20);
  }
  return 1;
}

our main autonomous loop is pretty basic:

void autonomous(void) {
  reset();
  //driveDist=12;
  //targetDeg=0;
  targetDeg=90;
  task odom(odometry);
  task dpid(drivePID);
  task hpid(headingPID);
}

yet when this portion runs, the drivetrain drives straight ahead instead of turning 90 degrees like I expected. I do not believe it is an encoder placement issue, and here’s a pictures of our encoder setup:

IMG_6103
IMG_61031920×2560 461 KB

IMG_6104
IMG_61041920×2560 315 KB

The encoders are directly attached to the wheels (for now) and are facing the same way, and they are attached to the brain correctly (top 3-wire first, then bottom 3-wire).

So I am wondering, what is the issue? If anybody could give any useful tips that would be amazing. :pray: :pray: :pray:

2

You need to make a thread for getheading() for it to run, like you did with the PIDs. There is nothing that calls getheading in your code, so your heading will always be 0.

Other tips:

(this also applies to regular total error)
This seems to try to address integral windup, but since headingError is a double and will pretty much never be 0, you should also reset the total error when the error passes 0 (you can do this with the previous error) to prevent the robot from extremely overcorrecting in some cases.

This should be switched, because you want to subtract your current position from your target position, not the other way around.

I would recommend (as this has worked well for me) making a button on the controller cycle your target PID values, and just switching whether it is using drive PID or turn PID in the code, to help you tune faster.

2 Likes
3

ah odometry() was originally named getheading(), and I forgot to change while transcribing to vex forums. The thread is being called though, thanks for the heads-up.

Will implement the windup fix as well as the negative error ASAP.

Thank you!

2 Likes