diff --git a/src/org/usfirst/frc/team2521/robot/OI.java b/src/org/usfirst/frc/team2521/robot/OI.java
index 8abeb6b..52832c4 100644
--- a/src/org/usfirst/frc/team2521/robot/OI.java
+++ b/src/org/usfirst/frc/team2521/robot/OI.java
@@ -2,8 +2,10 @@
 
 import org.usfirst.frc.team2521.robot.commands.automation.DriveToGear;
 import org.usfirst.frc.team2521.robot.commands.base.RunAgitator;
-import org.usfirst.frc.team2521.robot.commands.base.RunFeeder;
-import org.usfirst.frc.team2521.robot.commands.groups.RunShooterAndAgitator;
+import org.usfirst.frc.team2521.robot.commands.groups.AlignShooter;
+import org.usfirst.frc.team2521.robot.commands.groups.RunShooterSubsystems;
+
+import java.util.stream.IntStream;
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
@@ -13,31 +15,35 @@
  * driver to control the robot.
  */
 public class OI {
+	private static final int[] CUSTOM_BUTTONS = {2, 3, 4, 5};
+
 	private final Joystick left;
 	private final Joystick right;
 	private final Joystick secondary;
+	private final Joystick custom;
 
+	private JoystickButton alignShooterButton;
 	private JoystickButton driveToGearLeftButton;
 	private JoystickButton driveToGearRightButton;
-	private JoystickButton runShooterAndAgitatorButton;
-	private JoystickButton runFeederButton;
+	private JoystickButton runShooterSubsystemsButton;
 	private JoystickButton runAgitatorBackwardButton;
 
 	private OI() {
 		left = new Joystick(RobotMap.LEFT_STICK_PORT);
 		right = new Joystick(RobotMap.RIGHT_STICK_PORT);
 		secondary = new Joystick(RobotMap.SECONDARY_STICK_PORT);
+		custom = new Joystick(RobotMap.CUSTOM_STICK_PORT);
 
 		// Right joystick buttons
-		driveToGearLeftButton = new JoystickButton(right, RobotMap.DRIVE_TO_GEAR_LEFT_PORT);
-		driveToGearRightButton = new JoystickButton(right, RobotMap.DRIVE_TO_GEAR_RIGHT_PORT);
+		alignShooterButton = new JoystickButton(left, RobotMap.ALIGN_SHOOTER_BUTTON_PORT);
+		driveToGearLeftButton = new JoystickButton(left, RobotMap.DRIVE_TO_GEAR_LEFT_PORT);
+		driveToGearRightButton = new JoystickButton(left, RobotMap.DRIVE_TO_GEAR_RIGHT_PORT);
 
 		// Secondary joystick buttons
-		runShooterAndAgitatorButton =
-				new JoystickButton(secondary, RobotMap.RUN_SHOOTER_AND_AGITATOR_BUTTON_PORT);
-		runFeederButton = new JoystickButton(secondary, RobotMap.RUN_FEEDER_BUTTON_PORT);
-		runAgitatorBackwardButton =
-				new JoystickButton(secondary, RobotMap.RUN_AGITATOR_BACKWARD_BUTTON_PORT);
+		runShooterSubsystemsButton = new JoystickButton(secondary,
+														RobotMap.RUN_SHOOTER_SUBSYSTEMS_BUTTON_PORT);
+		runAgitatorBackwardButton = new JoystickButton(secondary,
+													   RobotMap.RUN_AGITATOR_BACKWARD_BUTTON_PORT);
 
 		setButtonListeners();
 	}
@@ -49,6 +55,13 @@ public static OI getInstance() {
 		return Holder.INSTANCE;
 	}
 
+	public int getAutoMode() {
+		return IntStream.range(0, CUSTOM_BUTTONS.length)
+				.filter(i -> custom.getRawButton(CUSTOM_BUTTONS[i]))
+				.map(i -> (int) Math.pow(2, i))
+				.sum();
+	}
+
 	/**
 	 * @return the left joystick
 	 */
@@ -71,10 +84,10 @@ public Joystick getSecondaryStick() {
 	}
 
 	private void setButtonListeners() {
+		alignShooterButton.toggleWhenActive(new AlignShooter());
 		driveToGearLeftButton.toggleWhenActive(new DriveToGear(true));
 		driveToGearRightButton.toggleWhenActive(new DriveToGear(false));
-		runShooterAndAgitatorButton.toggleWhenActive(new RunShooterAndAgitator());
-		runFeederButton.whileActive(new RunFeeder());
+		runShooterSubsystemsButton.toggleWhenActive(new RunShooterSubsystems());
 		runAgitatorBackwardButton.toggleWhenActive(new RunAgitator(false));
 	}
 
diff --git a/src/org/usfirst/frc/team2521/robot/Robot.java b/src/org/usfirst/frc/team2521/robot/Robot.java
index b5a5f44..87c0de4 100644
--- a/src/org/usfirst/frc/team2521/robot/Robot.java
+++ b/src/org/usfirst/frc/team2521/robot/Robot.java
@@ -38,6 +38,8 @@ public void robotInit() {
 		agitator = new Agitator();
 
 		auto = new Auto();
+
+		sensors.setCVCamera(Sensors.Camera.REAR);
 	}
 
 	@Override
@@ -47,6 +49,7 @@ public void disabledPeriodic() {
 
 	@Override
 	public void autonomousInit() {
+		Robot.sensors.setCVCamera(Sensors.Camera.FRONT);
 		auto.start();
 	}
 
diff --git a/src/org/usfirst/frc/team2521/robot/RobotMap.java b/src/org/usfirst/frc/team2521/robot/RobotMap.java
index 4eb76b7..8ffde5a 100644
--- a/src/org/usfirst/frc/team2521/robot/RobotMap.java
+++ b/src/org/usfirst/frc/team2521/robot/RobotMap.java
@@ -7,15 +7,16 @@ public class RobotMap {
 	public static final int LEFT_STICK_PORT = 0;
 	public static final int RIGHT_STICK_PORT = 1;
 	public static final int SECONDARY_STICK_PORT = 2;
+	public static final int CUSTOM_STICK_PORT = 3;
 
 	// Right joystick buttons
-	public static final int DRIVE_TO_GEAR_LEFT_PORT = 4; // On right stick
-	public static final int DRIVE_TO_GEAR_RIGHT_PORT = 5; // On right stick
+	public static final int ALIGN_SHOOTER_BUTTON_PORT = 7; // On Left stick
+	public static final int DRIVE_TO_GEAR_LEFT_PORT = 8; // On left stick
+	public static final int DRIVE_TO_GEAR_RIGHT_PORT = 9; // On left stick
 
 	// Secondary joystick buttons
-	public static final int RUN_SHOOTER_AND_AGITATOR_BUTTON_PORT = 1; // On secondary stick
-	public static final int RUN_FEEDER_BUTTON_PORT = 2; // On secondary stick
-	public static final int RUN_AGITATOR_BACKWARD_BUTTON_PORT = 4; // On secondary stick
+	public static final int RUN_SHOOTER_SUBSYSTEMS_BUTTON_PORT = 1; // On secondary stick
+	public static final int RUN_AGITATOR_BACKWARD_BUTTON_PORT = 2; // On secondary stick
 
 	public static final int FRONT_RIGHT_MOTOR = 10;
 	public static final int FRONT_LEFT_MOTOR = 14;
@@ -30,7 +31,17 @@ public class RobotMap {
 	public static final int SHOOTER_MOTOR = 13;
 	public static final int FEEDER_MOTOR = 12;
 
-	public static final int FRONT_ULTRA_PORT = 0;
-	public static final int REAR_ULTRA_PORT = 2;
-	public static final int SIDE_LIDAR_PORT = 1;
+	public static final int REAR_ULTRA_PORT = 1;
+	public static final int LEFT_LIDAR_PORT = 0;
+	public static final int RIGHT_LIDAR_PORT = 2;
+
+	public final static class AutoModes {
+		public final static int NOTHING = 0;
+		public final static int CROSS_BASE_LINE = 6;
+		public final static int BALL_THEN_GEAR = 15;
+		public final static int GEAR_LEFT = 8;
+		public final static int GEAR_MIDDLE = 4;
+		public final static int GEAR_RIGHT = 2;
+		public final static int BALLS_ONLY = 1;
+	}
 }
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToAngle.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToAngle.java
deleted file mode 100644
index 7759dc7..0000000
--- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToAngle.java
+++ /dev/null
@@ -1,55 +0,0 @@
-package org.usfirst.frc.team2521.robot.commands.automation;
-
-import org.usfirst.frc.team2521.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.PIDCommand;
-
-/**
- * This command drives to a specified angle using the navX.
- */
-public class DriveToAngle extends PIDCommand {
-	private static final double P = 0.003;
-	private static final double I = 0;
-	private static final double D = 0;
-
-	private static final double ERROR_THRESHOLD = 1;
-	private static final double MIN_OUTPUT = 0.1;
-
-	private double targetAngle;
-
-	/**
-	 * @param targetAngle the angle we want to drive to
-	 */
-	public DriveToAngle(double targetAngle) {
-		super(P, I, D);
-		requires(Robot.drivetrain);
-		this.targetAngle = targetAngle;
-		getPIDController().setAbsoluteTolerance(ERROR_THRESHOLD);
-	}
-
-	@Override
-	protected void initialize() {
-		Robot.sensors.resetNavxAngle();
-		targetAngle += Robot.sensors.getNavxAngle();
-		setSetpoint(targetAngle);
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return getPIDController().onTarget();
-	}
-
-	@Override
-	protected double returnPIDInput() {
-		return Robot.sensors.getNavxAngle();
-	}
-
-	@Override
-	protected void usePIDOutput(double output) {
-		if (Math.abs(output) < MIN_OUTPUT) {
-			output = Math.signum(output) * MIN_OUTPUT;
-		}
-		Robot.drivetrain.setLeft(output);
-		Robot.drivetrain.setRight(output);
-	}
-}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBlob.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBlob.java
new file mode 100644
index 0000000..149c2ef
--- /dev/null
+++ b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBlob.java
@@ -0,0 +1,67 @@
+package org.usfirst.frc.team2521.robot.commands.automation;
+
+import org.usfirst.frc.team2521.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.PIDCommand;
+
+/**
+ * This abstract class provides a template for driving to the gear or the boiler.
+ */
+public abstract class DriveToBlob extends PIDCommand {
+	private static final double CAMERA_PROJ_PLANE_DISTANCE = 216.226;
+
+	/** true if we're straight-on */
+	protected boolean oriented = false;
+	/** true if we're on the left side of the target */
+	private boolean onLeftSide;
+	private boolean hasFoundBlob = false;
+
+	/**
+	 * @param onLeftSide whether we're on the left side of the target
+	 */
+	protected DriveToBlob(double p, double i, double d, boolean onLeftSide) {
+		super(p, i, d);
+		this.onLeftSide = onLeftSide;
+		requires(Robot.drivetrain);
+	}
+
+	protected abstract double getSlowSpeed();
+
+	@Override
+	protected abstract void initialize();
+
+	@Override
+	protected final void execute() {
+		// Angle between camera line of sight and target
+		double targetAngle = Math.atan(Robot.sensors.getCVOffsetX() / CAMERA_PROJ_PLANE_DISTANCE);
+
+		// Convert angle to degrees
+		targetAngle *= 180 / Math.PI;
+
+		// Consider ourselves oriented when our angle error is less than 20
+		oriented = Math.abs(targetAngle) < 20;
+
+		targetAngle += Robot.sensors.getNavxAngle();
+		setSetpoint(targetAngle);
+
+		if (Robot.sensors.getBlobFound()) {
+			hasFoundBlob = true;
+		}
+	}
+
+	@Override
+	protected final double returnPIDInput() {
+		return Robot.sensors.getNavxAngle();
+	}
+
+	protected void setCurrentBlobFoundMotorSpeed() {
+		if (hasFoundBlob) {
+			Robot.drivetrain.setLeft(getSlowSpeed());
+			Robot.drivetrain.setRight(getSlowSpeed());
+		} else {
+			// Turn clockwise if we're too far left, counter-clockwise if we're too far right
+			Robot.drivetrain.setLeft(onLeftSide ? -getSlowSpeed() : getSlowSpeed());
+			Robot.drivetrain.setRight(onLeftSide ? -getSlowSpeed() : getSlowSpeed());
+		}
+	}
+}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBoiler.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBoiler.java
new file mode 100644
index 0000000..c2f8efa
--- /dev/null
+++ b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToBoiler.java
@@ -0,0 +1,54 @@
+package org.usfirst.frc.team2521.robot.commands.automation;
+
+import org.usfirst.frc.team2521.robot.Robot;
+import org.usfirst.frc.team2521.robot.subsystems.Sensors;
+
+/**
+ * This command drives to the correct spot to shoot from automatically.
+ */
+public class DriveToBoiler extends DriveToBlob {
+	private static final double P = 0.008;
+	private static final double I = 0;
+	private static final double D = 0;
+
+	private static final double P2 = 0.0125;
+	private static final double DISTANCE_SETPOINT = 35;
+	private static final double DISTANCE_ERROR_THRESHOLD = 3;
+
+	public DriveToBoiler() {
+		super(P, I, D, false);
+	}
+
+	@Override
+	protected double getSlowSpeed() {
+		return P2 * (DISTANCE_SETPOINT - Robot.sensors.getRearUltraInches());
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.sensors.setCVCamera(Sensors.Camera.REAR);
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return Math.abs(DISTANCE_SETPOINT - Robot.sensors.getRearUltraInches()) < DISTANCE_ERROR_THRESHOLD;
+	}
+
+	@Override
+	protected final void usePIDOutput(double output) {
+		if (Robot.sensors.getBlobFound()) {
+			// If we are already oriented, drive straight
+			if (oriented) {
+				Robot.drivetrain.setLeft(getSlowSpeed());
+				Robot.drivetrain.setRight(getSlowSpeed());
+			} else if (-output > 0) {
+				Robot.drivetrain.setLeft(output);
+			} else {
+				Robot.drivetrain.setRight(-output);
+			}
+		} else {
+			setCurrentBlobFoundMotorSpeed();
+		}
+	}
+
+}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToEncoder.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToEncoder.java
deleted file mode 100644
index 0d73770..0000000
--- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToEncoder.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package org.usfirst.frc.team2521.robot.commands.automation;
-
-import org.usfirst.frc.team2521.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class DriveToEncoder extends Command {
-	private final double ENC_PULSES_PER_REVOLUTION = 8192;
-	private final double WHEEL_DIAMETER = 6;
-
-	private double distance;
-
-	public DriveToEncoder(double distance) {
-		requires(Robot.drivetrain);
-
-		distance = (distance / (WHEEL_DIAMETER * Math.PI)) * ENC_PULSES_PER_REVOLUTION;
-		distance += Robot.drivetrain.getRightPosition();
-
-		this.distance = distance;
-	}
-
-	protected void execute() {
-		Robot.drivetrain.setPosition(distance);
-	}
-
-	@Override
-	protected void end() {
-		Robot.drivetrain.setLeft(0);
-		Robot.drivetrain.setRight(0);
-	}
-
-	@Override
-	protected boolean isFinished() {
-		return Math.abs(Robot.drivetrain.getRightEncoderError()) < 100;
-	}
-}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java
index b45ea0b..2543de5 100644
--- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java
+++ b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToGear.java
@@ -2,94 +2,54 @@
 
 import org.usfirst.frc.team2521.robot.Robot;
 import org.usfirst.frc.team2521.robot.subsystems.Drivetrain;
-
-import edu.wpi.first.wpilibj.command.PIDCommand;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team2521.robot.subsystems.Sensors;
 
 /**
  * This command drives to the gear drop-off automatically.
  */
-public class DriveToGear extends PIDCommand {
+public class DriveToGear extends DriveToBlob {
 	private static final double P = 0.008;
 	private static final double I = 0;
 	private static final double D = 0;
 
-	private static final double CAMERA_PROJ_PLANE_DISTANCE = 216.226;
-
-	/** true if we're straight-on */
-	private boolean oriented = false;
-	/** true if we're on the left side of the target */
-	private boolean onLeftSide;
-
-	private boolean hasFoundBlob = false;
-
-	/**
-	 * @param onLeftSide whether we're on the left side of the target
-	 */
 	public DriveToGear(boolean onLeftSide) {
-		super(P, I, D);
-		this.onLeftSide = onLeftSide;
-		requires(Robot.drivetrain);
+		super(P, I, D, onLeftSide);
 	}
 
 	@Override
-	protected void execute() {
-		// Angle between camera line of sight and target
-		double targetAngle = Math.atan(Robot.sensors.getCVOffsetX() / CAMERA_PROJ_PLANE_DISTANCE);
-
-		// Convert angle to degrees
-		targetAngle *= 180 / Math.PI;
-
-		// Consider ourselves oriented when our angle error is less than 20
-		oriented = Math.abs(targetAngle) < 20;
-
-		targetAngle += Robot.sensors.getNavxAngle();
-		setSetpoint(targetAngle);
-
-		if (Robot.sensors.getBlobFound()) {
-			hasFoundBlob = true;
-		}
+	protected double getSlowSpeed() {
+		return Drivetrain.SLOW_SPEED;
+	}
 
-		if (Robot.DEBUG) {
-			SmartDashboard.putNumber("Drive to gear setpoint", targetAngle);
-			SmartDashboard.putBoolean("Oriented", oriented);
-		}
+	@Override
+	protected void initialize() {
+		Robot.sensors.setCVCamera(Sensors.Camera.FRONT);
 	}
 
 	@Override
 	protected boolean isFinished() {
-		return false;
+		return !Robot.sensors.getBlobFound();
 	}
 
 	@Override
-	protected double returnPIDInput() {
-		return Robot.sensors.getNavxAngle();
+	protected void end() {
+		Robot.sensors.setCVCamera(Sensors.Camera.REAR);
 	}
 
 	@Override
-	protected void usePIDOutput(double output) {
-		if (Robot.DEBUG) {
-			SmartDashboard.putNumber("Drive to gear output", output);
-		}
+	protected final void usePIDOutput(double output) {
 		if (Robot.sensors.getBlobFound()) {
 			// If we are already oriented, drive straight
 			if (oriented) {
-				Robot.drivetrain.setLeft(Drivetrain.SLOW_SPEED);
-				Robot.drivetrain.setRight(-Drivetrain.SLOW_SPEED);
-			} else if (output < 0) {
+				Robot.drivetrain.setLeft(getSlowSpeed());
+				Robot.drivetrain.setRight(getSlowSpeed());
+			} else if (output > 0) {
 				Robot.drivetrain.setLeft(output);
 			} else {
-				Robot.drivetrain.setRight(output);
+				Robot.drivetrain.setRight(-output);
 			}
 		} else {
-			if (hasFoundBlob) {
-				Robot.drivetrain.setLeft(Drivetrain.SLOW_SPEED);
-				Robot.drivetrain.setRight(-Drivetrain.SLOW_SPEED);
-			} else {
-				// Turn clockwise if we're too far left, counter-clockwise if we're too far right
-				Robot.drivetrain.setLeft(onLeftSide ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED);
-				Robot.drivetrain.setRight(onLeftSide ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED);
-			}
+			setCurrentBlobFoundMotorSpeed();
 		}
 	}
 }
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToUltra.java b/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToUltra.java
deleted file mode 100644
index c975399..0000000
--- a/src/org/usfirst/frc/team2521/robot/commands/automation/DriveToUltra.java
+++ /dev/null
@@ -1,49 +0,0 @@
-package org.usfirst.frc.team2521.robot.commands.automation;
-
-import org.usfirst.frc.team2521.robot.Robot;
-import org.usfirst.frc.team2521.robot.subsystems.Drivetrain;
-
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- * This command drives to a specified ultrasonic distance automatically.
- */
-public class DriveToUltra extends Command {
-	private static final double ERROR_THRESHOLD = 1;
-	/** {@code true} if we should use the front ultrasonic */
-	protected boolean useRearUltra = false;
-	private double setpoint;
-	private double ultrasonicValue;
-
-	/**
-	 * @param setpoint     the ultrasonic setpoint
-	 * @param useRearUltra whether we should use the rear ultrasonic
-	 */
-	public DriveToUltra(double setpoint, boolean useRearUltra) {
-		this.setpoint = setpoint;
-		this.useRearUltra = useRearUltra;
-	}
-
-	@Override
-	protected void execute() {
-		ultrasonicValue = useRearUltra ? Robot.sensors.getRearUltraInches() : Robot.sensors.getFrontUltraInches();
-
-		if (useRearUltra) {
-			Robot.drivetrain.setLeft(setpoint - ultrasonicValue > 0 ? Drivetrain.SLOW_SPEED : -Drivetrain.SLOW_SPEED);
-			Robot.drivetrain.setRight(setpoint - ultrasonicValue > 0 ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED);
-		} else {
-			Robot.drivetrain.setLeft(setpoint - ultrasonicValue > 0 ? -Drivetrain.SLOW_SPEED : Drivetrain.SLOW_SPEED);
-			Robot.drivetrain.setRight(setpoint - ultrasonicValue > 0 ? Drivetrain.SLOW_SPEED : -Drivetrain.SLOW_SPEED);
-		}
-	}
-
-	@Override
-	protected boolean isFinished() {
-		if (Robot.DEBUG) {
-			SmartDashboard.putNumber("Drive to ultra setpoint", setpoint);
-			SmartDashboard.putNumber("Drive to ultra error", setpoint - ultrasonicValue);
-		}
-		return Math.abs(setpoint - ultrasonicValue) < ERROR_THRESHOLD;
-	}
-}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/Spintake.java b/src/org/usfirst/frc/team2521/robot/commands/automation/Spintake.java
deleted file mode 100644
index a827111..0000000
--- a/src/org/usfirst/frc/team2521/robot/commands/automation/Spintake.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package org.usfirst.frc.team2521.robot.commands.automation;
-
-import org.usfirst.frc.team2521.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- * This command backs away from the intake, spins the robot 180 degrees,
- * and drives back.
- */
-public class Spintake extends CommandGroup {
-	public Spintake() {
-		addSequential(new RearDriveToUltra(10));
-		addSequential(new DriveToAngle(180));
-		addSequential(new RearDriveToUltra(0));
-	}
-
-	private static class RearDriveToUltra extends DriveToUltra {
-		public RearDriveToUltra(double setpoint) {
-			super(setpoint, false);
-		}
-
-		@Override
-		protected void initialize() {
-			useRearUltra = Robot.sensors.getFrontUltraInches() > Robot.sensors.getRearUltraInches();
-		}
-	}
-}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/automation/TurnToBoiler.java b/src/org/usfirst/frc/team2521/robot/commands/automation/TurnToBoiler.java
new file mode 100644
index 0000000..3849447
--- /dev/null
+++ b/src/org/usfirst/frc/team2521/robot/commands/automation/TurnToBoiler.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team2521.robot.commands.automation;
+
+import org.usfirst.frc.team2521.robot.Robot;
+import org.usfirst.frc.team2521.robot.subsystems.Sensors;
+
+import edu.wpi.first.wpilibj.command.PIDCommand;
+
+public class TurnToBoiler extends PIDCommand {
+	private static final double P = 0.0025;
+	private static final double I = 0.00015;
+	private static final double D = 0;
+
+	public TurnToBoiler() {
+		super(P, I, D);
+		requires(Robot.drivetrain);
+	}
+
+	@Override
+	protected void initialize() {
+		Robot.sensors.setCVCamera(Sensors.Camera.REAR);
+	}
+
+	@Override
+	protected boolean isFinished() {
+		return false;
+	}
+
+	@Override
+	protected double returnPIDInput() {
+		return Robot.sensors.getCVOffsetX();
+	}
+
+	@Override
+	protected void usePIDOutput(double output) {
+		if (Robot.sensors.getBlobFound()) {
+			Robot.drivetrain.setLeft(-output);
+			Robot.drivetrain.setRight(output);
+		} else {
+			Robot.drivetrain.setLeft(0);
+			Robot.drivetrain.setRight(0);
+		}
+	}
+}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java b/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java
index 99f9d23..a968cfa 100644
--- a/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java
+++ b/src/org/usfirst/frc/team2521/robot/commands/base/RunDrivetrain.java
@@ -6,14 +6,13 @@
 import edu.wpi.first.wpilibj.command.Command;
 
 public class RunDrivetrain extends Command {
-
 	public RunDrivetrain() {
 		requires(Robot.drivetrain);
 	}
 
 	protected void execute() {
 		Robot.drivetrain.setLeft(Drivetrain.SLOW_SPEED);
-		Robot.drivetrain.setRight(-Drivetrain.SLOW_SPEED);
+		Robot.drivetrain.setRight(Drivetrain.SLOW_SPEED);
 	}
 
 	@Override
diff --git a/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java b/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java
index 7ab6c17..41fe348 100644
--- a/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java
+++ b/src/org/usfirst/frc/team2521/robot/commands/base/RunFeeder.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team2521.robot.commands.base;
 
 import org.usfirst.frc.team2521.robot.Robot;
+import org.usfirst.frc.team2521.robot.subsystems.Feeder;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -8,15 +9,13 @@
  * This command feeds the balls to the shooter.
  */
 public class RunFeeder extends Command {
-	private static final double FEEDER_SPEED = 0.75;
-
 	public RunFeeder() {
 		requires(Robot.feeder);
 	}
 
 	@Override
 	protected void execute() {
-		Robot.feeder.setMotor(-FEEDER_SPEED);
+		Robot.feeder.setMotor(-Feeder.FEEDER_SPEED);
 	}
 
 	@Override
diff --git a/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java b/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java
index 973a709..6928580 100644
--- a/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java
+++ b/src/org/usfirst/frc/team2521/robot/commands/base/RunShooter.java
@@ -13,7 +13,7 @@ public class RunShooter extends PIDCommand {
 	private static final double I = 0;
 	private static final double D = 0;
 
-	private static final int SETPOINT = -280;
+	private static final int SETPOINT = -290;
 
 	public RunShooter() {
 		super(P, I, D);
@@ -28,10 +28,7 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-		if (Robot.DEBUG) {
-			SmartDashboard.putNumber("Encoder Value", Robot.shooter.getEncVelocity());
-			SmartDashboard.putNumber("Error", SETPOINT - Robot.shooter.getEncVelocity());
-		}
+		SmartDashboard.putNumber("Encoder velocity", Robot.shooter.getEncVelocity());
 	}
 
 	@Override
@@ -52,6 +49,5 @@ protected double returnPIDInput() {
 	@Override
 	protected void usePIDOutput(double output) {
 		Robot.shooter.setMotor(-output);
-		//Robot.shooter.setMotor(0.535);
 	}
 }
diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/AlignShooter.java b/src/org/usfirst/frc/team2521/robot/commands/groups/AlignShooter.java
new file mode 100644
index 0000000..b510d97
--- /dev/null
+++ b/src/org/usfirst/frc/team2521/robot/commands/groups/AlignShooter.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team2521.robot.commands.groups;
+
+import org.usfirst.frc.team2521.robot.commands.automation.DriveToBoiler;
+import org.usfirst.frc.team2521.robot.commands.automation.TurnToBoiler;
+import org.usfirst.frc.team2521.robot.commands.base.RunShooter;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ * This command turns until the boiler is centered for the camera,
+ * drives to a specified distance, and then runs the shooter.
+ */
+public class AlignShooter extends CommandGroup {
+	public AlignShooter() {
+		addParallel(new RunShooter());
+		addSequential(new DriveToBoiler());
+		addParallel(new TurnToBoiler());
+		addSequential(new TimedCommand(1));
+		addParallel(new RunShooterSubsystems());
+	}
+}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java b/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java
index e5b7738..875a259 100644
--- a/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java
+++ b/src/org/usfirst/frc/team2521/robot/commands/groups/Auto.java
@@ -1,13 +1,46 @@
 package org.usfirst.frc.team2521.robot.commands.groups;
 
+import org.usfirst.frc.team2521.robot.OI;
+import org.usfirst.frc.team2521.robot.Robot;
+import org.usfirst.frc.team2521.robot.RobotMap;
 import org.usfirst.frc.team2521.robot.commands.automation.DriveToGear;
 import org.usfirst.frc.team2521.robot.commands.base.RunDrivetrain;
+import org.usfirst.frc.team2521.robot.subsystems.Sensors;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.TimedCommand;
 
 public class Auto extends CommandGroup {
 	public Auto() {
-		addSequential(new RunDrivetrain(), 2);
-		addSequential(new DriveToGear(false));
+		switch (OI.getInstance().getAutoMode()) {
+			case RobotMap.AutoModes.NOTHING:
+				break;
+			case RobotMap.AutoModes.CROSS_BASE_LINE:
+				addSequential(new RunDrivetrain(), 2);
+				break;
+			case RobotMap.AutoModes.BALLS_ONLY:
+				addSequential(new RunDrivetrain(), 1);
+				addSequential(new AlignShooter());
+				break;
+			case RobotMap.AutoModes.BALL_THEN_GEAR:
+				Robot.sensors.setCVCamera(Sensors.Camera.FRONT);
+				addSequential(new RunDrivetrain(), 1);
+				addSequential(new DriveToGear(false));
+				addSequential(new RunDrivetrain(), 0.25);
+				addSequential(new TimedCommand(1));
+				addSequential(new AlignShooter());
+				break;
+			case RobotMap.AutoModes.GEAR_LEFT:
+				addSequential(new RunDrivetrain(), 1);
+				addSequential(new DriveToGear(false));
+				break;
+			case RobotMap.AutoModes.GEAR_MIDDLE:
+				addSequential(new DriveToGear(false));
+				break;
+			case RobotMap.AutoModes.GEAR_RIGHT:
+				addSequential(new RunDrivetrain(), 1);
+				addSequential(new DriveToGear(true));
+				break;
+		}
 	}
 }
diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterAndAgitator.java b/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterAndAgitator.java
deleted file mode 100644
index 12a4d37..0000000
--- a/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterAndAgitator.java
+++ /dev/null
@@ -1,13 +0,0 @@
-package org.usfirst.frc.team2521.robot.commands.groups;
-
-import org.usfirst.frc.team2521.robot.commands.base.RunAgitator;
-import org.usfirst.frc.team2521.robot.commands.base.RunShooter;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-public class RunShooterAndAgitator extends CommandGroup {
-	public RunShooterAndAgitator() {
-		addParallel(new RunAgitator(true));
-		addParallel(new RunShooter());
-	}
-}
diff --git a/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterSubsystems.java b/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterSubsystems.java
new file mode 100644
index 0000000..5d1dca5
--- /dev/null
+++ b/src/org/usfirst/frc/team2521/robot/commands/groups/RunShooterSubsystems.java
@@ -0,0 +1,33 @@
+package org.usfirst.frc.team2521.robot.commands.groups;
+
+import org.usfirst.frc.team2521.robot.Robot;
+import org.usfirst.frc.team2521.robot.commands.base.RunAgitator;
+import org.usfirst.frc.team2521.robot.commands.base.RunFeeder;
+import org.usfirst.frc.team2521.robot.commands.base.RunShooter;
+import org.usfirst.frc.team2521.robot.subsystems.Feeder;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * This command runs the agitator and the shooter immediately, then
+ * starts the feeder once the shooter is up to speed.
+ */
+public class RunShooterSubsystems extends CommandGroup {
+	private double speedCutoff = -200;
+
+	public RunShooterSubsystems() {
+		addParallel(new RunAgitator(true));
+		addParallel(new RunShooter());
+		addSequential(new RunFeeder() {
+			private boolean upToSpeed = false;
+
+			@Override
+			protected void execute() {
+				if (upToSpeed || Robot.shooter.getEncVelocity() < speedCutoff) {
+					upToSpeed = true;
+					Robot.feeder.setMotor(-Feeder.FEEDER_SPEED);
+				}
+			}
+		});
+	}
+}
diff --git a/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java
index 3b571c4..c4eedb9 100644
--- a/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java
+++ b/src/org/usfirst/frc/team2521/robot/subsystems/Drivetrain.java
@@ -18,7 +18,7 @@
  */
 public class Drivetrain extends Subsystem {
 	/** Speed to set drivetrain to when we want to move at a slow, constant speed */
-	public static final double SLOW_SPEED = 0.2;
+	public static final double SLOW_SPEED = 0.4;
 
 	private final double P = 0.01;
 	private final double I = 0;
@@ -108,7 +108,7 @@ public void setLeft(double value) {
 	 */
 	public void setRight(double value) {
 		frontRight.changeControlMode(TalonControlMode.PercentVbus);
-		frontRight.set(value);
+		frontRight.set(-value);
 
 		rearRight.changeControlMode(TalonControlMode.Follower);
 		rearRight.set(RobotMap.FRONT_RIGHT_MOTOR);
diff --git a/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java b/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java
index 0448410..a246420 100644
--- a/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java
+++ b/src/org/usfirst/frc/team2521/robot/subsystems/Feeder.java
@@ -11,6 +11,8 @@
  * indexer to the fly wheel.
  */
 public class Feeder extends Subsystem {
+	public static final double FEEDER_SPEED = 0.75;
+
 	private CANTalon feeder;
 
 	public Feeder() {
diff --git a/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java b/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java
index 547c769..26908df 100644
--- a/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java
+++ b/src/org/usfirst/frc/team2521/robot/subsystems/Sensors.java
@@ -7,6 +7,7 @@
 import org.usfirst.frc.team2521.robot.commands.base.DisplaySensors;
 
 import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.networktables.NetworkTable;
@@ -17,9 +18,9 @@
  * methods to show sensor data on the SmartDashboard.
  */
 public class Sensors extends Subsystem {
-	private AnalogInput frontUltra;
 	private AnalogInput rearUltra;
-	private AnalogInput sideLidar;
+	private AnalogInput leftLidar;
+	private AnalogInput rightLidar;
 
 	private NetworkTable table;
 
@@ -30,67 +31,64 @@ public class Sensors extends Subsystem {
 	private double MED_LIDAR_B = -1.045;
 
 	public Sensors() {
-		frontUltra = new AnalogInput(RobotMap.FRONT_ULTRA_PORT);
 		rearUltra = new AnalogInput(RobotMap.REAR_ULTRA_PORT);
 
-		sideLidar = new AnalogInput(RobotMap.SIDE_LIDAR_PORT);
+		leftLidar = new AnalogInput(RobotMap.LEFT_LIDAR_PORT);
+		rightLidar = new AnalogInput(RobotMap.RIGHT_LIDAR_PORT);
 
 		table = NetworkTable.getTable("Vision");
 
 		ahrs = new AHRS(SPI.Port.kMXP);
 		ahrs.reset();
+
+		setCVThresholds();
 	}
 
 	/**
 	 * Displays sensor data on the SmartDashboard.
 	 */
 	public void display() {
+		SmartDashboard.putNumber("Rear ultra distance", getRearUltraInches());
+		SmartDashboard.putBoolean("Blob found", getBlobFound());
+		
 		if (Robot.DEBUG) {
-			SmartDashboard.putNumber("Angle", getNavxAngle());
-			SmartDashboard.putBoolean("Blob found", getBlobFound());
+			SmartDashboard.putNumber("CV offset", getCVOffsetX());
 		}
 	}
 
 	/**
-	 * @return the raw voltage from the front (gear side) ultrasonic sensor
-	 */
-	private double getFrontUltraRaw() {
-		return frontUltra.getVoltage();
-	}
-
-	/**
-	 * @return the raw voltage from the rear (shooter side) ultrasonic sensor
+	 * @return the distance in inches from the rear (shooter side) ultrasonic sensor
 	 */
-	private double getRearUltraRaw() {
-		return rearUltra.getVoltage();
+	public double getRearUltraInches() {
+		return rearUltra.getVoltage() * 1000 / 9.8;
 	}
 
 	/**
-	 * @return the distance in inches from the front (gear side) ultrasonic sensor
+	 * @return the raw value from the side lidar
 	 */
-	public double getFrontUltraInches() {
-		return getFrontUltraRaw() * 1000 / 9.8;
+	private double getLeftLidarRaw() {
+		return leftLidar.getValue();
 	}
 
 	/**
-	 * @return the distance in inches from the rear (shooter side) ultrasonic sensor
+	 * @return the distance in inches from the side lidar
 	 */
-	public double getRearUltraInches() {
-		return getRearUltraRaw() * 1000 / 9.8;
+	public double getLeftLidarInches() {
+		return MED_LIDAR_M / Math.pow(getLeftLidarRaw(), 2) + MED_LIDAR_B;
 	}
 
 	/**
 	 * @return the raw value from the side lidar
 	 */
-	private double getSideLidarRaw() {
-		return sideLidar.getValue();
+	private double getRightLidarRaw() {
+		return rightLidar.getValue();
 	}
 
 	/**
 	 * @return the distance in inches from the side lidar
 	 */
-	public double getSideLidarInches() {
-		return MED_LIDAR_M / Math.pow(getSideLidarRaw(), 2) + MED_LIDAR_B;
+	public double getRightLidarInches() {
+		return MED_LIDAR_M / Math.pow(getRightLidarRaw(), 2) + MED_LIDAR_B;
 	}
 
 	/**
@@ -112,6 +110,48 @@ public boolean getBlobFound() {
 		return table.getBoolean("blob_found", false);
 	}
 
+	/**
+	 * Sets which camera (front or back) vision code on the minnow
+	 * board should use.
+	 *
+	 * @param cameraType desired camera to use
+	 */
+	public void setCVCamera(Camera cameraType) {
+		table.putBoolean("front_camera", cameraType == Camera.FRONT);
+	}
+
+	/**
+	 * Sets the minnow board's blob thresholds based on SmartDashboard
+	 * preferences.
+	 */
+	private void setCVThresholds() {
+		double frontLowerRed = Preferences.getInstance().getDouble("front_lower_red", 0);
+		double frontLowerGreen = Preferences.getInstance().getDouble("front_lower_green", 55);
+		double frontLowerBlue = Preferences.getInstance().getDouble("front_lower_blue", 0);
+		double frontUpperRed = Preferences.getInstance().getDouble("front_upper_red", 50);
+		double frontUpperGreen = Preferences.getInstance().getDouble("front_upper_green", 175);
+		double frontUpperBlue = Preferences.getInstance().getDouble("front_upper_blue", 50);
+		double rearLowerRed = Preferences.getInstance().getDouble("rear_lower_red", 0);
+		double rearLowerGreen = Preferences.getInstance().getDouble("rear_lower_green", 55);
+		double rearLowerBlue = Preferences.getInstance().getDouble("rear_lower_blue", 0);
+		double rearUpperRed = Preferences.getInstance().getDouble("rear_upper_red", 50);
+		double rearUpperGreen = Preferences.getInstance().getDouble("rear_upper_green", 175);
+		double rearUpperBlue = Preferences.getInstance().getDouble("rear_upper_blue", 50);
+
+		table.putNumber("front_lower_red", frontLowerRed);
+		table.putNumber("front_lower_green", frontLowerGreen);
+		table.putNumber("front_lower_blue", frontLowerBlue);
+		table.putNumber("front_upper_red", frontUpperRed);
+		table.putNumber("front_upper_green", frontUpperGreen);
+		table.putNumber("front_upper_blue", frontUpperBlue);
+		table.putNumber("rear_lower_red", rearLowerRed);
+		table.putNumber("rear_lower_green", rearLowerGreen);
+		table.putNumber("rear_lower_blue", rearLowerBlue);
+		table.putNumber("rear_upper_red", rearUpperRed);
+		table.putNumber("rear_upper_green", rearUpperGreen);
+		table.putNumber("rear_upper_blue", rearUpperBlue);
+	}
+
 	/**
 	 * @return the Navx's current angle measurement
 	 */
@@ -130,5 +170,9 @@ public void resetNavxAngle() {
 	public void initDefaultCommand() {
 		setDefaultCommand(new DisplaySensors());
 	}
+
+	public enum Camera {
+		FRONT, REAR
+	}
 }