Merge branch murphey and master

Branch murphey was tested. Gyro PID needs to be tweaked but other than that the branch is working.