I have written working code for driving straight with the gyro in teleop. This works by holding a button and you can drive straight in the direction that you are facing when the button is pressed.
Code:
@Override
public void operatorControl() {
drive.setSafetyEnabled(true);
gyro.reset();
drive.setMaxOutput(0.75);
int reset = 0;
while (isOperatorControl() && isEnabled()) {
angle = gyro.getAngle() * kP;
/*
* If the button is pressed (and held), drive straight based on the
* angle the robot was facing when the button was initially pressed.
*/
if (logitech.getRawButton(6)) {
if (reset < 1) {
gyro.reset();
reset++;
System.out.println("Success! Gyro was reset.");
}
if (Math.abs(logitech.getRawAxis(1)) > 0.15) {
drive.mecanumDrive_Cartesian(logitech.getRawAxis(1), 0, angle, 0);
} else if (Math.abs(logitech.getRawAxis(0)) > 0.15) {
drive.mecanumDrive_Cartesian(0, logitech.getRawAxis(0), angle, 0);
}
/*
* If the button is not pressed, drive normally.
*/
} else {
reset = 0;
if (Math.abs(logitech.getRawAxis(1)) > 0.15 || Math.abs(logitech.getRawAxis(0)) > 0.15
|| Math.abs(logitech.getRawAxis(2)) > 0.15) {
drive.mecanumDrive_Cartesian(logitech.getRawAxis(1), logitech.getRawAxis(0),
-logitech.getRawAxis(2) * 0.5, 0);
}
}
This is using mecanum drive, but you could edit it to work with your drive train.