refactor drive train code
add debugging test