[notmattlythgoe]

Here is an idea I had recently for conditional wait commands. Thoughts?

With just a couple of static imports you could be writing CommandGroups that look like this.

package org.usfirst.frc.team2363.robot.commands;

import static org.usfirst.frc.team2363.robot.Robot.drivetrain;
import static org.usfirst.frc.team2363.robot.commands.ConditionalWaitCommand.Operation.*;
import static org.usfirst.frc.team2363.robot.commands.ConditionalWaitCommand.*;

import edu.wpi.first.wpilibj.command.CommandGroup;

public class DriveToLine extends CommandGroup {
    
    public  DriveToLine() {
    	addSequential(new DriveCommand(0.5));
        addSequential(waitUntil(drivetrain.getDistance(), GREATER_THAN, 50));
        addSequential(new DriveCommand(0));
    }
}

package org.usfirst.frc.team2363.robot.commands;

import edu.wpi.first.wpilibj.command.Command;

public class ConditionalWaitCommand extends Command {
	
	public enum Operation {
		LESS_THAN, LESS_THAN_EQUAL, EQUAL, GREATER_THAN_EQUAL, GREATER_THAN
	}
	
	public interface IProvidesValue {
		double getValue();
	}
	
	public static ConditionalWaitCommand waitUntil(IProvidesValue operand1, Operation operation, double operand2) {
		return new ConditionalWaitCommand(operand1, operation, operand2, true);
	}
	
	public static ConditionalWaitCommand waitWhile(IProvidesValue operand1, Operation operation, double operand2) {
		return new ConditionalWaitCommand(operand1, operation, operand2, false);
	}
	
	private IProvidesValue operand1;
	private Operation operation;
	private double operand2;
	private boolean isUntil;

    private ConditionalWaitCommand(IProvidesValue operand1, Operation operation, double operand2, boolean isUntil) {
        this.operand1 = operand1;
        this.operation = operation;
        this.operand2 = operand2;
        this.isUntil = isUntil;
    }
    
    @Override
	protected void initialize() { }

	@Override
	protected void execute() { }

    protected boolean isFinished() {
    	switch (operation) {
			case EQUAL:
				if (operand1.getValue() == operand2) {
					return isUntil;
				}
			case GREATER_THAN:
				if (operand1.getValue() > operand2) {
					return isUntil;
				}
			case GREATER_THAN_EQUAL:
				if (operand1.getValue() >= operand2) {
					return isUntil;
				}
			case LESS_THAN:
				if (operand1.getValue() < operand2) {
					return isUntil;
				}
			case LESS_THAN_EQUAL:
				if (operand1.getValue() <= operand2) {
					return isUntil;
				}
    	}
        return !isUntil;
    }

	@Override
	protected void end() { }

	@Override
	protected void interrupted() { }
}

package org.usfirst.frc.team2363.robot.subsystems;


import org.usfirst.frc.team2363.robot.commands.ConditionalWaitCommand.IProvidesValue;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.Subsystem;

public class Drivetrain extends Subsystem {
    
    private Distance distance;
    private Encoder encoder = new Encoder(1, 2);

    public void initDefaultCommand() { }
    
    public IProvidesValue getDistance() {
    	if (distance == null) {
    		distance = new Distance();
    	}
    	return distance;
    }
    
    private class Distance implements IProvidesValue {

		@Override
		public double getValue() {
			return encoder.getDistance();
		}
    }
}