package org.usfirst.frc.team4828;

import edu.wpi.first.wpilibj.*;
import org.usfirst.frc.team4828.Vision.Vision;

public class Robot extends IterativeRobot {
    private Joystick driveStick;
    private DriveTrain drive;
    private Vision vision;
    private DigitalInput[] dipSwitch;
    private int autonSelect;
    private Climber climb;

    @Override
    public void robotInit() {
        super.robotInit();
        System.out.println("THE ROBOT TURNED ON");
        driveStick = new Joystick(0);
        /*drive = new DriveTrain(
                Ports.DT_FRONT_LEFT,
                Ports.DT_BACK_LEFT,
                Ports.DT_FRONT_RIGHT,
                Ports.DT_BACK_RIGHT
        );*/
        climb = new Climber(7, 8);
        dipSwitch = new DigitalInput[4];
        dipSwitch[0] = new DigitalInput(Ports.DIPSWITCH_1);
        dipSwitch[1] = new DigitalInput(Ports.DIPSWITCH_2);
        dipSwitch[2] = new DigitalInput(Ports.DIPSWITCH_3);
        dipSwitch[3] = new DigitalInput(Ports.DIPSWITCH_4);
    }

    @Override
    public void autonomousInit() {
        super.autonomousInit();
        for (int i = 0; i < 4; i++) {
            autonSelect += (dipSwitch[i].get() ? 1 : 0) * (1 << i);
        }
        System.out.println("Entering auton number " + autonSelect);
        switch (autonSelect) {
            case 0:
                // TODO: Shoot 10 fuel
                break;
            case 1:
                // TODO: Place gear on right side
                break;
            case 2:
                // TODO: Place gear on center
                break;
            case 3:
                // TODO: Place gear on left side
                break;
            case 4:
                // TODO: Shoot 10 fuel and place gear on left side
                break;
            case 5:
                // TODO: The crazy running into hopper and shooting tons of balls plan
                break;
            default:
                // Do nothing
        }
    }

    @Override
    public void autonomousPeriodic() {
        Timer.delay(.1);
        super.autonomousPeriodic();
    }

    @Override
    public void teleopInit() {
        super.teleopInit();
        System.out.println("Entering teleop...");
    }

    @Override
    public void teleopPeriodic() {
        super.teleopPeriodic();
        drive.mecanumDrive(driveStick.getX(), driveStick.getY(), driveStick.getTwist());
        if (driveStick.getRawButton(11)) {
            drive.reset();
        }
    }

    @Override
    public void testInit() {
        super.testInit();
        System.out.println("Entering test...");
        vision = new Vision(Ports.PIXY_CHANNEL);
        vision.start();
    }

    @Override
    public void testPeriodic() {
        System.out.println(vision);
        Timer.delay(0.1);
    }

    @Override
    public void disabledInit() {
        if(vision != null) {
            vision.terminate();
        }
        System.out.println("Stopping thread");
    }
}

@@ -1279,16 +1279,135 @@
 elect);%0A
+        drive.reset();%0A    %7D%0A%0A    @Override%0A    public void autonomousPeriodic() %7B%0A        super.autonomousPeriodic();%0A
         
@@ -2094,122 +2094,26 @@
     
-%7D%0A%0A    @Override%0A    public void autonomousPeriodic() %7B%0A        Timer.delay(.1);%0A        super.autonomousPeriodic(
+    Timer.delay(.1
 );%0A 

Reset navx in autonInit and move auton 'code' to autonPeriodic