Working velocity code

Check the math. It works, but it's not pretty.
Working DriveStraight code