Added encoder and motor control test code