diff --git a/.DS_Store b/.DS_Store
new file mode 100644
index 0000000..222501b
Binary files /dev/null and b/.DS_Store differ
diff --git a/2020Code/build.gradle b/2020Code/build.gradle
index 30d5cc1..9720092 100644
--- a/2020Code/build.gradle
+++ b/2020Code/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2020.2.2"
+    id "edu.wpi.first.GradleRIO" version "2020.3.2"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11
diff --git a/2020Code/src/main/java/frc/robot/Constants.java b/2020Code/src/main/java/frc/robot/Constants.java
index 70a9430..6aaeb2d 100644
--- a/2020Code/src/main/java/frc/robot/Constants.java
+++ b/2020Code/src/main/java/frc/robot/Constants.java
@@ -7,6 +7,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants.  This class should not be used for any other purpose.  All constants should be
@@ -19,5 +21,16 @@ public final class Constants {
     public static final class DebugConstants {
         public static boolean debugMode = false;
     }
-
+    public static final class DriveConstants {
+        public static final double ksVolts = 0.22;
+        public static final double kvVoltSecondsPerInch = 1.98;
+        public static final double kaVoltSecondsSquaredPerInch = 0.2;
+        public static final double kPDriveVel = 8.5;
+        public static final double kTrackwidthInches = 0.69;
+        public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthInches);
+        public static final double kMaxSpeedInchesPerSecond = 3;
+        public static final double kMaxAccelerationInchesPerSecondSquared = 3;
+        public static final double kRamseteB = 2;
+        public static final double kRamseteZeta = 0.7;
+    }
 }
diff --git a/2020Code/src/main/java/frc/robot/Robot.java b/2020Code/src/main/java/frc/robot/Robot.java
index d0ea75e..1072452 100644
--- a/2020Code/src/main/java/frc/robot/Robot.java
+++ b/2020Code/src/main/java/frc/robot/Robot.java
@@ -70,6 +70,8 @@ public void robotPeriodic() {
    */
   @Override
   public void disabledInit() {
+    frc.robot.RobotContainer.ShooterT.zeroTurretAngle();
+    frc.robot.RobotContainer.PneumaticsT.setCompressor(true);
   }
 
   @Override
@@ -81,6 +83,9 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
+    frc.robot.RobotContainer.ShooterT.zeroTurretAngle();
+    frc.robot.RobotContainer.ClimbT.setClimb(false);
+    frc.robot.RobotContainer.PneumaticsT.setCompressor(false);
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
@@ -98,12 +103,13 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
+    frc.robot.RobotContainer.ClimbT.setClimb(false);
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
     if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+      //m_autonomousCommand.cancel();
     }
   }
 
diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index 69782b0..f602cc0 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -25,16 +25,15 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
+  public static DiskControl DiskControlT = new DiskControl();
+  public static Navx NavxT = new Navx();
   public static Drivetrain DrivetrainT = new Drivetrain();
   public static Pneumatics PneumaticsT = new Pneumatics();
   public static ShooterLimelight ShooterLimelightT = new ShooterLimelight();
-  public static DiskControl DiskControlT = new DiskControl();
-  public static Navx NavxT = new Navx();
   public static Intake IntakeT = new Intake();
   public static Indexer IndexerT = new Indexer();
   public static Climb ClimbT = new Climb();
   public static Shooter ShooterT = new Shooter();
-  public static BallLimelight BallLimelightT = new BallLimelight();
   //public static Lights LightsT = new Lights();
 
   public static XboxController m_driverController = new XboxController(0);
@@ -48,6 +47,8 @@ public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
     DrivetrainT.setDefaultCommand(new DriveWithJoystick());
