Have you looked at Tip Jar #9 and 10.
http://www.lvmastery.com/tipjar
They show adding servos and gyros to the typedef and propagating them basically to all modes of operation. The motors are probably the same difference. I need to do the same thing this weekend and am going to try to use Ben's example.