Timing in Simple Robot

How can I create timed loops using the SimpleRobot? The teleop code in SimpleRobot seems to run as fast as it can, while the IterativeRobot is throttled at 200Hz.

Right now we have tried something like this, but would rather make this work with the SimpleRobot


#include "WPILib.h"

class CustomRobot : public IterativeRobot
{
	RobotDrive drive; // robot drive system
	Joystick stick; // only joystick
	
	// Local variables to count the number of periodic loops performed
	UINT32 m_telePeriodicLoops;

public:
	CustomRobot(void): drive(1,3,2,4), stick(1)
	{
		GetWatchdog().SetExpiration(100);
		
		// Initialize counters to record the number of loops completed in autonomous and teleop modes
		m_telePeriodicLoops = 0;
	}

	void TeleopPeriodic(void){
		GetWatchdog().Feed();
		m_telePeriodicLoops++;
		
		//Lets check time over a few minutes
		if(m_telePeriodicLoops % 200 == 0){
			printf("Seconds: %d
",m_telePeriodicLoops / 200);
		}
		
		//Reset Timer
		if(stick.GetRawButton(2)){
			m_telePeriodicLoops = 0;
		}
		
		drive.ArcadeDrive(stick);
	}
};

START_ROBOT_CLASS(CustomRobot);


Any ideas? Best practices?

You can use the Timer class included in WPILib to get the number of seconds, returned as a double, since you called the Start() function of the class.