Improved offset and calculations.
Motor testing code. It works!