+    IntakeT.setDefaultCommand(new ControlIntake());
+    ShooterT.setDefaultCommand(new TurretWithJoystick());
   }
 
   /**
@@ -60,12 +61,16 @@ private void configureButtonBindings() {
     //final JoystickButton a = new JoystickButton(m_joystick, 1);
     //final JoystickButton b = new JoystickButton(m_joystick, 2);
     // final JoystickButton x = new JoystickButton(m_joystick, 3);
-    final JoystickButton y = new JoystickButton(m_joystick, 4);
+    final JoystickButton leftBumper = new JoystickButton(m_joystick, 4);
+    final JoystickButton climbButtonUp = new JoystickButton(m_driverController, 8);
+    final JoystickButton climbButtonDown = new JoystickButton(m_driverController, 7);
 
     //a.whenPressed(new GoToColor());
     //b.whenPressed(new SpinTimes());
-    // x.whenPressed(new AutoTest());
-    y.toggleWhenPressed(new TargetPort());
+    //x.whenPressed(new AutoTest());
+    leftBumper.toggleWhenPressed(new TargetPort());
+    climbButtonUp.whenPressed(new SetClimb(true));
+    climbButtonDown.whenPressed(new SetClimb(false));
   }
 
 
@@ -75,7 +80,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
-    return new AutoTest();
+    return new AutoTest(); //.withTimeout(20);
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java b/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
new file mode 100644
index 0000000..384efd7
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class AutoControlIntake extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  double m_intakeSpeed;
+  double m_indexerSpeed;
+  boolean m_intakeState;
+  public AutoControlIntake(double intakeSpeed, double indexerSpeed, boolean intakeState) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.IntakeT);
+    addRequirements(frc.robot.RobotContainer.IndexerT);
+    m_intakeSpeed = intakeSpeed;
+    m_indexerSpeed = indexerSpeed;
+    m_intakeState = intakeState;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    frc.robot.RobotContainer.IntakeT.setPiston(m_intakeState);
+    if(m_intakeSpeed < 0 && frc.robot.RobotContainer.IndexerT.ballAtEnd()){
+      frc.robot.RobotContainer.IndexerT.setMotor(0);
+    } 
+    else if(m_intakeSpeed < 0 && !frc.robot.RobotContainer.IndexerT.ballAtEnd()){
+      frc.robot.RobotContainer.IndexerT.setMotor(0);
+    }
+    else{
+      frc.robot.RobotContainer.IndexerT.setMotor(m_indexerSpeed);
+    }
+    frc.robot.RobotContainer.IntakeT.setMotor(m_intakeSpeed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest.java b/2020Code/src/main/java/frc/robot/commands/AutoTest.java
index 1dfb08f..2be170a 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest.java
@@ -11,11 +11,64 @@
 
 public class AutoTest extends SequentialCommandGroup {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  public static final double DriveDist = 150; //scale 106%
+  public static final double TurnAngle = 90;
+  
+
   public AutoTest() {
     addCommands(
-      new DriveDistance(100),
-      new TurnAngle(-90),
-      new DriveArc(40, 60)
+      new DriveDistance(150), //move forward first step
+      new DriveArc(30, 360)
+      //new TurnAngle(90), //turn right 90 degrees 
+
+      // new DriveDistance(50), //move forward 
+
+      // new TurnAngle(90), //turn right 90 degrees
+
+      // new DriveDistance(70), //drive forward
+
+      // new TurnAngle(90), //turn right 90 degrees
+
+      // new DriveDistance(70), //drive forward
+
+      // new TurnAngle(90), //turn right 90 degrees
+
+      // new DriveDistance(175), //move forward 
+
+      // new TurnAngle(-90), //turn left 90 degrees
+
+      // new DriveDistance(50), //drive forward
+
+      // new TurnAngle(-90), //turn left 90 degrees 
+
+      // new DriveDistance(50), //drive forward 
+
+      // new TurnAngle(-90), //turn left 90 degrees
+
+      // new DriveDistance(50), //move forward
+
+      // new TurnAngle(-90), //turn left 90 
+
+      // new DriveDistance(100), //move forward
+
+      // new TurnAngle(90), //turn right 90 
+
+      // new DriveDistance(50), //move forward
+
+      // new TurnAngle(-90), //turn left 90
+
+      // new DriveDistance(50), //move forward
+
+      // new TurnAngle(-90), //turn left 
+
+      // new DriveDistance(50), //move forward
+      
+      // new TurnAngle(-90), //turn left
+
+      // new DriveDistance(250) //back to the start!
+
+     //  new DriveArc(20, 360)
     );
   }
 }
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
new file mode 100644
index 0000000..18eae64
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+
+public class AutoTest2 extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+    public static final double DriveDist = 100; 
+  
+    public AutoTest2() {
+        addCommands(
+            new DriveArc(55, -68), //90 degree circle counterclockwise
+            new DriveArc(15, 59), //90 degree circle clockwise
+            new DriveDistance(100), //move forward 125 inches
+            new DriveArc(45, 65),  //90 degree circle clockwise
+            new DriveArc(45, 280), //full circle 360 degrees 
+            new DriveArc(45, -65), //90 degree circle counterclockwise
+            new DriveDistance(100), //move forward 125 inches
+            new DriveArc(15, 59), //90 degree circle clockwise
+            new DriveArc(55, -68)  //90 degree circle counterclockwise
+            ); 
+        }   
+    }
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
new file mode 100644
index 0000000..b52d163
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+
+public class AutoTest3 extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+    public static final double DriveDistance = 65; 
+  
+    public AutoTest3() {
+        addCommands(
+           new DriveArc(45, -90), //90 degree circle counterclowise
+           new TurnAngle(171), //170 degrees clockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(40, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new TurnAngle(180), //Turn 180 degrees
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(45, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new TurnAngle(180), //Turn 180 degrees
+           new DriveArc(35, -90) //90 degrees counterclockwise
+            ); 
+        }   
+    }
diff --git a/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java b/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java
new file mode 100644
index 0000000..5fc5f1b
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+
+public class BarrelRunTest extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  
+    public BarrelRunTest() {
+        addCommands(
+                new DriveDistance(90), //move forward 7.5 feet
+                new DriveArc(30, 340), //do a full circle clockwise
+                new DriveDistance(90), //move forward 90 inches
+                new DriveArc(40, -315), //rotate 315 degrees counterclockwise
+                new DriveDistance(85), //move forward 85 inches
+                new DriveArc(22.5, -225), //rotate 225 degrees counterclockwise
+                new DriveDistance(240) //move forward 240 feet
+            ); 
+        }   
+    }
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/ChooseAutoTest.java b/2020Code/src/main/java/frc/robot/commands/ChooseAutoTest.java
new file mode 100644
index 0000000..10ccb3c
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/ChooseAutoTest.java
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+public class ChooseAutoTest extends CommandBase {
+        private final Command m_BarrelRunTest = new BarrelRunTest();
+        private final Command m_SlalomPath = new AutoTest2();
+        private final Command m_BouncePath = new AutoTest3();
+
+        SendableChooser<Command> pathChooser = new SendableChooser<Command>();
+
+    public ChooseAutoTest(){
+        pathChooser.setDefaultOption("Barrel Run", m_BarrelRunTest);
+        pathChooser.addOption("Slalom Path", m_SlalomPath);
+        pathChooser.addOption("Bounce Path", m_BouncePath);
+        SmartDashboard.putData(pathChooser);
+        
+    }
+}
+
+
+
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
new file mode 100644
index 0000000..892bca2
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class ControlIntake extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  boolean prevButton6;
+  public ControlIntake() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.IntakeT);
+    addRequirements(frc.robot.RobotContainer.IndexerT);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(frc.robot.RobotContainer.m_joystick.getRawButton(2)) {
+      frc.robot.RobotContainer.IntakeT.setMotor(-0.6);
+    }
+    else {
+      if(frc.robot.RobotContainer.m_joystick.getRawButton(3)) {
+        frc.robot.RobotContainer.IntakeT.setMotor(0.4);
+      }
+      else {
+        frc.robot.RobotContainer.IntakeT.setMotor(0);
+      }
+    }
+    //frc.robot.RobotContainer.IntakeT.setPiston(frc.robot.RobotContainer.m_joystick.getRawButton(6));
+    if(!frc.robot.RobotContainer.m_joystick.getRawButton(6)) prevButton6 = true;
+    if(frc.robot.RobotContainer.m_joystick.getRawButton(6) && prevButton6){
+      frc.robot.RobotContainer.IntakeT.setPiston(!frc.robot.RobotContainer.IntakeT.getPiston());
+      prevButton6 = false;
+    }
+    if(frc.robot.RobotContainer.m_joystick.getRawButton(1)) {
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.8);
+    }
+    else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
+      frc.robot.RobotContainer.IndexerT.setMotor(0.8);
+    }
+    else if(frc.robot.RobotContainer.m_joystick.getRawButton(2) &&frc.robot.RobotContainer.IndexerT.ballAtEnd()){
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.9);
+    }
+    else{
+      frc.robot.RobotContainer.IndexerT.setMotor(0);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/2020Code/src/main/java/frc/robot/commands/DriveArc.java b/2020Code/src/main/java/frc/robot/commands/DriveArc.java
index f2936ea..6bc4ca4 100644
--- a/2020Code/src/main/java/frc/robot/commands/DriveArc.java
+++ b/2020Code/src/main/java/frc/robot/commands/DriveArc.java
@@ -9,7 +9,6 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpiutil.math.MathUtil;
 
 /**
@@ -39,18 +38,16 @@ public DriveArc(double radius, double angle) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    SmartDashboard.putNumber("ArcStage", 1);
     pid.setTolerance(2, 10);
     frc.robot.RobotContainer.DrivetrainT.allowDrive(false);
     frc.robot.RobotContainer.DrivetrainT.resetEncoders();
     output = 0.6 / (m_radius+wheelDistance/2) * (m_radius-wheelDistance/2);
-    SmartDashboard.putNumber("ArcStage", 2);
+    System.out.println("hello world");
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    SmartDashboard.putNumber("ArcStage", 3);
     if(m_angle > 0){
       target = (frc.robot.RobotContainer.DrivetrainT.getLeftVelocity() / (m_radius + (wheelDistance/2))) * (m_radius - (wheelDistance/2));
       output += MathUtil.clamp(pid.calculate(frc.robot.RobotContainer.DrivetrainT.getRightVelocity(),target), -0.6, 0.6);
@@ -61,7 +58,6 @@ public void execute() {
       output += MathUtil.clamp(pid.calculate(frc.robot.RobotContainer.DrivetrainT.getLeftVelocity(), target), -0.6, 0.6);
       frc.robot.RobotContainer.DrivetrainT.tankDrive(output, 0.6);
     }
-    SmartDashboard.putNumber("ArcStage", 4);
   }
 
   // Called once the command ends or is interrupted.
@@ -69,13 +65,12 @@ public void execute() {
   public void end(boolean interrupted) {
     frc.robot.RobotContainer.DrivetrainT.arcadeDrive(0,0);
     frc.robot.RobotContainer.DrivetrainT.allowDrive(true);
+    System.out.println("goodbye world");
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    SmartDashboard.putNumber("Goal", m_radius * Math.PI * 2 * m_angle/360);
-    SmartDashboard.putNumber("At", frc.robot.RobotContainer.DrivetrainT.getEncoderAverage());
     return Math.abs(frc.robot.RobotContainer.DrivetrainT.getEncoderAverage()) >= m_radius * Math.PI * 2 * Math.abs(m_angle)/360;
   }
 }
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/DriveDistance.java b/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
index 367aaaa..ceed2cd 100644
--- a/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
+++ b/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
@@ -23,8 +23,9 @@ public class DriveDistance extends CommandBase {
    *
    * @param subsystem The subsystem used by this command.
    */
