PD Loop to make robot go straighter

VEX V5 General Discussion
1

Hi, so currently our robot is using a P(roportional) controller to keep encoder values on each side of our drive as equal as possible. While it is mostly straight, I want it to be even more straight and consistent. I want to add the D(erivative) part to it. The way my current code works right now is that the motors accelerate to a value, and then the P controller adjusts the speeds based on error of the encoders on each side of the drive. Kind of like this:

void forward( target value )
{
while( encoders < target value)
{

error = leftencoder - rightencoder;

syncspeed = error*kP;

motor[left] = 80 - syncspeed
motor[right] = 80 + syncspeed
}

motor[left] = 0;
motor[right] = 0;
}

To add the D part, would I simply add the value of D to syncspeed?
Like this:

void forward( target value )
{
while( encoders < target value)
{

error = leftencoder - rightencoder;
Derivative = error - prevError;
prevError = error;

syncspeed = errorkP + DerivativekD;

motor[left] = 80 - syncspeed
motor[right] = 80 + syncspeed
}

motor[left] = 0;
motor[right] = 0;
}

2

Before you try making your gains more aggressive, try making one of the sides follow the other one, instead of trying to have both match each other. I find that doing this makes this situation a lot easier to manage and you can get smoother results without too much tuning.

So instead of

motor[left] = 80 - syncspeed
motor[right] = 80 + syncspeed

try just


motor[right] = 80 + syncspeed
3

But if I were to add the “D” part of the loop then I would just add it to syncspeed?

4

I have reasons to think that compensating both sides is a better solution:

  • compensating only one side is, in effect, not much different from halving the kP
  • compensating both sides is keeping the average robot speed constant (ignoring the motor non-linearity), thus no unwanted acceleration/deceleration caused by the compensation.

Now, for going straight, my two programmers implemented two strategies, but both used gyro. When taking higher-level view of their code, one was effectively a P-only controller, while the other was a kind of I-only controller. We evaluated them based on the behavior for different use cases:

  • Uneven friction: I-only is what you want - integral term is what would allow your algorithm to keep running the friction-handicapped side constantly with higher power, even when the robot is already going perfectly straight.
  • Bumping into obstacles: P-only is good, since the measured error is not caused by some inherent, constantly present issue.
    Given the wide drivebase and the cone alleys, they ended up running with P-only, solving the friction issue by aggresively high kP. It has proven great approach, since their auton is often able to quickly autocorrect even after being hit by an opponent kamikaze.
    Also, the encoder based approach is prone to accumulating wheel slip errors, and they do happen when bumping into the cones and much more on collisions. Not that gyro likes collisions much better though…
    (Edit: typo)
5

I don’t think this is quite true. By compensating only one side, you also remove acceleration forces from the side that stays constant, so instead of having both sides oscillate to try and match each other, only one side has to potentially oscillate. Your gains would have to increase but if you increase it to match what it was with both sides attempting to compensate, you should get smoother motion.

The average robot speed might be constant, but its heading isn’t correct. If compensating one side increases stability and reduces steady-state error then total error of a move will likely be less than the alternative.

@Joshua_84927A your implementation looks correct for adding a derivative gain to the loop. Make sure your loop is running at a constant frequency though, otherwise your rate of change calculation won’t be accurate. I usually like to calculate the time since last read/loop using nPgmTime instead of just adding a wait and hoping that the frequency of my program is constant and doing something like this:


int lastTime = nPgmTime;

while (encoders < target) {
.
.
.

float deltaTime = (nPgmTime - lastTime) / 1000.0;
lastTime = nPgmTime;

if (deltaTime == 0)
  Derivative = 0;
else
  Derivative = (error - prevError) / deltaTime;
.
.
.

delay(1);
}