Added McGough's optimized equations
Merge branch 'electric-devel' of https://github.com/smd-ros-devel/mecanum_drive into electric-devel