finish SwerveDrive
port over a lot of the motor code