Added servo functionality -- will test when I get home