Added proportional controller.  check_and_set needs to be tested
Minor tweeks.  Need to find funciton for fetching torque setting from motor.
Massive changes to readability and scalability.  Still lots of sign errors.

FIX:
Expectedpos contains no math to normalize result between -180 to 180
Expectedpos MAY need to consider orientation of servo, revisit this