[Billy_B]

I can't help too much with that code, but if you want a simpler Iterative Robot to play with team 2704 made a custom iterative robot this year, you can get a PDF copy of our code at http://www.firstplusplus.com/samplerobot-code.html

Our iterative robot is in our O2C class, if you just want the code it should be:
The .h

#ifndef O2C_H	// If not Defined
#define O2C_H	// Define
				// Assures that it does not define multiple times.

// This is our field class. It recieves inputs from the field and determines whether the robot should be in
// Autonomous, Teleoperated or test. It also ascertains whether or not an Initialization is needed for the
// Controller that it posesses.
#include "RobotBase.h"
#include "Timer.h"
#include "Watchdog.h"

#include "O2CDEF.h"

#include "O2CCONTROLLER.h"

class O2C : public RobotBase	// O2C is derived from RobotBase
{
public:
	O2C();	// Constructor
	virtual void StartCompetition();	// Is ran once at the start of the competition

private:
	// For the purposes of telling when initialization is needed.
	int autoiterations;
	int teleiterations;
	int testiterations;
	int disablediterations;
	int enablediterations;
	int firsttime;
	double currenttime;
	
	Timer O2CTimer;
	O2CCONTROLLER O2CController;	// O2C HAS a O2CController
	// For the purposes of telling when initialization is needed.
};

#endif //End O2C_H

and the .cpp:

#include "O2C.h"


O2C :: O2C()	// Constructor
{
	autoiterations = 0;
	teleiterations = 0;
	testiterations = 0;
	disablediterations = 0;
	enablediterations = 0;
	firsttime = 0;
	GetWatchdog().Feed();	// Need to feed Watchdog at start.
}

void O2C :: StartCompetition()
{
	O2CTimer.Start();
	while (1)
	{
		currenttime = O2CTimer.Get();
		O2CDatabuffer.Time = currenttime;
		GetWatchdog().Feed();	// Feed watchdog at the start of every iteration
		if (firsttime == 0)
		{
			// Put Robot Initialization here
			O2CController.Init();	// O2CController initialization.
			firsttime = 1;
			O2CTimer.Reset();
			currenttime = O2CTimer.Get();
			O2CDatabuffer.Time = currenttime;
		}
		if (IsEnabled())	// If any mode is enabled.
		{
			if (enablediterations == 0)
			{
				O2CTimer.Reset();
				currenttime = O2CTimer.Get();
				O2CDatabuffer.Time = currenttime;
				enablediterations = 1;
				// Enabled Initialization here
			}
			else
			{
				if (IsAutonomous())	// Autonomous Mode is enabled
				{
					if (autoiterations == 0)
					{
						// Auto Initialization goes here
						O2CController.AutoInit();
						O2CTimer.Reset();
						currenttime = O2CTimer.Get();
						O2CDatabuffer.Time = currenttime;
					}
					else
					{
						// Auto goes here
						O2CController.Autonomous();
					}
					autoiterations = autoiterations + 1; // Don't Touch
				}
				else if (IsOperatorControl()) // Teleoperated Mode is enabled
				{
					if (teleiterations == 0)
					{
						// Teleoperated Initializations
						
					}
					else
					{
						// Teleoperated goes here
						O2CController.Teleoperated();
					}
					teleiterations = teleiterations + 1; // Don't Touch
				}
				else if (IsTest())	// Test mode, for testing specific pieces of code.
				{
					if (testiterations == 0)
					{
						// Test Initialization goes here
						
					}
					else
					{
						// Test goes here
						
					}
					testiterations = testiterations + 1; // Don't Touch
				}
				else
				{
					// THIS SHOULD REALLY NEVER HAPPEN!
					// Only would happen if it was enabled and not in any mode.
					
				}
				// Enabled Stuff goes in here
				enablediterations = enablediterations + 1; // Don't Touch
															// Would also  work as enablediterations++;
			}
		}
		else if (IsDisabled())
		{
			if (disablediterations == 0)
			{
				// Disabled Init goes here.
			}
			else
			{
				//Disabled goes here.
			}
			disablediterations = disablediterations + 1;
		}
		else
		{
			// If you get here there is a problem.
			// It isn't ours though.
		}
		Wait(WAITTIME);
	} 
}

START_ROBOT_CLASS(O2C) // MACRO needed to show where the entry point is.

The stuff inside of the ifs is where the code would go for your robot.

Edit: If you want to learn how to do Object Oriented Coding for FRC and if you have an NXT our website FirstPlusPlus.com has a full tutorial that can teach you how we do Iterative, Event Driven Coding