Fixed normalization and magnitude calculation
replaced deadzone in magnitude calculation
cleaned up mecanum code