The first thing that immediately pops up to me is that you've got some funky stuff going on with your time calculations. Firstly, you're subtracting
start_time from
elapsed_time twice, once in execute() and once in isFinished(). You should drop it out of isFinished(), changing your return statement in isFinished() to just
Code:
return elapsed_time >= 2.0;
The other issue is there, with your comparison.
System.currentTimeMillis() gives values in just that; milliseconds. Your comparison checks to see if 2ms have passed, not 2sec as I'm guessing you were probably going for. You could change your code to be
return elapsed_time >= 2000; if you're going for 2 seconds, or you could drop the use of
System.currentTimeMillis() altogether and just use a WPILib Timer:
Create the Timer as you would any other object,
Code:
Timer timer = new Timer();
In initialize(), you should reset your timer to 0 seconds, and then start it
Code:
timer.reset();
timer.start();
in isFinished, change your code to
Code:
return timer.get() > 2;
**Note that the Timer object works in Seconds, not Milliseconds!
And lastly, to keep things tidy, you can throw a
timer.stop(); into your end() and interrupted() methods if you want.
I'm not entirely sure why your current code isn't doing anything, but these are a couple of things that should be cleaned up to get the debugging started.