Programming a Crab Drive System

[AustinSchuh]

I'm going to throw this solution out there, and let you figure out how to apply it to a Crab. It shouldn't be that hard, except that I have hw to do...

What you want to do is minimize the slip velocity squared of your wheels. If you set up a set of equations that describe the slip velocity of your wheels given the wheel angles, velocities, the goal velocity vector and the goal angular velocity, you can optimize the wheel angles and speeds.

I ran through this math for a swerve drive with paired wheels like you drew, but individual motors for each wheel, and when we downloaded and ran it, we were able to translate and rotate with ease. I was able to get out a closed form solution for the correct wheel angles.

Once you do that math, the bot will discover that if you want to turn while going sideways, you want to steer by turning the modules in opposite directions. It'll also figure out that you just skid steer when going forwards. It'll also figure out how to mix the two modes when going at an angle. It's a beauty to watch in action, and has no special cases. In other words, once you do the math, it just works.