balance, motor and testing code added

Added untested code to balance the copter.

Added additional functionality in the motors class.

Added some testing code.