Turning autonomously

I’m trying to make turning in autonomous accurate and consistent. I’ve tried a PID (P and PD) solution and had little success. I’ve tried simple while loops but they overshoot (and with compensation they end up completely inconsistent). What kind of algorithm / way of doing turning is the most consistent and successful? In the WPIlib, would RobotDrive.drive() work better than RobotDrive.arcadeDrive()? If so how would I make it turn more on a dime than it does by default?

Just generally, what’s the best way to turn consistently and accurately turn to any angle?

*Using a gyro and 6WD
**Assuming we are only turning and not going forwards

Here at 192, we’ve found that the most consistent way to turn is with PID control on a gyro.

Basically our turn-on-a-dime is something like this:

Here is our code for turning (we created our own custom Macro class that serves to execute a single piece of code. It’s good for creating modular controllers like in autonomous mode where all action can be broken down into simple actions):


package macro;

import core.GRTMacro;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import mechanism.GRTDriveTrain;
import sensor.GRTGyro;

/**
 * Macro that automatically turns the robot a certain angle.
 *
 */
public class MacroTurn extends GRTMacro {

    private double targetAngle;
    private double currentAngle;
    private final double turnAngle;
    private GRTGyro gyro;
    private GRTDriveTrain dt;
    private PIDController controller;
    private static final double P = 1.0;
    private static final double I = 0.0;
    private static final double D = 0.0;
    private static final double F = 0.0;
    private static final double POLL_TIME = 0.05;

    private PIDSource pidSource = new PIDSource() {
        public double pidGet() {
            return gyro.getAngle();
        }
    };
    private PIDOutput pidOutput = new PIDOutput() {
        public void pidWrite(double output) {
            logInfo("Drive left: " + output + " drive right: " + -output);
            dt.setMotorSpeeds(output, -output);
        }
    };
    
    /**
     * Creates a new turning macro, that turns a set number of degrees.
     * 
     * @param turnAngle angle to turn, in degrees
     * @param gyro gyroscope to track robot movement
     * @param dt drivetrain to command
     */
    public MacroTurn(GRTDriveTrain dt, GRTGyro gyro, double turnAngle, int timeout) {
        super("Turn Macro", timeout, 50);
        
        this.turnAngle = turnAngle;
        this.gyro = gyro;
        controller = new PIDController(P, I, D, F, pidSource, pidOutput, POLL_TIME);
        controller.setOutputRange(-1, 1);
        
        controller.setAbsoluteTolerance(3.0);
    }

    protected void perform() {
        if (controller.onTarget())
            hasCompletedExecution = true;
    }

    public void die() {
        controller.disable();
        controller.free();
    }
    
    public void initialize() {
        currentAngle = gyro.getAngle();
        targetAngle = currentAngle + turnAngle;
        controller.setSetpoint(targetAngle);
        controller.enable();
    }
}

We created a wrapper class for WPILib’s Gyro for our own purposes, but the .getAngle() call is analagous.

The most important parts of our class are a) That we create a PIDController which gets input from the gyro angle, and that b) it’s output is on a scale from -1 to 1, and drives the left and right drivetrains in opposite directions.

With this code, we’re able to get within about 2º of our target angle. Hope this helps a little!

Lol, I just found this the second after I submitted my last reply: http://wpilib.screenstepslive.com/s/3120/m/7912/l/85772-gyros-to-control-robot-driving-direction

We use what we call a PID-cheat.

First, we determine experimentally what input is required to the just barely turn the drivetrain consistently.

We then tune our PID proportional term to get us close at multiple angles (but never overshooting). We then put the PID cheat code after the PID, so that if the drivetrain is ever sent a drive value below the cheat, it will change it to the minimum value so that it will turn the robot.

For example:

Try to turn 30 degrees.
Robot turns 25 degrees. At this point the PID P term becomes small and can’t turn the robot. The PID cheat sees the drive command that is too small, and coerces it to the minimum value to turn so that you reach the requested angle.

