Made Steps Towards Encoder Implementation

Tried making the encoder implementation work, but had no luck with the
encoder actually reading during the motor moving.  I played around with
motor control.cpp and .h a lot, so if this is a catastrophic move,
simply revert this commit.  Also some organizational changes are
included.