Waypoint[] point = new Waypoint[] {
				new Waypoint(2.5,-1,45),
				new Waypoint(2,0,0),
				new Waypoint(1.5,0.5,34),
				new Waypoint(1,0,0),
				new Waypoint(0,0,0)
		};

		matchLogger.writeClean("PATHFINDER STARTING");

		Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.05, 1.7, 2.0, 60.0);
		Trajectory trajectory = Pathfinder.generate(point, config);

		TankModifier modifier = new TankModifier(trajectory).modify(0.627);

		EncoderFollower left = new EncoderFollower(modifier.getLeftTrajectory());
		EncoderFollower right = new EncoderFollower(modifier.getRightTrajectory());

		left.configureEncoder(sensorInput.getLeftDriveEncoder(), 83, 0.127);
		right.configureEncoder(sensorInput.getRightDriveEncoder(), 83, 0.127);

		left.configurePIDVA(1.0, 0.0, 0.0, 1 / 3, 0);
		right.configurePIDVA(1.0, 0.0, 0.0, 1 / 3, 0);

		double l, r, turn;

		while(!left.isFinished() || !right.isFinished()) {
			l = left.calculate(sensorInput.getLeftDriveEncoder());
			r = right.calculate(sensorInput.getRightDriveEncoder());

			turn = -(0.8 * (-1.0/80.0) * (Pathfinder.d2r(left.getHeading()) - sensorInput.getAHRSYaw()));

			matchLogger.writeClean("PATHFINDER left = " + l + " right = " + r + " turn = " + turn);

			robotOutput.tankDrive(l + turn, r - turn);
		}
		robotOutput.tankDrive(0, 0);
		robotOutput.setIntake(1.0);
		Thread.sleep(10000);

	}
}