Fix DriveTrain math (and unit tests)