Unable to Reduce Variation in Drive PID

Programming Support VEXcode Pro V5 Text Tech Support
1

I’m trying to tune my drive-train PID and I’m having an issue. Between different runs of the function, I get extremely varied results. Sometimes the drive-train stops at exactly the right time, other times it’s +/- 0.3 revolutions and stalls out. I tried to tune kP kI and kD to fix this (tried at least 50 combinations) but I’ve had no luck.

Is there something wrong with my code? I’ve also got a motorSlew command but I don’t think that’s the issue.

double kPDrive = 10; //15;
double kIDrive = 0.2; //0.15;
double kDDrive = 8; //7;

double target;
double maxSpeedDrive = 12;

int drivePID() {
  while(1) {
    double targetRev = inToRev(target);
    double error = 0;
    double prevError = 0;
    double derivative = 0;
    double integral = 0;

    while( !adjustAngle && fabs(rightDtBack.rotation(rotationUnits::rev) - targetRev) > 0.02 ) {
      error = targetRev - rightDtBack.rotation(rotationUnits::rev);
      derivative = error - prevError;

      prevError = error;

      if(fabs(error) < 0.5 && error != 0) {
        integral += error;
      }
      else {
        integral = 0;
      } 

      double powerDrive = error*kPDrive + derivative*kDDrive + integral*kIDrive;

      if(powerDrive > maxSpeedDrive) {
        powerDrive = maxSpeedDrive;
      }
      else if(powerDrive < -maxSpeedDrive) {
        powerDrive = -maxSpeedDrive;
      }

      // cout << "power: " << powerDrive << endl;
      // cout << "error: " << error << endl;
      // cout << "rotation: " << rightDtBack.rotation(rotationUnits::rev) << endl;
      // cout << "targetRev: " << targetRev << endl;
      // cout << "prevEror: " << prevError << endl;
      // cout << "derivative: " << derivative << endl;
      // cout << "derivativePower: " << derivative*kDDrive << endl;
      // cout << "integral: " << integral*kIDrive << endl;
      // cout << "\\\\" << endl;

      goalVoltageDrive = powerDrive;
      this_thread::sleep_for(15);
    }

    goalVoltageDrive = 0;
    this_thread::sleep_for(15);
  }
  return 1;
}
bool up = true;
double actualVoltage = 0;
double goalVoltageDrive = 0;
double goalVoltageAngle = 0;
double step = 1;

int motorSlew() {
  while(1) {
    if(goalVoltageDrive == 0 && goalVoltageAngle == 0) {
      actualVoltage = 0;
      dt.spin(directionType::fwd, actualVoltage, voltageUnits::volt);
    }
    else if(goalVoltageDrive != 0 && goalVoltageAngle == 0) {
      if(up && actualVoltage <= goalVoltageDrive) {
        actualVoltage += step;

        if(actualVoltage >= goalVoltageDrive) {
          actualVoltage = goalVoltageDrive;
          up = false;
        }
      } else {
        up = false;
        actualVoltage -= step;

        if(actualVoltage <= goalVoltageDrive) {
          actualVoltage = goalVoltageDrive;
          up = true;
        }
      }

      dt.spin(directionType::fwd, actualVoltage, voltageUnits::volt);
    }
    else if(goalVoltageDrive == 0 && goalVoltageAngle != 0) {
      if(up && actualVoltage <= goalVoltageAngle) {
        actualVoltage += step;

        if(actualVoltage >= goalVoltageAngle) {
          actualVoltage = goalVoltageAngle;
          up = false;
        }
      } else {
        up = false;
        actualVoltage -= step;

        if(actualVoltage <= goalVoltageAngle) {
          actualVoltage = goalVoltageAngle;
          up = true;
        }
      }
      
      rightDt.spin(directionType::rev, actualVoltage, voltageUnits::volt);  
      leftDt.spin(directionType::fwd, actualVoltage, voltageUnits::volt); 
    }
    else {
      cout << "CONFLICTING: BOTH MOTORPIDTASK & ROTATEPIDTASK ARE SENDING VALUES > 0" << endl;
    }
    
    this_thread::sleep_for(15);
  }
}
2

I would suggest lowering your max voltage. Slower drive speeds are more accurate.

As for tuning, @Connor said it well in another thread:
"You increase P until there is subtle oscillation, then you increase D until the oscillation stops, and then you increase I until the target is exactly achieved. "

One other possible issue could be use of chain in the drive. Would not cause a .3 revolution difference, but any motor turning that is put to taking out slack in the chain before the wheels turn is still counted as movement to the target by your code.

3 Likes