added fail reply if emergency stop is active and motor or servo gets switched on
added fail reply if emergency stop is active and motor or servo gets switched on

PRIArobotics/HedgehogLightFirmware,PRIArobotics/HedgehogLightFirmware