Add diagnostic to corobot_pantilt and corobot_ssc32