[tux]

I implemented my plan to throw an exception on every mode change and it is working pretty well.

I wrapped wpilib.Wait

from mode import check_mode

def wait():
    check_mode()
    wpilib.Wait(0.01)

and mode.py looks like this:

MODE_DIS = 0 # Disabled
MODE_AUTO = 1 # Autonomous
MODE_TELE = 2 # Teleoperated
mode = None


class ModeChange(Exception):
    'Exception raised when mode changes.'
    pass


def check_mode():
    'Check for a change in mode. Raise an exception if changed.'

    global mode

    if wpilib.IsDisabled():
        if mode != MODE_DIS:
            mode = MODE_DIS
            raise ModeChange
        return

    if wpilib.IsAutonomous():
        if mode != MODE_AUTO:
            mode = MODE_AUTO
            raise ModeChange

    if wpilib.IsOperatorControl():
        if mode != MODE_TELE:
            mode = MODE_TELE
            raise ModeChange

As long as any looping code calls this custom wait function, as soon as the mode changes an exception will be thrown.

In robot.py I catch the mode change and any other exception that happens to be thrown.

def run():
    while True:

        try:
            if wpilib.IsDisabled():
                print('Running disabled()')
                while wpilib.IsDisabled():
                    disabled()
                    wait()

            elif wpilib.IsAutonomous():
                print('Running autonomous()')
                while wpilib.IsAutonomous() and wpilib.IsEnabled():
                    autonomous()
                    wait()

            elif wpilib.IsOperatorControl():
                print('Running teleop()')
                while wpilib.IsOperatorControl() and wpilib.IsEnabled():
                    teleop()
                    wait()

            wait()

        except ModeChange:
            pass

        except KeyboardInterrupt:
            raise

        except:
            disable()
            print('ERROR')
            import traceback
            traceback.print_exc()

Seems to be working well, but if anyone sees a potential problem, I'd be glad to hear about it.


I like the idea about having an emergency autonomous escape button. With the code I have, it could just be a button that raises an exception.