Moved servo movement functions to Kinematics.h