+  double speed = 0.4;
   double m_distance = 0;
-  PIDController pid = new PIDController(0.02, 0.001, 0);
+  PIDController pid = new PIDController(0.05, 0.005, 0);
   public DriveDistance(double distance) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(frc.robot.RobotContainer.DrivetrainT);
@@ -34,7 +35,7 @@ public DriveDistance(double distance) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pid.setTolerance(2, 10);
+    pid.setTolerance(5, .4);
     frc.robot.RobotContainer.DrivetrainT.allowDrive(false);
     frc.robot.RobotContainer.DrivetrainT.resetEncoders();
     frc.robot.RobotContainer.NavxT.resetHeading();
diff --git a/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java b/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
new file mode 100644
index 0000000..98a64a7
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class DriveDistanceSlow extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  double m_distance = 0;
+  PIDController pid = new PIDController(0.04, 0.003, 0);
+  public DriveDistanceSlow(double distance) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.DrivetrainT);
+    m_distance = distance;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    pid.setTolerance(5, 10);
+    frc.robot.RobotContainer.DrivetrainT.allowDrive(false);
+    frc.robot.RobotContainer.DrivetrainT.resetEncoders();
+    frc.robot.RobotContainer.NavxT.resetHeading();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    frc.robot.RobotContainer.DrivetrainT.arcadeDrive(MathUtil.clamp(pid.calculate(frc.robot.RobotContainer.DrivetrainT.getEncoderAverage(), m_distance), -0.4, 0.4), 0/*frc.robot.RobotContainer.NavxT.getHeading() * 0.1*/);
+    SmartDashboard.putNumber("Off", frc.robot.RobotContainer.DrivetrainT.getEncoderAverage());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    frc.robot.RobotContainer.DrivetrainT.arcadeDrive(0,0);
+    frc.robot.RobotContainer.DrivetrainT.allowDrive(true);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return pid.atSetpoint();
+  }
+}
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/GoToBall.java b/2020Code/src/main/java/frc/robot/commands/GoToBall.java
deleted file mode 100644
index 096b4ca..0000000
--- a/2020Code/src/main/java/frc/robot/commands/GoToBall.java
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-/**
- * An example command that uses an example subsystem.
- */
-public class GoToBall extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  double angle = 0;
-  double size = 0;
-  boolean followOffscreen;
-  double lastKnownPosition;
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public GoToBall(boolean followOffscreenT) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(frc.robot.RobotContainer.DrivetrainT);
-    followOffscreen = followOffscreenT;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    frc.robot.RobotContainer.BallLimelightT.setPipeline(1);
-    frc.robot.RobotContainer.DrivetrainT.allowDrive(false);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    angle = frc.robot.RobotContainer.BallLimelightT.getXSkew();
-    size = frc.robot.RobotContainer.BallLimelightT.getYellowBoxHorizontal();
-    float speed = 0;
-    float rotation = 0;
-    //If angle is high, turn right at propprtional speed
-    if (angle > 1) {
-        rotation = (float)(angle/22.5 * 0.5 + 0.5);
-    }
-    //If angle is low, turn left at proportional speed
-    else if (angle < -1) {
-        rotation = (float)(angle/22.5 * 0.5 - 0.5);
-    }
-    //If the size is less than 120, move closer.
-    if (size < 120) {
-        speed = (float)0.6;
-    }
-    //Go to cone if it is there
-    if(frc.robot.RobotContainer.BallLimelightT.limelightHasTarget()) {
-      frc.robot.RobotContainer.DrivetrainT.arcadeDrive(speed, rotation);
-    }
-    //if the cone isnt there, but we are in follow offscreen mode, 
-    //and the cone was most recently at an angle near the edge, turn in that direction without moving forward.
-    if(followOffscreen) {
-        if(lastKnownPosition < -15 && !frc.robot.RobotContainer.BallLimelightT.limelightHasTarget()) frc.robot.RobotContainer.DrivetrainT.arcadeDrive(0, -0.5);
-        if(lastKnownPosition < 15 && !frc.robot.RobotContainer.BallLimelightT.limelightHasTarget()) frc.robot.RobotContainer.DrivetrainT.arcadeDrive(0, 0.5);
-        //Track last known angle
-        lastKnownPosition = angle;
-    }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    frc.robot.RobotContainer.DrivetrainT.arcadeDrive(0, 0);
-    frc.robot.RobotContainer.DrivetrainT.allowDrive(true);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return size > 120;
-  }
-}
diff --git a/2020Code/src/main/java/frc/robot/commands/GoToColor.java b/2020Code/src/main/java/frc/robot/commands/GoToColor.java
index 1184dec..77d4cd5 100644
--- a/2020Code/src/main/java/frc/robot/commands/GoToColor.java
+++ b/2020Code/src/main/java/frc/robot/commands/GoToColor.java
@@ -47,19 +47,16 @@ public void execute() {
     else if (frc.robot.RobotContainer.DiskControlT.senseColor().green > frc.robot.RobotContainer.DiskControlT.senseColor().blue && frc.robot.RobotContainer.DiskControlT.senseColor().blue > frc.robot.RobotContainer.DiskControlT.senseColor().red) detected = 3;
     else if (frc.robot.RobotContainer.DiskControlT.senseColor().red > frc.robot.RobotContainer.DiskControlT.senseColor().green && frc.robot.RobotContainer.DiskControlT.senseColor().green > frc.robot.RobotContainer.DiskControlT.senseColor().blue || frc.robot.RobotContainer.DiskControlT.senseColor().green < 0.5) detected = 0;
     else if (frc.robot.RobotContainer.DiskControlT.senseColor().green > frc.robot.RobotContainer.DiskControlT.senseColor().red && frc.robot.RobotContainer.DiskControlT.senseColor().red > frc.robot.RobotContainer.DiskControlT.senseColor().blue) detected = 1;
-    // if (detected != -1 && detected != (int)colorChooser.getSelected()) {
-    //   frc.robot.RobotContainer.DiskControlT.DiskMotorController(.2);
-    // }
     if (map[detected][(int)colorChooser.getSelected()].equals("CW")) {
-      frc.robot.RobotContainer.DiskControlT.DiskMotorController(-.25);
+      frc.robot.RobotContainer.IntakeT.setMotor(-.25);
     }
-    else frc.robot.RobotContainer.DiskControlT.DiskMotorController(.25);
+    else frc.robot.RobotContainer.IntakeT.setMotor(.25);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    frc.robot.RobotContainer.DiskControlT.DiskMotorController(0);
+    frc.robot.RobotContainer.IntakeT.setMotor(0);
       
   }
 
