Best way to drive straight using a gyro?

Technical Java
1

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

     }

}

2

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.DriveWithJoysticks;
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() {
    [INDENT]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);

[/INDENT]
}

3

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 )**
4

Why did you use 3, .2, and .1 as your PID gains?

5

ProjectZero hasn’t posted here since 4 years ago.

1 Like
6

Note that ProjectZero has not been on Chief Delphi in over a year.

The P, I, and D gains are usually determined through a bit of math and a lot of trial and error once the hardware is running. If you search Chief Delphi on “PID Drivetrain Tuning” you will turn up a few good threads that take you through several schools of how to determine your optimum PID values.

Note that when you add/subtract (or even move) weight to your robot, you will want to adjust (or at least verify) your PID constants.