PID Control: Is adding a delay before the next loop a good idea?

Question

I am implementing PID control in C++ to make a differential drive robot turn an accurate number of degrees, but I am having many issues.

Exiting control loop early due to fast loop runtime

If the robot measures its error to be less than 0.5 degrees, it exits the control loop and consider the turn "finished" (the 0.5 is a random value that I might change at some point). It appears that the control loop is running so quickly that the robot can turn at a very high speed, turn past the set point, and exit the loop/cut motor powers, because it was at the set point for a short instant. I know that this is the entire purpose of PID control, to accurately reach the set point without overshooting, but this problem is making it very difficult to tune the PID constants. For example, I try to find a value of kp such that there is steady oscillation, but there is never any oscillation because the robot thinks it has "finished" once it passes the set point. To fix this, I have implemented a system where the robot has to be at the setpoint for a certain period of time before exiting, and this has been effective, allowing oscillation to occur, but the issue of exiting the loop early seems like an unusual problem and my solution may be incorrect.

D term has no effect due to fast runtime

Once I had the robot oscillating in a controlled manner using only P, I tried to add D to prevent overshoot. However, this was having no effect for the majority of the time, because the control loop is running so quickly that 19 loops out of 20, the rate of change of error is 0: the robot did not move or did not move enough for it to be measured in that time. I printed the change in error and the derivative term each loop to confirm this and I could see that these would both be 0 for around 20 loop cycles before taking a reasonable value and then back to 0 for another 20 cycles. Like I said, I think that this is because the loop cycles are so quick that the robot literally hasn't moved enough for any sort of noticeable change in error. This was a big problem because it meant that the D term had essentially no effect on robot movement because it was almost always 0. To fix this problem, I tried using the last non-zero value of the derivative in place of any 0 values, but this didn't work well, and the robot would oscillate randomly if the last derivative didn't represent the current rate of change of error.

Note: I am also using a small feed forward for the static coefficient of friction, and I call this feed forward "f"

Should I add a delay?

I realized that I think the source of both of these issues is the loop running very very quickly, so something I thought of was adding a wait statement at the end of the loop. However, it seems like an overall bad solution to intentionally slow down a loop. Is this a good idea?

turnHeading(double finalAngle, double kp, double ki, double kd, double f){
    std::clock_t timer;
    timer = std::clock();

    double pastTime = 0;
    double currentTime = ((std::clock() - timer) / (double)CLOCKS_PER_SEC);

    const double initialHeading = getHeading();
    finalAngle = angleWrapDeg(finalAngle);

    const double initialAngleDiff = initialHeading - finalAngle;
    double error = angleDiff(getHeading(), finalAngle);
    double pastError = error;

    double firstTimeAtSetpoint = 0;
    double timeAtSetPoint = 0;
    bool atSetpoint = false;

    double integral = 0;
    double derivative = 0;
    double lastNonZeroD = 0;

    while (timeAtSetPoint < .05)
    {
        updatePos(encoderL.read(), encoderR.read());
        error = angleDiff(getHeading(), finalAngle);

        currentTime = ((std::clock() - timer) / (double)CLOCKS_PER_SEC);
        double dt = currentTime - pastTime;

        double proportional = error / fabs(initialAngleDiff);
        integral += dt * ((error + pastError) / 2.0);
        double derivative = (error - pastError) / dt;
        
        //FAILED METHOD OF USING LAST NON-0 VALUE OF DERIVATIVE
        // if(epsilonEquals(derivative, 0))
        // {
        //     derivative = lastNonZeroD;
        // }
        // else
        // {
        //     lastNonZeroD = derivative;
        // }

        double power = kp * proportional + ki * integral + kd * derivative;

        if (power > 0)
        {
            setMotorPowers(-power - f, power + f);
        }
        else
        {
            setMotorPowers(-power + f, power - f);
        }

        if (fabs(error) < 2)
        {
            if (!atSetpoint)
            {
                atSetpoint = true;
                firstTimeAtSetpoint = currentTime;
            }
            else //at setpoint
            {
                timeAtSetPoint = currentTime - firstTimeAtSetpoint;
            }
        }
        else //no longer at setpoint
        {
            atSetpoint = false;
            timeAtSetPoint = 0;
        }
        pastTime = currentTime;
        pastError = error;
    }
    setMotorPowers(0, 0);
}

turnHeading(90, .37, 0, .00004, .12);

Answer 1

implementing discontinuities into a feedback controller is usually not the best idea. In fact, it might even be a control goal to combat system discontinuities in the feedback loop (by using Feedforward for instance). With discontinuities I mean things like hysteresis, physical hard limits (like obstacles or the ground), input clipping, etc.

First off all, have you tried controlling the system without the rule that it stops controlling if the angle is less than 0.5 degrees? The fact that it approaches the desired angle does not mean the velocity of the system approaches 0. If the controller stabilizes the closed-loop and the controller has an integral action, the system should converge to the setpoint.

Next, if you D term is not doing anything most of the time because the sensors have not detected a change, I can advice you to reduce the clock frequency of the controller. This is to prevent the integral term from flooding and pretty much dominating the control signal. If your D only changes every 20 samples, the Integral register has added the current error 20 times before the controller physically measures a change.

Quick question about your code, why do you assign the proportional term as the error divided by the initialangleDiff, whilst you use the error only in both the derivative and the integral terms?

So to return to the main question: Is implementing a delay a good move: No. Implementing a delay will not only increase the response time, but can also cause undamped oscillations or even unstable oscillations. Again, delays are usually something a control engineer must combat with the controller.