PIDSubsystem works once then disables the entire RobotDrive

[pblankenbaker]

It looks to me like your DriveToDistance command will only work one time the way you have it coded. The "go" flag was being set to "false" in the original end() method in the original implementation. Once the flag is set to false, then your drive train PID was never enabled on any subsequent invocation (until you power cycle).

Can you try refactoring it without the "go" flag to something like:

package us.mn.k12.fairmont.robotics.commands;

public class DriveToDistance extends CommandBase {

    double setpoint;

    /**
     * Require the drive train and store the desired setpoint.
     *
     * @param setpoint The desired setpoint for the drive train.
     */
    public DriveToDistance(double setpoint) {
        requires(drivetrain);
        this.setpoint = setpoint;
    }

    // Called just before this Command runs the first time
    /**
     * Set the setpoint to the stored value and enable PID on the drivetrain.
     */
    protected void initialize() {
        System.out.println(this);
        drivetrain.enable();
        drivetrain.setSetpoint(setpoint);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    /**
     * @return true when it's close enough to the setpoint
     */
    protected boolean isFinished() {
        return drivetrain.onTarget();
    }

    // Called once after isFinished returns true
    /**
     * When this command ends, disable the drivetrain's PID
     */
    protected void end() {
        System.out.println("done");
        drivetrain.disable();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    /**
     * When this command exits, disable the drivetrain's PID
     */
    protected void interrupted() {
        end();
    }
}

You may also want to add a time out parameter to your command. For example, if you know it should always take less than 5 seconds for the command to complete, set the timeout to 5 seconds to make sure you get back manual control should something go wrong in reaching the set point (like a broken encoder).