diff --git a/2020Code/src/main/java/frc/robot/commands/SetClimb.java b/2020Code/src/main/java/frc/robot/commands/SetClimb.java
new file mode 100644
index 0000000..9c07148
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/SetClimb.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class SetClimb extends CommandBase {
+  boolean m_state;
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public SetClimb(boolean state) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.ClimbT);
+    m_state = state;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      frc.robot.RobotContainer.ClimbT.setClimb(m_state);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}
diff --git a/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java b/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
new file mode 100644
index 0000000..5b4cd86
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class SixBallSimple extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  public SixBallSimple() {
+    addCommands(
+      new ParallelCommandGroup(
+        new AutoControlIntake(0, 0, true).withTimeout(1),
+        new TurretToAngle(150).withTimeout(1)
+      ),
+      new TargetPortAuto(false, true).withTimeout(2),
+      new TargetPortAuto(true, true).withTimeout(0.5),
+      new ParallelCommandGroup(
+        new TargetPortAuto(true, true).withTimeout(1.5),
+        new AutoControlIntake(0, -0.8, true).withTimeout(1.5)
+      ),
+      new TargetPortAuto(false, false).withTimeout(0.1),
+      new AutoControlIntake(-0.6, 0, true).withTimeout(0.1),
+      new DriveDistanceSlow(110).withTimeout(5),
+      new AutoControlIntake(0, 0, true).withTimeout(0.1),
+      new ParallelCommandGroup(
+        new DriveDistance(-110).withTimeout(3),
+        new TargetPortAuto(false, true).withTimeout(3)
+      ),
+      new TargetPortAuto(true, true).withTimeout(0.5),
+      new ParallelCommandGroup(
+        new TargetPortAuto(true, true).withTimeout(3),
+        new AutoControlIntake(0, -0.8, true).withTimeout(3)
+      ),
+      new TargetPortAuto(false, false).withTimeout(0.1)
+    );
+  }
+}
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/SpinTimes.java b/2020Code/src/main/java/frc/robot/commands/SpinTimes.java
index 7236783..5b72f4b 100644
--- a/2020Code/src/main/java/frc/robot/commands/SpinTimes.java
+++ b/2020Code/src/main/java/frc/robot/commands/SpinTimes.java
@@ -39,7 +39,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    frc.robot.RobotContainer.DiskControlT.DiskMotorController(0.25);
+    frc.robot.RobotContainer.IntakeT.setMotor(0.25);
     if (frc.robot.RobotContainer.DiskControlT.senseColor().blue > frc.robot.RobotContainer.DiskControlT.senseColor().green && frc.robot.RobotContainer.DiskControlT.senseColor().green > frc.robot.RobotContainer.DiskControlT.senseColor().red || frc.robot.RobotContainer.DiskControlT.senseColor().blue > 0.27) detected =2;
     else if (frc.robot.RobotContainer.DiskControlT.senseColor().green > frc.robot.RobotContainer.DiskControlT.senseColor().blue && frc.robot.RobotContainer.DiskControlT.senseColor().blue > frc.robot.RobotContainer.DiskControlT.senseColor().red) detected = 3;
     else if (frc.robot.RobotContainer.DiskControlT.senseColor().red > frc.robot.RobotContainer.DiskControlT.senseColor().green && frc.robot.RobotContainer.DiskControlT.senseColor().green > frc.robot.RobotContainer.DiskControlT.senseColor().blue || frc.robot.RobotContainer.DiskControlT.senseColor().green < 0.5) detected = 0;
