FOD using trig and comments

Using trig for field oriented driving and adding comments to help out
noobs. Maybe PID control next
FOD using just Mech_Function

Really just cleaning up. had to change the orientation of gyro -_-