Not working Flight Test Changes

Code from 5/18/2014 that have the flight to be a bit more stable, but it is still off.