# Rotational PID Issues

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

}

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

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                                                                           */
/*  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.
// ..........................................................................

Inertial.calibrate(2000);
//cout << "Done calibrating" << endl;

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

rotatePID(180);
rotatePID(90);
rotatePID(0);
rotatePID(-90);
``````

edit: code tags added by mods.

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

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.

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

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!