@@ -0,0 +1,2878 @@
+package org.usfirst.frc.team6027.robot.commands;%0A%0Aimport edu.wpi.first.wpilibj.PIDController;%0Aimport edu.wpi.first.wpilibj.PIDOutput;%0Aimport edu.wpi.first.wpilibj.command.Command;%0Aimport edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;%0Aimport org.usfirst.frc.team6027.robot.Robot;%0A%0Apublic class ArcDriveDistance extends Command %7B%0A    private PIDController pidX;%0A    private PIDController pidY;%0A    double xOutput=0;%0A    double yOutput=0;%0A    double setpoint1=0;%0A    double setpoint2=0;%0A    public ArcDriveDistance(double radius, boolean isRighthand)%7B%0A        requires(Robot.drivetrain);%0A        requires(Robot.driveEncoders);%0A        requires(Robot.gyro);%0A        requires(Robot.ultrasonic);%0A        if (isRighthand)%7B%0A            setpoint1 = 2*Math.PI*radius;%0A            setpoint2 = 2*Math.PI*(radius-34);%0A        %7Delse%7B%0A            setpoint1 = 2*Math.PI*(radius-34);%0A            setpoint2 = 2*Math.PI*radius;%0A        %7D%0A        pidX= new PIDController(0.05, 0, 0, Robot.driveEncoders.getEncoderLeft(), new pidXOutput());%0A        if(setpoint1%3E0)%7B%0A            pidX.setInputRange(0,setpoint1+10);%0A        %7Delse%7B%0A            pidX.setInputRange(setpoint1-10,0);%0A        %7D%0A        pidX.setAbsoluteTolerance(0.5);%0A        pidX.setOutputRange(-0.6,0.6);%0A        pidX.setSetpoint(setpoint1);%0A%0A        pidY = new PIDController(0.05, 0, 0, Robot.driveEncoders.getEncoderRight(), new pidYOutput());%0A        if(setpoint2%3E0)%7B%0A            pidY.setInputRange(0,setpoint2+10);%0A        %7Delse%7B%0A            pidY.setInputRange(setpoint2-10,0);%0A        %7D%0A        pidY.setAbsoluteTolerance(0.5);%0A        pidY.setOutputRange(-0.6,0.6);%0A        pidY.setSetpoint(setpoint2);%0A    %7D%0A    @Override%0A    protected void initialize() %7B%0A    %09Robot.driveEncoders.getEncoderLeft().reset();%0A    %09Robot.gyro.reset();%0A    %09pidX.enable();%0A    %09pidY.enable();%0A    %7D%0A%0A    @Override%0A    protected void execute() %7B%0A        SmartDashboard.putNumber(%22Distance%22, Robot.driveEncoders.getEncoderLeft().getDistance());%0A        Robot.drivetrain.tankDrive(xOutput,yOutput);%0A    %7D%0A%0A    @Override%0A    protected boolean isFinished() %7B%0A        return (pidX.onTarget() && pidY.onTarget()) %7C%7C (Robot.ultrasonic.getDistance()%3C4);%0A    %7D%0A%0A    @Override%0A    protected void end() %7B%0A        pidX.disable();%0A        pidX.reset();%0A        pidY.disable();%0A        pidY.reset();%0A        Robot.drivetrain.arcadeDrive(0,0);%0A    %7D%0A%0A    @Override%0A    protected void interrupted() %7B%0A        pidX.disable();%0A        pidX.reset();%0A        pidY.disable();%0A        pidY.reset();%0A        Robot.drivetrain.arcadeDrive(0,0);%0A    %7D%0A    private class pidXOutput implements PIDOutput %7B%0A        @Override%0A        public void pidWrite(double output) %7B%0A            xOutput=output;%0A        %7D%0A    %7D%0A    private class pidYOutput implements PIDOutput %7B%0A        @Override%0A        public void pidWrite(double output) %7B%0A            yOutput=output;%0A        %7D%0A    %7D%0A%7D%0A

Add arc drive