Started fixing servos stuff