Example PID loop for turning?

I was wondering if anyone could provide a little sample code for their turning with a PID loop. I was reading Gillard’s guide, and he says that the robot should oscillate after it reaches its value. However, the way my code is written, it will always stop once it reaches the sensor value. I’ll post mine…

void turnRight(int targetAngle) {
float integral = 0;
float derivative = 0;
float lastError = 0;
float kP = 0;
float kI = 0;
float kD = 0;
float error = 0;
float motorSpeed = 0;
while (InertialSensor.heading(rotationUnits::deg) < targetAngle) {
error = targetAngle - InertialSensor.heading(rotationUnits::deg);
integral = integral + error;
if (error == 0) {
integral = 0;
}
if (error > 100){
integral = 0;
}
derivative = error - lastError;
lastError = error;
motorSpeed = errorkP + integralkI + derivative*kD;
FRDrive.spin(directionType::fwd, motorSpeed, velocityUnits::pct);
FLDrive.spin(directionType::rev, motorSpeed, velocityUnits::pct);
BRDrive.spin(directionType::fwd, motorSpeed, velocityUnits::pct);
BLDrive.spin(directionType::rev, motorSpeed, velocityUnits::pct);
wait(15);
}
FRDrive.stop(brakeType::brake);
FLDrive.stop(brakeType::brake);
BRDrive.stop(brakeType::brake);
BLDrive.stop(brakeType::brake);
return;
}

I changed all the kp, ki, and kd values back to 0 just for the example…but can anyone tell me if this code should work, and how they have tuned theirs?

1 Like