West Coast Drive Code

[Nathan Powell]

My team, 2582, is switching to the West Coast drive this year. As the team's head programmer, I'm looking for information on the drive code itself. Can anyone help me learn the 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.

[Ether]

Here's a numerical contour map of WPILib arcade code:

Numerical contour map

[Ether]

Here's pseudocode and sample contour maps for a 2-parameter tunable Arcade algorithm.

One parameter adjusts the strength of the turn at max throttle, the other adjusts the strength of the turn at zero throttle.

The left and right wheel speeds are smoothly interpolated for partial throttle so that the wheel speeds stay in the range -1 to +1 without the need for clipping.

Please note: The tunable algorithm attachment is near the bottom of the list¹:

http://www.chiefdelphi.com/media/papers/2661

¹As of 8/17/2012, that is. As new attachments are added, they are put at the bottom of the list. AFAIK, vBulletin provides no way to rearrange the attachments into a more sensible order

[F22Rapture]

@Tom

How does one determine the "gain?" Trial and error, algorithms, etc?

Would something like this be correct?

// Xbox Controller    
    Joystick Xbox = new Joystick(1);  
           
    // Drivetrain
    public double throttle = Xbox.getRawAxis(3);
    public double turn = applyDeadband(Xbox);
    public double leftMtr = throttle + turn;
    public double rightMtr = throttle + turn;
    //This will need to be tuned
    public double gain = 1;

private double applyDeadband(Joystick Xbox) {
        if(Math.abs(Xbox.getRawAxis(1)) < 0.1) return 0;
        else return Xbox.getRawAxis(1);
    }

 private 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;
}
    
    public double getLeftMotor() {
        return leftMtr + skim(rightMtr);
    }
    
    public double getRightMotor() {
        return rightMtr + skim(leftMtr);
    }

@ Ether what are the benefits of your method vs Tom's? What would that code look like in Java?

[Ether]

Originally Posted by F22Rapture

Would something like this be correct?

...

Run the code and create a contour map. Then carefully study the contour map to see if it's what you want.

@ Ether what are the benefits of your method vs Tom's?

I haven't yet coded up Tom's method and made a contour map, so I can't say for sure. But the method I posted requires no clipping or normalization of the wheel speeds, because they are guaranteed to be within the range -1 to +1 (as long as the tuning constants "a" and "b" are kept in the ranges indicated). The left and right wheel speeds are linearly interpolated all the way out to +/-1 in the Y and X joystick directions.

I have pseudocode that takes that a step further and does throttle-dependent nonlinear interpolation of the wheel speeds, but that seems like overkill. However if there's any interest I'll post that too.

What would that code look like in Java?

I'm not sure what you mean. It would look the same, only using Java syntax and semantics.

[JesseK]

Originally Posted by Tom Bottiglieri

[Lots of Stuff]

Thanks for the plain-english explanation Tom. This is a great explanation of how to take generic C++ and make it speak Robot for brand new programmers.

@Rapture If you want to go the trial & error route:
The simplest way for the driver to determine what gain is best suited for the specific style of driving would be to use a throttle or button setup to adjust it on the fly. That way you don't have to re-compile and re-upload code to make adjustments. Or, better yet, the gain could be adjustable from the driver's station.

[Ether]

OK I can take a hint.

Here's a plain-English explanation for arcade linear interpolation:

http://www.chiefdelphi.com/media/papers/download/3495

[Tom Bottiglieri]

Originally Posted by F22Rapture

How does one determine the "gain?" Trial and error, algorithms, etc?

For the most part trial an error. A lot of it comes down to driver preference and "feel" of the system. You want to be sure that your robot has enough "bite" in to high speed turns while at the same time making sure it doesn't overturn. A bit of time on a field with the gain hooked up to something you can change fast (some buttons, the throttle wheel on a joystick, etc) will work wonders.

For the most part, we tune our turning around 100% throttle. Any time spent out of 100% throttle while traversing the field is time wasted. Any time that requires less than 100% throttle is usually a straight line approach.

@Ether
Nice! We will definitely need to try that out.

[Nathan Powell]

Thanks to everyone who offered advice; this will really help me understand the west coast drive code. Is this all compatible with the Wind River C++ coding program?

[Tom Line]

I've read through this several times. I believe I understand it, but it only seems to be useful in the case where you are using one joystick as the throttle and the other to turn.

Wouldn't using a two joystick setup where you only use the y-axis on both not have the problem all this coding is trying to correct? You can turn at high speeds by feathering one joystick, and turn at lows speeds by slamming the joysticks in opposite directions.

Or am I missing a benefit in the throttle/turn joystick setup?

[Ether]

Originally Posted by Tom Line

I've read through this several times. I believe I understand it, but it only seems to be useful in the case where you are using one joystick as the throttle and the other to turn.

Wouldn't using a two joystick setup where you only use the y-axis on both not have the problem all this coding is trying to correct? You can turn at high speeds by feathering one joystick

The "tank style" JUI (Joystick User Interface) you describe above suffers from this same problem:

Originally Posted by Tom Bottiglieri

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.

[arun4444]

Originally Posted by Ether

Run the code and create a contour map. Then carefully study the contour map to see if it's what you want.

I have pseudocode that takes that a step further and does throttle-dependent nonlinear interpolation of the wheel speeds, but that seems like overkill. However if there's any interest I'll post that too.

Could you post the non-linear throttle-dependent interpolation code also.

cheers
arun

[Ether]

Originally Posted by arun4444

Could you post the non-linear throttle-dependent interpolation code also.

You can achieve a tunable nonlinear response to each axis of your joystick by running it through this calculation¹ before sending it to the linear interpolated arcade.


¹for those interested, the derivation is explained here

[arun4444]

Great, quick question -

would setting

a = 0.25;
b = 0.40;

be similar to setting:

a=0
b=1

and passing the joystick inputs through a cubic?