Rotational PID Issues

Programming Support VEXcode Pro V5 Text Tech Support
#1

I am a beginner in the realm of PID control loops, so if anybody is willing to help me point out any issues with my program that would be incredibly appreciated.

The issues are that our robots drivetrain will turn endlessly, and that It turns on the right side instead of turning in place.

Code Pasted below:

double kPAngle = 0.1;
double kIAngle = 0.0;
double kDAngle = 0.0;
double maxSpeed = 6;


void rotatePID(double angleTurn) {
  double error = 0;
  double lastError = 0;
  double Derivative = 0;
  double integral = 0;
  double totalerror = 0;


  while( fabs(Inertial.rotation() - angleTurn) > 0.5) {
    error = angleTurn - Inertial.rotation(rotationUnits::deg);
    derivative = error - lastError;
    lastError = error;

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

  if(abs(error) < integralBound) {
      totalerror+=error;
    }
    else {
      totalerror = 0;
    }
    //turntotalError += turnerror;
    
    //This would cap the integral
    turntotalError = abs(turntotalError) > maxTurnIntegral ? signnum_c(turntotalError) * maxTurnIntegral : turntotalError;

    double powerDrive = error*kPAngle + Derivative*kDAngle + integral*kIAngle;

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

       
    //cout << "power: " << powerDrive << endl;
    //cout << "angle: " << Inertial.rotation() << endl;
    //cout << "error: " << error << endl;
    // cout << "prevError: " << prevError << endl;
    // cout << "derivative: " << derivative << endl;
    // cout << "integral: " << integral*kIDrive << endl;
    //cout << "////" << endl;
    
    // turning = true;
    // goalVoltage = powerDrive;
    rightfront.spin(forward, powerDrive, voltageUnits::volt);
    leftfront.spin(reverse, powerDrive, voltageUnits::volt);
    rightback.spin(forward, powerDrive, voltageUnits::volt);
    rightfront.spin(reverse, powerDrive, voltageUnits::volt);

    this_thread::sleep_for(15);
  }
  
  // turning = false;
  // goalVoltage = 0;
  rightfront.stop(brakeType::brake);
leftfront.stop(brakeType::brake);
rightback.stop(brakeType::brake);
leftback.stop(brakeType::brake);
return;
}


/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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 initpid(drivePID);
Inertial.calibrate(2000);
this_thread::sleep_for(2000);
//cout << "Done calibrating" << endl;

Inertial.setRotation(270.00, rotationUnits::deg);
//cout << "rotation: " << Inertial.rotation() << endl;

rotatePID(180);
this_thread::sleep_for(1000);
rotatePID(90);
this_thread::sleep_for(1000);
rotatePID(0);
this_thread::sleep_for(1000);
rotatePID(-90);
this_thread::sleep_for(1000);

edit: code tags added by mods.

#2

You should use

while( fabs(error) > 0.5)

Also your code spin commands spin your right front motor twice both forward and reverse and does not spin your left back motor at all.

I would also recommend starting with something simpler. It seems you have “jumped in” to PID loops and its more effective to start small with just a P loop and add from there. This helps you understand what is happening deeper and make sure each piece individually works.

3 Likes
#3

Thank you for the information, but I cannot afford to suddenly change course after having spent so much time. My team is depending on me and I am going to try my best to deliver. Thank you for your concern though! If you notice any other issues please update me.

#4

Yes, please learn how a PID works and try to work out your issues or start over before you ask the forum. Nevertheless

  • You should put lastError = error; right at the end just before the wait. This will make it marginally more predictable.
  • You did rightfront forward once and reverse once, and leftback (which I assume exists) not at all. as @Bradley_Andersen mentioned.
  • Your if (fabs(error) < 5) is indented only to the same level as the while loop. I know it doesn’t cause compile errors, but it makes code much easier to read.
  • You’re using a variable error, but isn’t that also in your going straight loop? Wouldn’t you want to use turnError?
3 Likes
#5

I usually would, but I am very short on time and am trying to make as much progress as fast as possible. I genuinely appreciate the help, thank you so much!