Implemented correct behavior for arm/safe and enable/disable servo