Code for a ZigBee router controlling a servo