more changes and position equations
Merge branch 'odometry' of https://github.com/rmmccann/CS503-Robot-Controller into odometry
Uploading Haohan's code