Ok here is my first round. I have done a lot so I am not going to tell you everything should work on the first try. I am still working on autonomous and I will update that as soon as possible. I have a lot of it done already. I am also trying to make the code do what I think you are trying to get it to do. I am available for questions.
https://www.dropbox.com/home/2016CompRobotProject
Good Luck,
Tim