We have a command which runs during auto that just simply moves backward or forward, based on the FPGA Timestamp. This is pretty much our contingency command for auto, incase the limelight fails for whatever reason. This command does its job well, however the command never really ends, so when we schedule other commands after it, those commands won’t run.
Here is the code for the Command
public class AutoMovement extends Command {
private DriveTrain driveTrain;
private double startTime;
public AutoMovement(DriveTrain driveTrain) {
this.driveTrain = driveTrain;
addRequirements(driveTrain);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
startTime = Timer.getFPGATimestamp();
}
@Override
public void execute() {
double time = Timer.getFPGATimestamp();
System.out.println(time - startTime);
if (time - startTime < 1.5) {
driveTrain.driveForward(.5);
}
else if(time - startTime > 3 && time - startTime <6 ) {
driveTrain.driveToSpeaker();
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
driveTrain.stopDriveTrain();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
our auto is a sequence of commands structured like this (its a bit messy):
public Command getAutonomousCommand() {
return new SpinUpShooter(shooter).withTimeout(3)
.andThen(new AutoShoot(shooter, feeder).withTimeout(1.5))
.andThen(new TimeBasedBackUp(sparkDriveTrain)
.alongWith(new autoRunIntake(intake)
.alongWith(new ContainNoteAuto(feeder))
.until(
() -> intake.getNoteSwitch() == false
)
)
);
}
we tried using .withTimeout() other command compositions but they usually end up bugging out.