Please explain the logic behind this line and what it’s doing. Specifically, I want to understand the PID constants (28 and 40) and what their role is in this equation

I moved your post to the Robots section of the forum since it is about the 3pi+.

As mentioned in the comments of the code, 28 is the proportional coefficient and 1/40 is the derivative coefficient. The proportional term essentially amplifies the error (e.g. a large error will result in an even larger control output). The derivative term considers the rate of change of the error and can reduce or eliminate overshoot (e.g. when the target is approaching the goal and the error is getting smaller and smaller, it will reduce the control output to dampen the response).

If you’re new to PID control systems, you might consider reading through the PID Wikipedia for a more elaborate explanation. There are also a lot of good YouTube videos, such as this two-part series by Brian Douglas.

In this case, the error is how much the robot deviates from the current turn angle, yes. The values for the P and D coefficients were found empirically through trial and error.

Fascinating. Thank you for sharing the PID stuff. Your explanation simplified this entire concept for me.

What would be the role of kI (integral constant) in the turning angle example? I know how to get the integral (sum of errors), but how would having a kI come into play?

The Integral term is for correcting steady state error. So, for example if you tried to turn to a specific angle and it always ends a couple degrees shy, you could add an I term that would build up over time, causing it to eventually correct itself.

In the case you’re describing, the steady state error would be 0, so the Integral term would not be helpful. The scenario where it would be useful is when the PD algorithm is not enough to overcome the forces when the error is small. For example, lets say there was a lot of friction to turn the robot (e.g. a tracked robot or some large high-traction wheels); in that case, the algorithm could settle into steady state after only turning 89 degrees and the output of the control algorithm is too small to overcome the friction of turning that final amount. In that case, adding an Integral term would cause the output of the algorithm to continue increasing until it is able to overcome the friction.

Huh fascinating. In the scenario of a robot with high traction wheels, there is a steady state error present on each turn. Like you said, each turn could be 80 degrees, making the error 1 degree. How would my robot detect this 1 degree error? The robot’s IMU thinks that the robot has turned 90 degrees, so how would a steady state error be detected? I guess that’s what my question is.

If your robot has not reached the goal position, your feedback data should represent that, which creates a measurable error. The situation I was describing occurs when the output of your control algorithm with a small error is not high enough to actually correct for the error.

If the robot finishes turning, but hasn’t reached the target, and is reporting 0 error, then there is no way to correct for it without some way of detecting that there is a discrepancy (i.e. additional feedback). In a case like that, I would suspect either your feedback data doesn’t have a high enough resolution to get the results you want or maybe there’s some kind of hysteresis effect.

Yeah, I notice that my PID algorithm given in the code above is not enough to make the robot turn at slow speeds. Therefore, my robot turns to around 89 degrees (according to the gyroscope) and stops. I’m learning that using an Integral term in my PID loop would mitigate this.

I know that the integral is an accumulation of error. So each time my PID loop runs, it adds up the error (90-x degrees)? Doesn’t that mean my integral term is going to end up as some really large number?

It’s either that OR - my robot’s error was 1 degree (90-89) when the robot stopped moving (turn speed became 0). Does this mean that 1 degree is the integral? So on the next turn, the robot should turn 1 more degree that it previously did? (In order to correct the error)?

Your first interpretation is more accurate. The integral value essentially records the history of your robot’s motion; in practice, you can usually use a sum of all of the error values measured since the robot started running (which then gets multiplied by your Integral coefficient). The Integral coefficient is typically a pretty small value, so it generally isn’t a dominating factor compared to the Proportional and Derivative terms. Also, note that when the error is negative, adding it to the running total will pull it back toward zero. Even considering those factors, it is sometimes useful to set a limit for the Integral term to prevent it from going too high, or reset it to 0 when certain conditions are met.