Autonmouos - add code to handle when the path planner can't make a path

[gerth2]

Follow up on the TODO in the autonomous code to flash the "I can't auto" light at the driver if the path planner returns a zero length path (ie there is no solution given the current orientation of the robot relative to the target).

[gerth2]

Expanded scope - rework the autonomuos class into a more formal state machine as drawn out on that piece of paper Jack has.

Here's the contents of a generic state machine:

package frc.robot;

import frc.lib.DataServer.Signal;

/*
 *******************************************************************************************
 * Copyright (C) 2019 FRC Team 1736 Robot Casserole - www.robotcasserole.org
 *******************************************************************************************
 *
 * This software is released under the MIT Licence - see the license.txt
 *  file in the root of this repo.
 *
 * Non-legally-binding statement from Team 1736:
 *  Thank you for taking the time to read through our software! We hope you
 *    find it educational and informative! 
 *  Please feel free to snag our software for your own use in whatever project
 *    you have going on right now! We'd love to be able to help out! Shoot us 
 *    any questions you may have, all our contact info should be on our website
 *    (listed above).
 *  If you happen to end up using our software to make money, that is wonderful!
 *    Robot Casserole is always looking for more sponsors, so we'd be very appreciative
 *    if you would consider donating to our club to help further STEM education.
 */

public class MyStateMachine {

    /* All possible states for the state machine */
    public enum StateEnum {
        StateA(0),   /* A description for state A */
        StateB(1),   /* A description for state B */
        StateC(2);   /* A description for state C */

        public final int value;

        private StateEnum(int value) {
            this.value = value;
        }

        public int toInt() {
            return this.value;
        }
    }

    StateEnum curState;
    StateEnum prevState;

    //Define the state you should start in
    final StateEnum INITAL_STATE = StateEnum.StateA; 

    public MyStateMachine(){
        curState = INITAL_STATE;
        prevState = INITAL_STATE;
    }

    public void update(){
        //Main update loop
        StateEnum nextState = curState;

        //Step 0 - save previous state
        prevState = curState;

        //Do different things depending on what state you are in
        switch(curState){
            case StateA:
                //Step 1 - Read inputs relevant to this state
                //TODO - call methods to read inputs as needed

                //Step 2 - Set outputs for the current state
                //TODO - perform actions and set outputs as needed

                //Step 3 - Detremine if we need to transition to a different state, and set nextState to that one.
                nextState = StateEnum.StateA; //TODO - create logic to populate nextState with a reasonable value.
            break;

            case StateB:
                //Step 1 - Read inputs relevant to this state
                //TODO - call methods to read inputs as needed

                //Step 2 - Set outputs for the current state
                //TODO - perform actions and set outputs as needed

                //Step 3 - Detremine if we need to transition to a different state, and set nextState to that one.
                nextState = StateEnum.StateA; //TODO - create logic to populate nextState with a reasonable value.
            break;

            case StateC:
                //Step 1 - Read inputs relevant to this state
                //TODO - call methods to read inputs as needed

                //Step 2 - Set outputs for the current state
                //TODO - perform actions and set outputs as needed

                //Step 3 - Detremine if we need to transition to a different state, and set nextState to that one.
                nextState = StateEnum.StateA; //TODO - create logic to populate nextState with a reasonable value.
            break;

            default:
                System.out.println("ERROR: unhandled CurState. Tell SW team they wrote bad code!");
                nextState = INITAL_STATE;
            break;
        }

        curState = nextState;
    }

    public StateEnum getState(){
        return curState;
    }

}

[gerth2]

Looks good enough to me for now! Will try more later.