Here is an example of setting/tracking a start time as you transition through your fire sequence. It is broken out into one private attribute (time the sequence was started) and three helper methods.
NOTE: This assumes you are programming using the SimpleRobot framework.
Code:
// This will be set to the time when the fire sequence was started. It
// will be reset to 0 when the fire sequence completes
private double fireSequenceStart = 0.0;
/**
* Helper method to indicate whether we are currently firing.
*/
public boolean isFiring() {
// When the fire start time is set to a non-zero value, then
// we are firing
return (fireSequenceStart != 0);
}
/**
* Helper method to start the fire sequence (call this after a certain
* button has been pressed during operatorControl(), or call it from
* autonomous() if you want to fire then).
*/
public void fire() {
// Don't start a second fire sequence until the first completes
if (isFiring() == false) {
// Save time the fire sequence was started at
fireSequenceStart = Timer.getFPGATimestamp();
// Not sure if you want this, but you may want to clear your
// internal "isFired" flag here if you don't clear it elsewhere
// isFired = false;
}
}
/**
* Repeatedly call this method from your main loop in operatorControl() or
* autonomous(). It should take you through your fire sequence without
* blocking (requires no Timer.delay() calls).
*
* ***WARNING*** Make sure your other code does not block or have delays in
* it, or this timing sequence may fail!
*/
public void updateFireState() {
if (isFiring() == true) {
// Determine number of seconds into fire sequence
double secsElapsed = Timer.getFPGATimestamp() - fireSequenceStart;
// This is an attempt to refactor your original Fire() code
// to change states based on the time elapsed (but with no Timer.delay() calls)
if (secsElapsed < 0.25) {
// State for first quarter second (0.0 to 0.25)
IntakeDown.set(false);
IntakeUp.set(true);
isIntakeUp = true;
} else if (secsElapsed < 1.25) {
// State for the next second (0.25 to 1.0)
shooterLatch.set(false);
shooterUnlatch.set(true);
// Hmmm, shouldn't isIntakeUp be set to false here?
} else {
// Finished (1.25 seconds have elapsed)
// Reset start time to 0 (indicates we are done with the fire sequence)
fireSequenceStart = 0;
isLatched = false;
isBottom = false;
isReset = false;
isFired = true;
}
}
}
Hope that helps.