Fix for NPEs

This request has fixes for NPEs which are preventing the auto op feature from working properly in the [VileBot project](https://github.com/oldterns/VileBot). Included in the first commit is a check for legal state that will hopefully prevent any similar errors in the future.