Best way to drive straight using a gyro?

[ProjectZero]

Hi all,

Working on coding our robot to be able to drive straight using a gyroscope. The gyroscope initializes at the beginning of the map, getting it's initial heading. Now, I want the robot to at the press of a button be able to turn back to that orientation. I think I've finally structured the code below correctly. Questions:

1. Am I correct in assuming that when I enable PID in my DriveStraight command by writing enable(); that the robot will start doing what is written in the usePidOutput method?
2. What will the bounds of the "output" variable be? I know it's a finely tuned variable of the error between my gyro heading and my desired heading of 0, but I don't know if I can plug it into my motors or not.

I apologize in advance if there's a topic that answers this question. If that's the case, please just link me.Thanks in advance for your help.



SUBSYSTEM CODE

package edu.wpi.first.berserker.subsystems

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


public class DriveTrain extends PIDSubsystem (

private static final double Kp = 3;
private static final double Ki = .2;
private static final double Kd = .1;

private Robotdrive drive = new RobotDrive(Robotmap.leftMotor, Robotmap.rightmotor);
private Gyro newgyro = new Gyro(Robotmap.gyro);


private DriveTrain() {
super ("DriveTrain", Kp, Ki, Kd);
}

public void initDefaultCommand() {
setDefaultCommand(new DrivewithJoysticks());

{

protected double returnPID(){
return gyro.pidGet();
}

protected void usePIDOutput(double outpout) {
//Not certain what needs to go here.
}
}

COMMAND CODE

package edu.wpi.first.berserker.commands;




public class DriveStraight extends CommandBase (

double setPoint;

public DriveStraight(double setpoint) {

requires(drivetrain);
this.setpoint = setpoint;

}

protected void initialize() {
drivetrain.setSetpoint(setpoint);
drivetrain.enable();

}

protected void execute() {

}

protected boolean isFinished() {
return Math.abs(drivetrain.getPosition() - setpoint <.02);

}

protected void end() {
drivetrain.disable();

}

protected void interrupted() {
drivetrain.disable();

}
}