This is not a PID algorithm. Let’s start by taking a look at the actual loop:
// Calculate error
float error = adcCalculateError(initialDegree, movedDegree, step);
writeDebugStreamLine("adcMove(), error = %f", error);
accError += error;
distance -= step;
}
while (distance > 0);
accError *= FINE_TUNE_RATIO;
// Only adjust the error while error > 10mm
if (abs(accError) > 10)
{
switch (moveCommand)
{
case FORWARD:
case BACKWARD:
horizontalMove(accError, 50);
writeDebugStreamLine("adcMove(), horizontalError = %f", accError);
break;
case LEFTWARD:
case RIGHTWARD:
verticalMove(accError, 50);
writeDebugStreamLine("adcMove(), verticalError = %f", accError);
}
}
Accumulating the error is in fact done in PID, but not for the purpose demonstrated here. You simply do it to move the drivetrain when the error is significant. Once it is, you call either horizontalMove() or verticalMove().
We’ll first analyze horizontalMove()
bool horizontalMove(float distance, short speed)
{
resetGyro(gyroSensor);
resetMotorEncoder(midMotor);
startTask(keepStraight);
setMotor(midMotor, speed);
float motorDegree = distance * CENTER_WHEEL_SPEED;
waitUntil(abs(getMotorEncoder(midMotor)) >= motorDegree);
stopTask(keepStraight);
stopMotor(midMotor);
return true;
}
This does little more than spin the motor until it reaches a certain number of degrees, which you determine by calculating the error. It runs your algorithm to drive straight during this, but that is not PID.
You actually don’t need to call this function in a while loop since it won’t end until the bot reaches its target. You could simply measure out the distance you need to drive and call the function passing your measurement as a parameter.
Now let’s look at verticalMove()
bool verticalMove(float distance, short speed)
{
float motorDegree = distance * DRIVETRAIN_SPEED;
moveMotorTarget(leftMotor, motorDegree, speed);
// moveMotorTarget(rightMotor, motorDegree, speed * -1);
moveMotorTarget(rightMotor, motorDegree, speed);
// Holds program flow until the motors comes to a complete stop.
waitUntilMotorStop(leftMotor);
waitUntilMotorStop(rightMotor);
return true;
}
This essentially does the same thing as horizontalMove(), just using moveMotorTarget() instead of starting the motor and stopping it when the encoder reads that it reached its target, and not using your keepStraight algorithm.
Notice how for both of these, the speed of the motors will be the same no matter how large your error is. This is not how PID works.
PID stands for Proportional, Integral, Derivative. It is a feedback loop that uses a proportional measurement of a system’s error (P), the accumulation of the system’s error (I), and the rate of change of that error (D) to accurately drive the error of the system to 0.
Starting with the Proportional (P) term, we simply multiply our error by a constant kp. As the error decreases, so will the Proportional term, meaning in this case that the robot will slow down as it nears its target. The problems with this alone are steady state error and that it can overshoot its target and oscillate.
To solve the overshooting problem, we use the Derivative (D) term. This is found by multiplying the derivative of our error by a constant kd. In this case, the Derivative term allows us to slow our robot down proportionally to its current speed. The faster the robot is moving, the more the Derivative term does to lower its speed, and vice versa. However, we can still run into the problem of steady state error.
Steady state error is when the error of a system is not 0, but remaining constant. For example, if the robot is 20mm from the target, the Proportional term may not be enough to overcome friction, and since the robot isn’t moving the Derivative term will be 0. In a lot of cases, this isn’t enough to matter. However, over the course of a programming skills run, it could add up and cause inconsistency.
To solve this, we use the Integral term. This is calculated by taking the integral of the error and multiplying it by a constant ki. Since the integral is an accumulation, this will continue to grow as long as the error of the system isn’t 0, so it will be able to eliminate steady state error. It also runs the risk of getting too large though, because the system may start with a large error. Thus, we put restrictions on the Integral term such as only allowing it to accumulate a certain distance from the target, and putting a hard limit on how high it can get.
Here is an example of a PID loop:
void PID(double target) { //target is the desired state of the system
double kp = 0.33;
double ki = 0.0005;
double kd = 0.2;
//you need to tune the constants for yourself;
double proportion;
double totalError;
double integral;
double integralActiveZone = 90; //you need to tune this with the constants
double integralPowerLimit = 50 / ki; //this will probably need to be adjusted too, depending on the velocity units
double derivative;
double finalPower;
double error = target;
double lastError = target;
sleepMs(50);
while(abs(error) > 3){ //will stop when the system is within 3 units of target, in this case degrees;
error = target - motor.rotation(rotationUnits::deg);
if(error == 0){
break;
}
proportion = kp * error;
if(abs(error) < integralActiveZone && error != 0){
totalError += error;
}
else totalError = 0.0; //sets the Integral term to 0 if it's too far from the target
//hard limit on the integral term
if(totalError > integralPowerLimit){
totalError = integralPowerLimit;
}
if(totalError < -integralPowerLimit){
totalError = -integralPowerLimit;
}
integral = ki * totalError;
derivative = kd * (error - lastError);
lastError = error;
if(error == 0){
derivative = 0;
}
finalPower = 0.5 * (proportion + integral + derivative); //.5 is the gain; you can adjust that as needed
motor.spin(fwd, finalPower, velocityUnits::pct);
sleepMs(20);
}
motor.stop();
Hopefully this helps clear up some of the confusion.
Edit: @Illyana caught a missed negative in the Integral term limit
