View Single Post
  #2   Spotlight this post!  
Unread 13-03-2013, 01:16
gerberduffy gerberduffy is offline
Registered User
FRC #0192
 
Join Date: Jun 2011
Location: California
Posts: 8
gerberduffy is an unknown quantity at this point
Re: Turning autonomously

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):

Code:
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!

Last edited by gerberduffy : 13-03-2013 at 01:19.