Added servo controls, changed up autonomous a bit

Finished hard coding function library