Added servo, algorithm code, and docs

-Untested with hardware
-Needs functions to be written