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();

}
}

[ProjectZero]

This is the code as updated at our last meeting before our regional. We'd really like to get an idea of how well it'd work before we get there:

package edu.wpi.first.ReboundRumble.subsystems;

mport edu.wpi.first.ReboundRumble.OI;
import edu.wpi.first.ReboundRumble.RobotMap;
import edu.wpi.first.ReboundRumble.commands.DriveWithJoys ticks;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.ReboundRumble.commands.CommandBase;


/**
*
* @author Developer
*/
public class DriveSystem extends PIDSubsystem {

private static final double Kp = 3;
private static final double Ki = 0.2;
private static final double Kd = 0.1;


RobotDrive drive= new RobotDrive(RobotMap.LeftDrive, RobotMap.RightDrive);
Gyro gyro = new Gyro(RobotMap.gyroPort);


// Initialize your subsystem here
public DriveSystem() {
super("DriveSystem", Kp, Ki, Kd);
setSetpoint(0);
// Use these to get going:
// setSetpoint() - Sets where the PID controller should move the system
// to
// enable() - Enables the PID controller.
}

public void initDefaultCommand() {
// Set the default command for a subsystem here.
//setDefaultCommand(new MySpecialCommand());
setDefaultCommand(new DriveWithJoysticks());
}

protected double returnPIDInput() {
// Return your input value for the PID loop
// e.g. a sensor, like a potentiometer:
// yourPot.getAverageVoltage() / kYourMaxVoltage;
return gyro.getAngle();
}

protected void usePIDOutput(double output) {
// Use output to drive your system, like a motor
// e.g. yourMotor.set(output);
if (gyro.getAngle() > 1) {
DriveReg(0, output);
}

if (gyro.getAngle() < -1){
DriveReg(output, 0);
}
drive.tankDrive(.5,.5);

}

[Ether]

...
protected double returnPIDInput() {
// Return your input value for the PID loop
// e.g. a sensor, like a potentiometer:
// yourPot.getAverageVoltage() / kYourMaxVoltage;
return gyro.getAngle();
}
...
protected void usePIDOutput(double output) {

// Use output to drive your system, like a motor
// e.g. yourMotor.set(output);
if (gyro.getAngle() > 1)


Not like a potentiometer. See bolded portions below:

/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the oversampling rate, the
* gyro type and the A/D calibration values.
* The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
* want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
*
* @return the current heading of the robot in degrees. This heading is based on integration
* of the returned rate from the gyro.
*/
float Gyro::GetAngle( void )