[pblankenbaker]

Here is an example of a command which drives the robot for a particular amount of time using the command based framework. When using the command based approach, you never want to "block" (don't use Timer.delay() in your code). Instead you look for timeout conditions.

package com.techhounds.robot.commands.drive;

import com.techhounds.robot.subsystems.DriveSubsystem;
import com.techhounds.robot.subsystems.RobotParts;
import edu.wpi.first.wpilibj.command.Command;

public final class DriveByPower extends Command {
    private final double _LeftPower;
    private final double _RightPower;
    private final double _RunTime;
    private final DriveSubsystem _Drive;
    
    public DriveByPower(double leftPower, double rightPower, double runTime) {
        super("Drive (" + leftPower + ", " + rightPower + ", " + runTime + ")");
        
        // Save settings to apply/use when the command is started and running
        _LeftPower = leftPower;
        _RightPower = rightPower;
        _RunTime = runTime;
        
        // Indicate what subsystems are required
        _Drive = RobotParts.getInstance().getDrive();
        requires(_Drive);
        
        // Allow other commands that need to use the drive system to take control
        setInterruptible(true);
    }
    
    protected void initialize() {
        // Turn on the motors when the command is started
        _Drive.driveTank(_LeftPower, _RightPower);
    }
    
    protected boolean isFinished() {
        // Done once the run time period has been reached
        return (timeSinceInitialized() >= _RunTime);
    }

    protected void execute() {
       // Nothing to do here (motors should still be running)
    }

    protected void end() {
        // Make sure we turn off the motors when done
        _Drive.driveTank(0, 0);
    }

    protected void interrupted() {
        end();
    }
}

NOTE: The code above uses the timeSinceIntialized() method to determine when to turn the motors off (this does not block). This approach also "cleanly" ends the command.

There is a setTimeout(secs) method available to all commands. I would recommend avoiding that option as it interrupts the command (as opposed to letting the command terminate normally as shown in the above example). It will probably yield the same results when running the command by itself, but has caused us issues when grouping commands (command groups seem to prefer commands that run cleanly as opposed to those that are interrupted).

A PID based drive command would be similar, but would:

• Set up and start the PID controller in the initialize() method.
  
• Disable the PID controller and stop the motors in the end() method.
  
• Update the isFinished() method to return true if the time out period was reached OR the PID had reached its set point.

Hope that helps,
Paul