Added preliminary code to check for huge changes in heading. Switched drive down ramp code to rps turns

Signed-off-by: Aaron Maharry <aaronmaharry@gmail.com>