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