@@ -54,7 +54,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    frc.robot.RobotContainer.DiskControlT.DiskMotorController(0);
+    frc.robot.RobotContainer.IntakeT.setMotor(0);
   }
 
   // Returns true when the command should end.
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index ee0e413..8f5352d 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -48,17 +48,20 @@ public void initialize() {
   @Override
   public void execute() {
     // turretAngle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + frc.robot.RobotContainer.ShooterT.getAngle();
-    distance = (Math.sqrt(frc.robot.RobotContainer.ShooterLimelightT.getArea()) / Math.sqrt(0.618)) * 55.8752 * Math.sin(Math.toRadians(frc.robot.RobotContainer.ShooterLimelightT.getYSkew())); //needs testing for equation
+    //distance = (Math.sqrt(frc.robot.RobotContainer.ShooterLimelightT.getArea()) / Math.sqrt(0.618)) * 55.8752 * Math.sin(Math.toRadians(frc.robot.RobotContainer.ShooterLimelightT.getYSkew())); //needs testing for equation
+    distance = (95 - 17) / Math.tan((frc.robot.RobotContainer.ShooterLimelightT.getYSkew() + 22) * Math.PI/ 180);
     SmartDashboard.putNumber("Distance to target", distance);
+    //frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
     // robotVelocity = frc.robot.RobotContainer.DrivetrainT.getVelocity();
     // ballVelocity = distance; //needs testing for equation
     // predictor = 0;//Math.asin((robotVelocity * Math.sin(Math.toRadians(turretAngle))) / ballVelocity);
     // angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + predictor;
-    // if (Math.abs(angle) > 0.1) frc.robot.RobotContainer.ShooterT.setTurretSpeed(angle * 0.1); //needs testing for equation
-    // //frc.robot.RobotContainer.ShooterT.setShooterSpeed(distance * 2 / 20); //needs testing for equation
-    if(true) {
-      angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew();
-      if (Math.abs(angle) > 0.05) frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
+    if(frc.robot.RobotContainer.m_joystick.getRawButton(5)) frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
+    else frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
+    if(frc.robot.RobotContainer.ShooterLimelightT.limelightHasTarget()) {
+      if(!frc.robot.RobotContainer.m_joystick.getRawButton(1)) frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00529*distance-0.0000113*(Math.pow(distance, 2)));
+      angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + 1.2;
+      frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
     }
   }
 
@@ -67,7 +70,7 @@ public void execute() {
   public void end(boolean interrupted) {
     frc.robot.RobotContainer.ShooterLimelightT.setLEDMode(0);
     frc.robot.RobotContainer.ShooterT.setTurretSpeed(0);
-    frc.robot.RobotContainer.ShooterT.setShooterSpeed(0);
+    frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
     frc.robot.RobotContainer.ShooterLimelightT.setPipeline(0);
   }
 
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
new file mode 100644
index 0000000..bc14770
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class TargetPortAuto extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  double angle;
+  double distance;
+  double predictor;
+  double robotVelocity;
+  double ballVelocity;
+  double turretAngle;
+  long time;
+  boolean m_aim;
+  boolean m_rev;
+  PIDController pid = new PIDController(0.1, 0, 0.0);
+  public TargetPortAuto(boolean aim, boolean rev) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.ShooterT);
+    m_aim = aim;
+    m_rev = rev;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    frc.robot.RobotContainer.ShooterLimelightT.setLEDMode(3);
+    angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew();
+    frc.robot.RobotContainer.ShooterLimelightT.setPipeline(1);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    distance = (95 - 17) / Math.tan((frc.robot.RobotContainer.ShooterLimelightT.getYSkew() + 22) * Math.PI/ 180);
+    if(m_rev) frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
+    else frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
+    frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.005299*distance-0.0000113*(Math.pow(distance, 2)));
+    if(frc.robot.RobotContainer.ShooterLimelightT.limelightHasTarget() && m_aim) {
+      angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + 1.2;
+      frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    frc.robot.RobotContainer.ShooterLimelightT.setLEDMode(0);
+    frc.robot.RobotContainer.ShooterT.setTurretSpeed(0);
+    frc.robot.RobotContainer.ShooterLimelightT.setPipeline(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/2020Code/src/main/java/frc/robot/commands/ThreeBallClose.java b/2020Code/src/main/java/frc/robot/commands/ThreeBallClose.java
new file mode 100644
index 0000000..a377d1c
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/ThreeBallClose.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class ThreeBallClose extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  public ThreeBallClose() {
+    addCommands(
+      new AutoControlIntake(0, 0, true).withTimeout(1),
+      new ParallelCommandGroup(
+        new DriveDistance(10),
+        new TargetPortAuto(false, true).withTimeout(2)
+      ),
+      new TargetPortAuto(true, true).withTimeout(0.5),
+      new ParallelCommandGroup(
+        new TargetPortAuto(true, true).withTimeout(3),
+        new AutoControlIntake(0, -0.8, true).withTimeout(3)
+      ),
+      new AutoControlIntake(0, 0, true).withTimeout(0.1),
+      new TargetPortAuto(false, false).withTimeout(0.1)
+    );
+  }
+}
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/ThreeBallFar.java b/2020Code/src/main/java/frc/robot/commands/ThreeBallFar.java
new file mode 100644
index 0000000..ae6ef60
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/ThreeBallFar.java
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class ThreeBallFar extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  public ThreeBallFar() {
+    addCommands(
+      new AutoControlIntake(0, 0, true).withTimeout(1),
+      new ParallelCommandGroup(
+        new DriveDistance(-20),
+        new TargetPortAuto(false, true).withTimeout(2)
+      ),
+      new TargetPortAuto(true, true).withTimeout(0.5),
+      new ParallelCommandGroup(
+        new TargetPortAuto(true, true).withTimeout(3),
+        new AutoControlIntake(0, -0.8, true).withTimeout(3)
+      ),
+      new AutoControlIntake(0, 0, true).withTimeout(0.1),
+      new TargetPortAuto(false, false).withTimeout(0.1)
+    );
+  }
+}
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java b/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
new file mode 100644
index 0000000..dadb4a2
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.controller.PIDController;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class TurretToAngle extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  double m_angle;
+  PIDController pid = new PIDController(0.01, 0.00, 0.0);
+  public TurretToAngle(double angle) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.ShooterT);
+    m_angle = angle;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    pid.setTolerance(5, 10);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    frc.robot.RobotContainer.ShooterT.setTurretSpeed(-pid.calculate(frc.robot.RobotContainer.ShooterT.getAngle(), m_angle));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return pid.atSetpoint();
+  }
+}
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
new file mode 100644
index 0000000..753205c
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class TurretWithJoystick extends CommandBase {
+  double hoodOut;
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public TurretWithJoystick() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.ShooterT);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      frc.robot.RobotContainer.ShooterT.setTurretSpeed(-frc.robot.RobotContainer.m_joystick.getRawAxis(0));
+      hoodOut = frc.robot.RobotContainer.ShooterT.getHood();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    frc.robot.RobotContainer.ShooterT.setTurretSpeed(-frc.robot.RobotContainer.m_joystick.getRawAxis(0));
+    if(frc.robot.RobotContainer.m_joystick.getRawButton(5)) frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
+    else if(frc.robot.RobotContainer.m_joystick.getRawButton(7)) frc.robot.RobotContainer.ShooterT.setShooterVoltage(-9);
+    else frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
+    frc.robot.RobotContainer.ShooterT.setHood(hoodOut);
+    if(Math.abs(frc.robot.RobotContainer.m_joystick.getRawAxis(5)) > 0.1) hoodOut -= frc.robot.RobotContainer.m_joystick.getRawAxis(5) * 0.003;
+    hoodOut = MathUtil.clamp(hoodOut, 0.0, 0.7);
+    SmartDashboard.putNumber("Hood pos", frc.robot.RobotContainer.ShooterT.getHood());
+    SmartDashboard.putNumber("Hood out", hoodOut);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/2020Code/src/main/java/frc/robot/subsystems/BallLimelight.java b/2020Code/src/main/java/frc/robot/subsystems/BallLimelight.java
deleted file mode 100644
index 7b39b20..0000000
--- a/2020Code/src/main/java/frc/robot/subsystems/BallLimelight.java
+++ /dev/null
@@ -1,103 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc.robot.subsystems;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.NetworkTableEntry;
-
-public class BallLimelight extends SubsystemBase {
-  /**
-   * Creates a new ExampleSubsystem.
-   */
-  public BallLimelight() {
-
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-  protected NetworkTable limeTable = NetworkTableInstance.getDefault().getTable("limelight");
-    
-    //Has target
-    protected NetworkTableEntry tv = limeTable.getEntry("tv");
-    //Horizontal Offset
-    protected NetworkTableEntry tx = limeTable.getEntry("tx");
-    //Vertical Offset
-    protected NetworkTableEntry ty = limeTable.getEntry("ty");
-    //Amount of 
-    protected NetworkTableEntry ta = limeTable.getEntry("ta");
-    //Skew
-    protected NetworkTableEntry ts = limeTable.getEntry("ts");
-    //Latency
-    protected NetworkTableEntry tl = limeTable.getEntry("tl");
-    //Short side of blue box
-    protected NetworkTableEntry tshort = limeTable.getEntry("tshort");
-    //Long side of blue box
-    protected NetworkTableEntry tlong = limeTable.getEntry("tlong");
-    //Horizontal of yellow box
-    protected NetworkTableEntry thor = limeTable.getEntry("thor");
-    //Vertical of yellow box
-    protected NetworkTableEntry tvert = limeTable.getEntry("tvert");
-
-    public void setLEDMode(double mode) {
-        if (mode == 0 && getLEDMode() != 0) {
-            limeTable.getEntry("ledMode").setNumber(0);
-        } else if (mode == 1 && getLEDMode() != 1) {
-            limeTable.getEntry("ledMode").setNumber(1);
-        } else if (mode == 2 && getLEDMode() != 2) {
-            limeTable.getEntry("ledMode").setNumber(2);
-        } else if (mode == 3 && getLEDMode() != 3) {
-            limeTable.getEntry("ledMode").setNumber(3);
-        }
-    }
-
-    public double getLEDMode() {
-		final double LEDMode = limeTable.getEntry("ledMode").getDouble(1);
-		return LEDMode;
-    }
-    /*private void debugLimeLED() {
-        System.out.println(limeTable.getEntry("ledMode").getDouble(1));
-    }*/
-    public double getXSkew() {
-        return tx.getDouble(0);
-    }
-    public double getYSkew() {
-        return ty.getDouble(0);
-    }
-    public boolean limelightHasTarget() {
-        if(tv.getDouble(0) == 1.0) return true;
-        else return false;
-    }
-    public double getAreaPercent() {
-        return ta.getDouble(0);
-    }
-    public double getArea() {
-        return (ta.getDouble(0) / 100) * 76800;
-    }
-    public double getLatency() {
-        return tl.getDouble(0);
-    }
-    public double getBlueBoxLong() {
-        return tlong.getDouble(0);
-    }
-    public double getBlueBoxShort() {
-        return tshort.getDouble(0);
-    }
-    public double getYellowBoxHorizontal() {
-        return thor.getDouble(0);
-    }
-    public double getYellowBoxVertical() {
-        return tvert.getDouble(0);
-    }
-    public void setPipeline(int pipeline) {
-        limeTable.getEntry("pipeline").setNumber(pipeline);
-    }
-}
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Climb.java b/2020Code/src/main/java/frc/robot/subsystems/Climb.java
index 1cfc015..3207a1b 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Climb.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Climb.java
@@ -9,19 +9,27 @@
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.Solenoid;
+import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 
 public class Climb extends SubsystemBase {
   /**
    * Creates a new ExampleSubsystem.
    */
   private Solenoid piston;
+  WPI_VictorSPX Winch = new WPI_VictorSPX(21);
   public Climb() {
-    piston = new Solenoid(30, 3);
+    piston = new Solenoid(0, 2);
     addChild("piston", piston);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    if(frc.robot.RobotContainer.m_driverController.getBButton()) Winch.set(1);
+    else Winch.set(0);
+  }
+
+  public void setClimb(boolean state){
+    piston.set(state);
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java b/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
index 57b6a24..d61238a 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
@@ -12,8 +12,6 @@
 import com.revrobotics.ColorSensorV3;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.I2C;
-import com.ctre.phoenix.motorcontrol.*;
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import edu.wpi.first.wpilibj.Solenoid;
 import frc.robot.Constants.DebugConstants;
 // import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -22,23 +20,12 @@ public class DiskControl extends SubsystemBase {
   /**
    * Creates a new ExampleSubsystem.
    */
-    TalonSRX diskWheel = new TalonSRX(21);
     private final I2C.Port i2cPort = I2C.Port.kOnboard;
-
     private final ColorSensorV3 m_colorSensor = new ColorSensorV3(i2cPort);
-
-    // SendableChooser<Integer> colorChooser = new SendableChooser<Integer>();
-
     private Solenoid piston;
 
   public DiskControl() {
-    diskWheel.set(ControlMode.PercentOutput, 0);
-    // colorChooser.addOption("Blue", 0);
-		// colorChooser.addOption("Green", 1);
-		// colorChooser.addOption("Red", 2);
-    // colorChooser.addOption("Yellow", 3);
-    // SmartDashboard.putData("Color Chooser", colorChooser);
-    piston = new Solenoid(30, 2);
+    piston = new Solenoid(0, 3);
     addChild("piston", piston);
   }
 
@@ -55,11 +42,8 @@ public void periodic() {
       else if (detectedColor.green > detectedColor.red && detectedColor.red > detectedColor.blue) SmartDashboard.putString("DetectedColor", "Yellow");
     }
   }
+  
   public Color senseColor(){
     return m_colorSensor.getColor();
-  
-  }
-  public void DiskMotorController(double speed){
-    diskWheel.set(ControlMode.PercentOutput, speed);
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
index ce3931e..b30072a 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -12,11 +12,10 @@
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-// import edu.wpi.first.wpilibj.GenericHID.Hand;
-// import edu.wpi.first.wpilibj.Solenoid;
-// import edu.wpi.first.wpilibj.Spark;
-// import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
 
 public class Drivetrain extends SubsystemBase {
   private CANSparkMax rightMaster;
@@ -34,6 +33,8 @@ public class Drivetrain extends SubsystemBase {
   private CANEncoder leftFollowerEncoder2;
 
   private DifferentialDrive diffDrive;
+  
+  private final DifferentialDriveOdometry m_odometry;
 
   double workingSpeed = 1;
   double demoSpeed = 0.5;
@@ -48,53 +49,58 @@ public class Drivetrain extends SubsystemBase {
   double rotationScale = 0;
   //Direct driving varibles
   boolean drivingEnabled = true;
-  double encoderFactor = 268/182.1;
-  
+  double encoderFactor = 268/182.1;  
   /**
    * Creates a new ExampleSubsystem.
    */
   public Drivetrain() {
     rightMaster = new CANSparkMax(10, MotorType.kBrushless);
+    rightMaster.restoreFactoryDefaults();
     rightMaster.setInverted(true);
     rightMasterEncoder = rightMaster.getEncoder();
     rightMasterEncoder.setPositionConversionFactor(encoderFactor);
     rightMasterEncoder.setVelocityConversionFactor(encoderFactor);
-    rightMaster.setOpenLoopRampRate(0.1);
-
+    rightMaster.setOpenLoopRampRate(0.25);
+  
     rightFollower1 = new CANSparkMax(11, MotorType.kBrushless);
+    rightFollower1.restoreFactoryDefaults();
     rightFollower1.setInverted(true);
     rightFollowerEncoder1 = rightFollower1.getEncoder();
     rightFollowerEncoder1.setPositionConversionFactor(encoderFactor);
     rightFollowerEncoder1.setVelocityConversionFactor(encoderFactor);
-    rightFollower1.setOpenLoopRampRate(0.1);
+    rightFollower1.setOpenLoopRampRate(0.25);
 
     rightFollower2 = new CANSparkMax(12, MotorType.kBrushless);
+    rightFollower2.restoreFactoryDefaults();
     rightFollower2.setInverted(true);
     rightFollowerEncoder2 = rightFollower2.getEncoder();
     rightFollowerEncoder2.setPositionConversionFactor(encoderFactor);
     rightFollowerEncoder2.setVelocityConversionFactor(encoderFactor);
-    rightFollower2.setOpenLoopRampRate(0.1);
+    rightFollower2.setOpenLoopRampRate(0.25);
 
     leftMaster = new CANSparkMax(15, MotorType.kBrushless);
+    leftMaster.restoreFactoryDefaults();
     leftMaster.setInverted(true);
     leftMasterEncoder = leftMaster.getEncoder();
     leftMasterEncoder.setPositionConversionFactor(encoderFactor);
     leftMasterEncoder.setVelocityConversionFactor(encoderFactor);
-    leftMaster.setOpenLoopRampRate(0.1);
+    leftMaster.setOpenLoopRampRate(0.25);
     
     leftFollower1 = new CANSparkMax(14, MotorType.kBrushless);
+    leftFollower1.restoreFactoryDefaults();
     leftFollower1.setInverted(true);
     leftFollowerEncoder1 = leftFollower1.getEncoder();
     leftFollowerEncoder1.setPositionConversionFactor(encoderFactor);
     leftFollowerEncoder1.setVelocityConversionFactor(encoderFactor);
-    leftFollower1.setOpenLoopRampRate(0.1);
+    leftFollower1.setOpenLoopRampRate(0.25);
 
     leftFollower2 = new CANSparkMax(13, MotorType.kBrushless);
+    leftFollower2.restoreFactoryDefaults();
     leftFollower2.setInverted(true);
     leftFollowerEncoder2 = leftFollower2.getEncoder();
     leftFollowerEncoder2.setPositionConversionFactor(encoderFactor);
     leftFollowerEncoder2.setVelocityConversionFactor(encoderFactor);
-    leftFollower2.setOpenLoopRampRate(0.1);
+    leftFollower2.setOpenLoopRampRate(0.25);
     
 
     leftFollower1.follow(leftMaster);
@@ -107,45 +113,30 @@ public Drivetrain() {
     diffDrive.setSafetyEnabled(false);
     diffDrive.setExpiration(0.1);
     diffDrive.setMaxOutput(1.0);
+
+    frc.robot.RobotContainer.NavxT.resetHeading();
+    m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(frc.robot.RobotContainer.NavxT.getHeading()));
   }
 
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    SmartDashboard.putNumber("Left Encoders", getLeftEncoders());
-    SmartDashboard.putNumber("Right Encoders", getRightEncoders());
-    SmartDashboard.putNumber("Avg Encoders", getEncoderAverage());
+    m_odometry.update(Rotation2d.fromDegrees(frc.robot.RobotContainer.NavxT.getHeading()), getLeftEncoders(), getRightEncoders());
   }
 
   public void arcadeDrive(double speed, double rotation) {
-    SmartDashboard.putNumber("Drive Speed", speed);
-    SmartDashboard.putNumber("Rot Speed", rotation);
     diffDrive.arcadeDrive(speed*maxSpeed, rotation*maxSpeed, true);
     lastSpeed = speed;
   }
 
-  public void arcadeDriveScaled(double speed, double rotation) {
-    diffDrive.arcadeDrive((speed*maxSpeed) * (1 - driveScale) + driveScale, (rotation*maxSpeed)* (1 - rotationScale) + rotationScale, true);
-    lastSpeed = speed;
-  }
-
   public void tankDrive(double leftSpeed, double rightSpeed) {
     diffDrive.tankDrive(leftSpeed, rightSpeed);
   }
 
-  public void setDemoMode(boolean newDemoMode) {
-    demoMode = newDemoMode;
-    if (demoMode == true) {
-        maxSpeed = demoSpeed;
-    } else {
-        maxSpeed = workingSpeed;
-    }
-    return;
-  }
-
-  public double setMaxSpeed() {
-    return maxSpeed;
+  public void tankDriveVolts(double leftVolts, double rightVolts) {
+    leftMaster.setVoltage(leftVolts);
+    rightMaster.setVoltage(rightVolts);
   }
 
   public double getLeftEncoders() {
@@ -183,4 +174,10 @@ public double getLeftVelocity() {
   public double getRightVelocity() {
     return rightMasterEncoder.getVelocity();
   }
+  public Pose2d getPose() {
+    return m_odometry.getPoseMeters();
+  }
+  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
+    return new DifferentialDriveWheelSpeeds(leftMasterEncoder.getVelocity(), rightMasterEncoder.getVelocity());
+  }
 }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Indexer.java b/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
index 5dbb4c0..cb4ba70 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
@@ -7,31 +7,32 @@
 
 package frc.robot.subsystems;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.*;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Indexer extends SubsystemBase {
   /**
    * Creates a new ExampleSubsystem.
    */
   TalonSRX indexMotor = new TalonSRX(20);
+  DigitalInput indexerSensor = new DigitalInput(5);
   public Indexer() {
-    
-
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    if(frc.robot.RobotContainer.m_joystick.getRawButton(1)) {
-      indexMotor.set(ControlMode.PercentOutput, 0.8);
-    }
-    else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
-      indexMotor.set(ControlMode.PercentOutput, -0.6);
-    }
-    else {
-      indexMotor.set(ControlMode.PercentOutput, 0);
-    }
+    SmartDashboard.putBoolean("IndexerSensor", indexerSensor.get());
+  }
+
+  public void setMotor(double speed){
+    indexMotor.set(ControlMode.PercentOutput, speed);
+  }
+
+  public boolean ballAtEnd(){
+    return indexerSensor.get();
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Intake.java b/2020Code/src/main/java/frc/robot/subsystems/Intake.java
index b0ca2b8..8b77175 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Intake.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Intake.java
@@ -20,28 +20,21 @@ public class Intake extends SubsystemBase {
   private Solenoid piston;
   boolean prevButton6 = false;
   public Intake() {
-    piston = new Solenoid(0, 4);
+    piston = new Solenoid(0, 1);
     addChild("piston", piston);
     motor = new CANSparkMax(16, MotorType.kBrushless);
   }
 
   @Override
   public void periodic() {
-    if(frc.robot.RobotContainer.m_joystick.getRawButton(2)) {
-      motor.set(-0.6);
-    }
-    else {
-      if(frc.robot.RobotContainer.m_joystick.getRawButton(3)) {
-        motor.set(0.4);
-      }
-      else {
-        motor.set(0);
-      }
-    }
-    if(!frc.robot.RobotContainer.m_joystick.getRawButton(6)) prevButton6 = true;
-    if(frc.robot.RobotContainer.m_joystick.getRawButton(6) && prevButton6){
-      piston.set(!piston.get());
-      prevButton6 = false;
-    }
+  }
+  public void setMotor(double speed){
+    motor.set(speed);
+  }
+  public void setPiston(boolean state){
+    piston.set(state);
+  }
+  public boolean getPiston(){
+    return piston.get();
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Lights.java b/2020Code/src/main/java/frc/robot/subsystems/Lights.java
deleted file mode 100644
index 4cdd9e3..0000000
--- a/2020Code/src/main/java/frc/robot/subsystems/Lights.java
+++ /dev/null
@@ -1,119 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc.robot.subsystems;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj.AddressableLED;
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import java.lang.System;
-
-public class Lights extends SubsystemBase {
-  /**
-   * Creates a new ExampleSubsystem.
-   */
-  AddressableLED m_led = new AddressableLED(9);
-  AddressableLEDBuffer m_ledBuffer = new AddressableLEDBuffer(5);
-  String pattern = "none";
-  int red = 0;
-  int green = 0;
-  int blue = 0;
-  int specialA = 0;
-  int specialB = 0;
-  public Lights() {
-    m_led.setLength(m_ledBuffer.getLength());
-    m_led.setData(m_ledBuffer);
-    m_led.start();
-  }
-
-  @Override
-  public void periodic() {
-    switch(pattern) {
-      case "solid":
-        for (var i = 0; i < m_ledBuffer.getLength(); i++) {
-          m_ledBuffer.setRGB(i, red, green, blue);
-        }
-        break;
-      case "rainbow road":
-        for (var i = 0; i < m_ledBuffer.getLength(); i++) {
-          hsvToRgb((i % specialA)/specialA, 1, 1);
-          m_ledBuffer.setRGB(i, red, green, blue);
-        }
-      case "simple":
-        for (var i = 0; i < m_ledBuffer.getLength(); i = i + specialA) {
-          for (var j = i; j < i + specialB; j++) {
-            m_ledBuffer.setRGB(j, red, green, blue);
-          }
-        }
-        break;
-      case "blink":
-        if(System.currentTimeMillis() % (1000 *(specialA + specialB)) < 1000 *specialA) {
-          for (var i = 0; i < m_ledBuffer.getLength(); i++) {
-            m_ledBuffer.setRGB(i, red, green, blue);
-          }
-        }
-        else {
-          for (var i = 0; i < m_ledBuffer.getLength(); i++) {
-            m_ledBuffer.setRGB(i, 0, 0, 0);
-          }
-        }
-        break;
-      default:
-        break;
-    }
-    m_led.setData(m_ledBuffer);
-  }
-  public void setLedPattern(String Pattern, int RedValue, int GreenValue, int BlueValue, int ExtraOne, int ExtraTwo) {
-    pattern = Pattern;
-    red = RedValue;
-    blue = BlueValue;
-    green = GreenValue;
-    specialA = ExtraOne;
-    specialB = ExtraTwo;
-  }
-  // public void convertRainbow(int H){
-  //   if (0 < H < 60)
-  //   return (1,x,0);
-  // }
-  public void hsvToRgb(float hue, float saturation, float value) {
-
-    int h = (int)(hue * 6);
-    float f = hue * 6 - h;
-    float p = value * (1 - saturation);
-    float q = value * (1 - f * saturation);
-    float t = value * (1 - (1 - f) * saturation);
-
-    switch (h) {
-      case 0: 
-        rgbToInt(value, t, p);
-        break;
-      case 1: 
-        rgbToInt(q, value, p);
-        break;
-      case 2:
-        rgbToInt(p, value, t);
-        break;
-      case 3:
-        rgbToInt(p, q, value);
-        break;
-      case 4:
-        rgbToInt(t, p, value);
-        break;
-      case 5: 
-        rgbToInt(value, p, q);
-        break;
-      default: throw new RuntimeException("Something went wrong when converting from HSV to RGB. Input was " + hue + ", " + saturation + ", " + value);
-    }
-  }
-
-  public void rgbToInt(float r, float g, float b) {
-    red = (int)(r * 255);
-    blue = (int)(b * 255);
-    green = (int)(g * 255);
-  }
-}
-
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Pneumatics.java b/2020Code/src/main/java/frc/robot/subsystems/Pneumatics.java
index 2a95e86..2aca57f 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Pneumatics.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Pneumatics.java
@@ -25,4 +25,11 @@ public Pneumatics() {
   public void periodic() {
     // This method will be called once per scheduler run
   }
+
+  public void setCompressor(boolean set){
+    if(set)
+    compressor.start();
+    else
+    compressor.stop();
+  }
 }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Shooter.java b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
index 4af4734..3af86d5 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
@@ -9,7 +9,6 @@
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.revrobotics.CANSparkMax;
-//import com.revrobotics.CANSparkMaxLowLevel;
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
@@ -18,7 +17,6 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.Servo;
 
 public class Shooter extends SubsystemBase {
@@ -31,55 +29,19 @@ public class Shooter extends SubsystemBase {
   Encoder ShootEncoder = new Encoder(8, 9, false, EncodingType.k4X);
   CANSparkMax Turret = new CANSparkMax(17, MotorType.kBrushless);
   CANEncoder TurretEncoder = Turret.getEncoder();
-  PIDController pid = new PIDController(0.00007, 0, 0);
-  double output = 0;
   double hoodSet = 0;
-  double total = 0;
-  int count = 0;
   double[] rollingAverage = {0,0,0,0,0};
   int pointer = 0;
   public Shooter() {
     Shoot2.follow(Shoot1);
     Shoot2.setInverted(false);
     Hood.setBounds(2.0, 1.8, 1.5, 1.2, 1.0);
-    //Turret.setOpenLoopRampRate(5);
+    TurretEncoder.setPosition(0);
   }
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
-    if (frc.robot.RobotContainer.m_joystick.getRawAxis(3) > 0.8 && hoodSet < 0.35){
-      hoodSet += 0.005;
-    }
-    if (frc.robot.RobotContainer.m_joystick.getRawAxis(2) > 0.8 && hoodSet > 0){
-      hoodSet -= 0.005;
-    }
-    SmartDashboard.putNumber("Hood value", hoodSet);
-    Hood.setPosition(hoodSet);
-    if(frc.robot.RobotContainer.m_joystick.getRawButton(5)){
-      // rollingAverage[pointer] = ShootEncoder.getRate();
-      // pointer = (pointer + 1) % 4;
-      // double average = (rollingAverage[0] + rollingAverage[1] + rollingAverage[2] + rollingAverage[3] + rollingAverage[4]) / 5;
-      // if(average > -450){
-      //   output = 1;
-      // }
-      // else{
-      //   output = 0.8;
-      // }
-      setShooterVoltage(9);
-      // if(frc.robot.RobotContainer.m_joystick.getRawButton(3)){
-      //   total = total + ShootEncoder.getRate();
-      //   count++;
-      //   SmartDashboard.putNumber("Shooter Average", total / count);
-      // }
-    }
-    else {
-      output = 0;
-      setShooterVoltage(0);
-    }
-    SmartDashboard.putNumber("Shooter Output", output);
-    setTurretSpeed(-frc.robot.RobotContainer.m_joystick.getRawAxis(0));
-    SmartDashboard.putNumber("shooterencoder", ShootEncoder.getRate());
+    SmartDashboard.putNumber("Shooter angle", getAngle());
   }
 
   public void setShooterSpeed(double speed) {
@@ -89,9 +51,29 @@ public void setTurretSpeed(double speed) {
     Turret.set(MathUtil.clamp(-speed * 0.1, -0.1, 0.1));
   }
   public double getAngle() {
-    return TurretEncoder.getPosition();
+    SmartDashboard.putNumber("Shooter angle", TurretEncoder.getPosition() * 90 / 8.0238);
+    return TurretEncoder.getPosition() * 90 / 8.0238;
   }
   public void setShooterVoltage(double volt){
     Shoot1.setVoltage(volt);
+    Shoot2.setVoltage(volt);
+    SmartDashboard.putNumber("Shooter Direct", ShootEncoder.getRate());
+  }
+  public void setHood(double value){
+    Hood.set(MathUtil.clamp(value, 0, 0.61));
+  }
+  public double getHood(){
+    return Hood.get();
+  }
+  public double getFlywheelSpeed(){
+    double total = 0;
+    for(int i = 0; i < 5; i++){
+      total += rollingAverage[i];
+    }
+    total /= 5;
+    return total;
+  }
+  public void zeroTurretAngle(){
+    TurretEncoder.setPosition(0);
   }
 }
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/subsystems/ShooterLimelight.java b/2020Code/src/main/java/frc/robot/subsystems/ShooterLimelight.java
index f558e65..99aa6cd 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/ShooterLimelight.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/ShooterLimelight.java
@@ -11,13 +11,14 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.cameraserver.CameraServer;;
 
 public class ShooterLimelight extends SubsystemBase {
   /**
    * Creates a new ExampleSubsystem.
    */
   public ShooterLimelight() {
-
+    CameraServer.getInstance().startAutomaticCapture();
   }
 
   @Override