Working THIGH code 

Currently working version of the THIGH robot.