Our head mentor uploaded some new code to get and it seemed to work. However the controls are off. See the video.
https://www.youtube.com/watch?v=uDY6QJa7FKw Thanks everyone for there help and especially ozrien for putting the code on your robot. Tomorrow I'll post how he was able to fix it but for now sleep and class.