servo code modified, added pointers, need to test