Servo and Motor Changes

Added new servo control code.
Removed a - sign in motor control code. Motor direction should now be
set in the pragma statements.