Atm our bot is using a pid loop that seems to work fine outside of the competition template. However, when we put it into the comp template it drives forward to the requested distance, then suddenly drives backwards forever. Here is the function.
@Entropy170 the reason it drives back is, probably, because you overshoot and when you exit the loop the motors are still set to negative direction (that would be necessary to drive back a little).
The simplest solution is to set all motor power to zero explicitly after you break out of the loop.
Also, if you don’t drive very fast (i.e. all turbos on 4" wheels or faster) you, probably, don’t need I and D components. Simple P+C controller should be enough, where C is a constant base power - the motor power that is slightly less that what you would need to move your robot.
For example, when we need to program our robot for slow moving autonomous we would do
int C = 15;
int maxCycles = 5 * 1000 / 20; // 5 sec
int errorL = targetPosition - SensorValue(left);
int errorR = targetPosition - SensorValue(right);
if( abs(errorL) < 5 && abs(errorR) < 5 ) break;
motor[LL] = errorL * kP + C * sign(errorL);
motor[RR] = errorR * kP + C * sign(errorR);
“D” component is usually very noisy with small intervals (20 msec) you would need to measure it over longer period of time as you get closer to your destination.
“I” component would be necessary to estimate if you are carrying variable load (or no load). If you don’t have such uncertainty, just pick good value for C instead. For example, pick 15 if your robot would always start moving at 19 but will not move at 16 even with fresh battery.