Field oriented holonomic code?

[Jared Russell]

I assume by this you mean a holonomic drive where forwards on the stick means that your robot moves forwards no matter what the bot's orientation.

The first thing you need is normal holonomic code. You probably want it to be closed-loop (i.e. velocity feedback control). There are plenty of other threads on that here.

The next thing you need is a gyro to get the robot heading. Kevin Watson's gyro code is well written, and a good jumping off point.

Note that since the gyro measures angular velocity - not displacement - you must integrate its output (i.e. sum the velocities and divide by time) to get your orientation. This is the biggest hurdle for true orientation-independent holonomic code: your gyro will drift by a little bit, especially once you start bumping into things. I've seen other teams with omni/mecanum drives give the driver a button to reset the gyro heading to 0 when the drift gets bad enough.

You could try to use an electric compass as well, but these are more expensive, slower to respond, and shouldn't be used near magnets - like electric motor.

Once you have your robot heading, things are easy. Get out a piece of paper and figure out the trig - take the forward (in the robot coordinate system) and left/right translation (in the robot c.s.) commands, and multiply them by the sine and cosine of the heading angle (which one goes to which depends on which direction the gyro thinks is positive). Leave the yaw command untouched. This correctly moves your robot in a straight path regardless of orientation.

Like I said, be mindful of gyro drift, and you will most likely need a tight feedback loop on the drive motors, but it has and can be done.