Added mode selector to RC command, added mode function to controller