Self diagnostic of control system, servo system