added servotest and changed things