West Coast Drive Code

[Tom Bottiglieri]

In terms of software, WCD is no different than any other skid steer setup. Differing velocities of the wheels against the ground on either side of the robot create a torque about the center of the robot which causes it to turn.

So, a naive approach to how to control a WCD robot is something like this. In fact if you boot up a robot in WPILib using a single joystick and arcade drive, this is exactly what you will get.

// Throttle is how fast we want to be moving foward or backward.
// Turn is how fast we want to spin about the center.
left = throttle + turn;
right = throttle - turn;

For the most part, this will work. There are a few issues though. Let's dig in to this with two examples: 0% throttle with 50% turn and 100% throttle with 50% turn. In the first case, you have this:

// 0 throttle, .5 turn
left = 0 + .5 = .5
right = 0 - .5 = -.5
net difference: 1.0

// 1 throttle, .5 turn
left = 1 + .5 = 1.5 = 1.0 (can't go faster than 100%
right = 1 - .5 = .5 
net difference : .5

Obviously the latter case is applying less differential power, and therefore less torque is created. One of the ways to solve this is to take a portion of the lost power from one side (anything above 1 or below -1) and apply it in the opposite direction to the opposite side of the DT.

double skim(double v) {
  // gain determines how much to skim off the top
  if (v > 1.0)
    return -((v - 1.0) * gain);
  else if (v < -1.0)
    return -((v + 1.0) * gain);
  return 0;
}

t_left = throttle + turn;
t_right = throttle - turn;

left = t_left + skim(t_right);
right = t_right + skim(t_left);

Now when we are driving fast, we don't lose our ability apply full turn. Sometimes, this is not enough. If the robot is moving fast enough and the gearing is sufficiently high, the internal resistance of a neutral motor is not enough to create a torque which can power the robot to turn. Some robots are geared high enough such that when you are moving at full speed and apply [0, 1.0] to each motor, the robot will continue to go in a straight line. While this is great if you are on target, getting around obstacles at high speed becomes impossible.

One approach to fix this is to amp up turn so you can turn harder while going fast

turn = turn * 1.5;
...
drive code from above
...

This makes a robot than can turn well at high speeds but its completely uncontrollable at low speeds. So how do you fix that? Easy, simple math!

turn = turn * (another_gain * fabs(throttle));

Now, a full joystick of turn at low speeds will result in low amounts of turn and a ton of it at high speeds. This makes it very easy for drivers to zoom around the field and control their robot. But you may be asking "what happens when the robot is sitting still and the throttle is 0?". The answer is quite obviously: the robot cannot turn. This tends to be an issue. So how do we fix it? Well, this is really up to your preference. On 254, we use a button of the joystick to indicate we are trying to turn in place.

if (!turnButton)
  turn = turn * (another_gain * fabs(throttle));

When the turn button is pressed, the driver has full control of the turn. You could also make turn BIGGER when the button is pressed to make for a more aggressive control feel, but that's up to you.

Another way to solve this would be only apply the throttle dependent turning when throttle has reached a certain threshold. This can get a little weird at the transition point, though.

if (throttle > .5)
    turn = turn * (another_gain * fabs(throttle));

This is basically the algorithm that my team (and a bunch of other teams) have used to much success. It's a bit more simplified than our implementation, but it has a lot of the main ideas.