We made an omniwheel holonomic drive in 2015 (Grand Blanc East Middle School, The Gear Mashers). I had meant to post this for a while. Here’s the write up on the math involved in tracking its movements on the floor. It uses 2d vectors and requires an encoder for each omniwheel. Make sure to watch the khanacademy links as it explains the vector operations.
Hope you find it useful. Let me know if there are questions or you find errors. Good Luck!
GearMashersNavigation4b.pdf (621 KB)
Thanks for posting this. The “M” in STEM is “mathematics.”
@kjvansic very nice paper!
What is your navigation accuracy after, let say, 15 sec of driving with turning?
Also, do you reset encoders at each step or keep the running totals? I wonder how much roundoff errors contribute to the total error or if you are doing something to cancel them out? Thanks
It was pretty good. We managed to grab a couple skyrise bricks using the encoder data in this way for the autonomous period. We read the encoders then immediately clear them at the beginning of each cycle of the program main loop. They are integers and we did not allow any time for the count to increase before clearing the value. So each time it was a delta. The calculated motion of the bot on the floor was summed in variables at the end of the calculations.
I believe we tracked the position and angle well, but the bot would wander a bit since we just scaled the velocity vector to the motor power. It would seek around a bit at the end of its travel until it arrived at the target destination. The limiting factors are mostly error accumulated by wheel slip.
We had used the real time clock to keep the main loop constant 25 milliseconds per loop. This would be helpful for a PID loop but we never used one. It would be an improvement. We had considered using the range finder to scan the walls and re calibrate our location. We did rotate and read the range sensor to locate bricks, but it was too slow for competition. I believe the same method could be used to find the location and angle of a wall. Research needed 
It is useful to design operations of the bot by using state machines.