[Joe Derrick]

This is our auto drive straight command for our robot this year.
We are using command based programming and java.
We used the RobotBuilder "PIDCommand" as a template that we modified to what you see below.

It drives forward at a set speed and maintains an angle using values from encoders on each side of the drivetrain.
We have removed some parts of the code as they are specific to our robot.
Hopefully this is of use to you.

public class AutoDriveStraight extends PIDCommand {

double driveSpeed = 0;
public AutoDriveStraight(double speed) {

super("AutoDriveStraight", 0.25, 0.0, 0.0, 0.02);

//we set our P to be 0.25 but this will need tuning.

getPIDController().setContinuous(false);
getPIDController().setAbsoluteTolerance(1.0);
driveSpeed = speed;


requires(Robot.driveTrain);

}
protected double returnPIDInput() {

return Robot.driveTrain.getDegreesFromEncoderValues();

//This is a method that uses Encoders and a formula
//to calculate a degree value. You could use the
//difference between the drivetrain encoders.

}
protected void usePIDOutput(double output) {

Robot.driveTrain.arcadeDrive(driveSpeed, output);

//This is a method within our Drivetrain subsystem.
//The driveSpeed is the constant, and the
//PIDController maintains the Setpoint set below.

}

protected void initialize() {



this.setSetpoint(0);

// This maintains the previously calculated
//degrees from the PIDInput at 0. ie: "Straight ahead"

this.getPIDController().enable();

}

protected void execute() {
}

protected boolean isFinished() {

return //a boolean that tells when you want your command to finish

}

protected void end() {
}

protected void interrupted() {
}
}