[BradAMiller]

Originally Posted by TubaMorg

That is pretty slick. My only concern was/is that the code would be stuck in Initialize() in the while(1) loop, but from what you said this isn't true? So say we start the robot up normally for testing with our dongle set to enable and manual (OperatorControl). Initialize is called first, but how does it get out of the while loop and continue on to OperatorControl? Similarly, during competition, how does the program know when Initialize is done if it's always in the while loop in Initialize()?

You are correct... I don't know what I was thinking.

How's this:

// code sample from EasyC (or WPILib)
void Initialize ( void )
{
      unsigned char mode = 0; 

      while ( !IsEnabled() )
      {
            while ( GetOIDInput(1,1) == 0 )     // wait for the trigger
            {
            }
            mode++ ;                            // increment mode
            while ( GetOIDInput(1,1) == 1 )     // wait for trigger release
            {
            }
            mode %= 5 ;                         // make sure mode < 5
            SetUserDisplay ( mode ) ;           // put mode in LED for feedback
      }
}

Thanks for pointing out the error!

The only other thing with this method is making sure that the field doesn't become enabled before the operator is finished incrementing the mode to the right value. Otherwise, autonomous will start with some intermediate setting.