We’ve found this system to be much easier than trying to tune a perfect PID. It’s much more forgiving of changes from robot to robot, match to match.

We have always gotten ‘close enough’ in the turn, using turn-in-place (differential steer) and slightly more accurate turn almost in place (driving only one side while holding the other) depending on desired accuracy, and compensated for whatever angular error is in the next straight move.

Our 2011 code has all of this, it’s not as neat as more recent code but it all works.

As for the ‘cheat’ system, we occasionally overshoot the target given to the PID controller (e.g. add 5degrees * sign of angle) to help it along. We always do this with the distance-based controllers, by up to 2 feet (I believe we’re currently running 18" of distance bias).

I think it depends what you are trying to do. When the desired motion is to turn in place THEN drive somewhere, we’re pretty happy to let the turn in place motion overshoot or undershoot as the drive motion should straighten it out (so long as the turn controller is running in parallel with the “drive straight” controller).

If you are trying to turn in place then shoot a ball or a frisbee, I think a bunch of your issues can be taken out by an I term that only applies at low error. Basically:


out = err*kp + (err-last_err)*kd;
if (err < 5 degrees) {
  sum += err;
  out += sum*ki;
} else {
  sum = 0;
}

To tune this controller, increase KP until the robot is just overshooting, then increase KD until it stops a bit faster with less overshoot. At this point you should settle within a few degrees of your target and you can increase KI (this should be an order of magnitude or 2 smaller than KP) until it snaps into place.

At Plasma Robotics, we use a sort of “ghetto-PID” with a gyro - basically, we have a loop that turns the robot, slows it down once it’s close, and stops once it’s within an acceptable range. It then waits for a few milliseconds to make sure it doesn’t overshoot - if it does, we start the process again.

It’s not quite as accurate as a PID, but it’s easier to understand and debug. That is, unless you do it in LabVIEW like we did and have wire-spaghetti all over the place :stuck_out_tongue:

Here’s the guts of our autonomous turning code from last year’s robot. The values assigned to turnRate were determined experimentally.


// If we're within this # of degrees to target, then we're good.

#define DEGREES_PRECISION 2


/*
 * This command turns the number of degrees specified in the dashboard field.
 */
TurnSpecifiedDegreesCommand::TurnSpecifiedDegreesCommand(float degreesToTurn) 
	: CommandBase("TurnSpecifiedDegreesCommand") {
	Requires(chassis);
	this->degreesToTurn = degreesToTurn;
}

void TurnSpecifiedDegreesCommand::Initialize() {
	this->finished = false;
	this->startingAngle = sensorSubsystem->GetYawGyroAngle();
	this->goalAngle = startingAngle + this->degreesToTurn;
	printf("TurnSpecifiedDegreesCommand %f
", this->degreesToTurn);
}

void TurnSpecifiedDegreesCommand::Execute() {
	float currentAngle = sensorSubsystem->GetYawGyroAngle();
	float angleDifference = goalAngle - currentAngle;
	
	float turnRate = 0;
	
	if (fabs(angleDifference) < DEGREES_PRECISION) {
		chassis->stop();
		finished = true;
	} else {
		// We slow our rate of turn as we get close to the angle we want.
		// These values are guesses.  A PID would be better here.
		if (angleDifference > 10 || angleDifference < -10) {
			turnRate = 0.7;
		} else {
			// Look at changing this at competition
			turnRate = 0.6;
		}
		
		if (angleDifference > 0) {
			chassis->turnLeft(turnRate);
		} else {
			chassis->turnRight(turnRate);
		}
	}
}

// Make this return true when this Command no longer needs to run execute()
bool TurnSpecifiedDegreesCommand::IsFinished() {
	return finished;	
}

// Called once after isFinished returns true
void TurnSpecifiedDegreesCommand::End() {
	printf("TurnSpecifiedDegreesCommand completed.
");
	chassis->stop();
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void TurnSpecifiedDegreesCommand::Interrupted() {
	printf("TurnSpecifiedDegreesCommand interrupted.
");
	chassis->stop();
}