Added functionality to RDMotor

Basic functionality and testing to motor driver library.