The default TeleopContinuous() shipped in 2011 WPILib code:
/**
* Continuous code for teleop mode should go here.
*
* Users should override this method for code which will be called repeatedly as frequently
* as possible while the robot is in teleop mode.
*/
void IterativeRobot::TeleopContinuous()
{
static bool firstRun = true;
if (firstRun)
{
printf("Default %s() method... Overload me!\n", __FUNCTION__);
firstRun = false;
}
taskDelay(1);
}
The taskDelay(1) allows your task to give up the processor for a very short period of time every loop instead of staying in a tight loop and using 100% of the processor until TeleopPeriodic() is run.
This might be related to the performance issue you are seeing.
Here is the "loop" I'm referring to...
void IterativeRobot::StartCompetition()
{
// first and one-time initialization
RobotInit();
// loop forever, calling the appropriate mode-dependent function
while (TRUE)
{
// Call the appropriate function depending upon the current robot mode
if (IsDisabled())
{
// call DisabledInit() if we are now just entering disabled mode from
// either a different mode or from power-on
if(!m_disabledInitialized)
{
DisabledInit();
m_disabledInitialized = true;
// reset the initialization flags for the other modes
m_autonomousInitialized = false;
m_teleopInitialized = false;
}
if (NextPeriodReady())
{
DisabledPeriodic();
}
DisabledContinuous();
}
else if (IsAutonomous())
{
// call AutonomousInit() if we are now just entering autonomous mode from
// either a different mode or from power-on
if(!m_autonomousInitialized)
{
AutonomousInit();
m_autonomousInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_teleopInitialized = false;
}
if (NextPeriodReady())
{
AutonomousPeriodic();
}
AutonomousContinuous();
}
else
{
// call TeleopInit() if we are now just entering teleop mode from
// either a different mode or from power-on
if(!m_teleopInitialized)
{
TeleopInit();
m_teleopInitialized = true;
// reset the initialization flags for the other modes
m_disabledInitialized = false;
m_autonomousInitialized = false;
}
if (NextPeriodReady())
{
TeleopPeriodic();
}
TeleopContinuous();
}
}
}
Your code eliminated the default taskdelay() which ends up causing wasted CPU processing. The system should run smoother if you leave some time for valuable processing (in other tasks).