Need to tweak a bit more. AH-509

Working 90% of the time. Still need to tweak so going up stairs isn't a problem. Also need to account for high speed somehow.