[adein]

I use a state system and a Timer object inside of an 'iterable' function. The function returns false until the operation is complete. This works well if you use the 'Iterable' robot template that comes with WindRiver.

The state variable 'state_' must be set to kPhase1 before the function is called the first time (make sure you don't keep setting the state to kPhase1 each time through the control loop). The timer will have a resolution equal to the duration of the calling control loop.

bool TechnoJays::SomeFunction() {
	double time_left = 0.0;
	// Get the timer value since the last event
	double elapsed_time = timer_->Get();

	switch (state_) {
	// Start the timer and move to phase 2
	case kPhase1:
		timer_->Stop();
		timer_->Reset();
		timer_->Start();
		elapsed_time = timer_->Get();
		state_ = kPhase2;
		// Fall through into kPhase2
	// Do nothing until the time has elapsed
	case kPhase2:
		// Calculate time left
		time_left = delay_time_ - elapsed_time;
		// If enough time has passed, continue
		if (time_left <= 0.0) {
			timer_->Stop();
			state_ = kPhase3;
			// Fall through to kPhase3
		} else {
			break;
		}
	// The time delay is finished, continue on
	case kPhase3:
		// Return true indicating that the function is complete
		state_ = kNotRunning;
		return true;
	default:
		state_ = kNotRunning;
		return true;
	}

	return false;
}