fixes static variable bugs, and adds servo commands