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;
        // 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);
            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);
  • 4
    $\begingroup$ Its not really a pid controller if it quits $\endgroup$
    – joojaa
    Jan 10, 2021 at 16:18
  • 2
    $\begingroup$ Why shut off the control? Step 1: stop exiting control loop. Step 2: tune PID. Step 3: Decide you never needed exit control loop at all. $\endgroup$
    – Supa Nova
    Oct 8, 2021 at 0:19
  • $\begingroup$ "I know that this is the entire purpose of PID control, to accurately reach the set point without overshooting." I do not agree with this. Overshoot is in many cases perfectly acceptable behavior in a PID and may even be necessary to obtain a required rise-time. $\endgroup$
    – Chris_abc
    Jan 31, 2023 at 9:11

1 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.

  • $\begingroup$ I will try a lower threshold than .5 degrees and will get back to you, thanks for the response. However, what do you mean by "reduce the clock frequency"? To answer your questions, the proportional term is divided just so that it represents the actual proportion of the turn completed. It shouldn't make a difference overall if I am not mistaken, just means that the value of kp will be different. $\endgroup$
    – droiddoes9
    Jan 10, 2021 at 21:57
  • $\begingroup$ your PID controller is currently able to compute a new control input $N$ times per second. This is currently limited by the processor it runs on and the amount of instructions this processor has to perform. Try implementing a manual time-delay between each time the new control input is computed (such that it can only do $0.5N$ cycles each second). With this, you reduce the clock frequency $\endgroup$
    – Petrus1904
    Jan 11, 2021 at 16:24
  • 1
    $\begingroup$ These answers are conflicting. "Try implementing a manual time-delay between each time the new control input is computed" and "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" are exact opposites. One says to implement a delay and the other says not to. $\endgroup$
    – droiddoes9
    Jan 11, 2021 at 22:02
  • $\begingroup$ the only time it doesn't go past the setpoint is with a tiny, tiny angle, such as .00125 degrees (the angle the robot stops at, used to be .5). I think that this will be unachievable accuracy in the context of a 100+ degree turn, so I don't think that is a reasonable solution. $\endgroup$
    – droiddoes9
    Jan 11, 2021 at 22:16
  • $\begingroup$ Hahaha yes, I figured that could be oddly confusing. Delays are usually a principle where the computed control input at time instance $t$ is only applied at instance $t+T_s$. meaning the controller lags behind. reducing the clock cycle is fine however, as long as the time it takes between the new control input and the measured position is minimal.. Accuracy is usually bound by the hardware, not the controller. Yes, without feedforward or an integral action, the system might not perfectly approach the setpoint, but it should not drift away over time. $\endgroup$
    – Petrus1904
    Jan 11, 2021 at 23:21

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.