Timing in Simple Robot

[raptorf22]

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

PHP Code:

#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\r\n",m_telePeriodicLoops / 200);
        }
        
        //Reset Timer
        if(stick.GetRawButton(2)){
            m_telePeriodicLoops = 0;
        }
        
        drive.ArcadeDrive(stick);
    }
};

START_ROBOT_CLASS(CustomRobot); 


Any ideas? Best practices?