From eb26fa21a232aa4746aa632624ab2b05d904958a Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Wed, 19 Feb 2020 20:01:18 -0500
Subject: [PATCH 01/23] Moved things from periodic to commands.

---
 .../main/java/frc/robot/RobotContainer.java   |   8 +-
 .../frc/robot/commands/ControlIndexer.java    |  57 ++++++++++
 .../frc/robot/commands/ControlIntake.java     |  65 +++++++++++
 .../java/frc/robot/commands/DriveArc.java     |   7 --
 .../java/frc/robot/commands/GoToBall.java     |  84 --------------
 .../java/frc/robot/commands/GoToColor.java    |   9 +-
 .../java/frc/robot/commands/SpinTimes.java    |   4 +-
 .../java/frc/robot/commands/TargetPort.java   |   7 +-
 .../robot/commands/TurretWithJoystick.java    |  49 +++++++++
 .../frc/robot/subsystems/BallLimelight.java   | 103 ------------------
 .../frc/robot/subsystems/DiskControl.java     |  18 +--
 .../java/frc/robot/subsystems/Drivetrain.java |   6 -
 .../java/frc/robot/subsystems/Indexer.java    |  15 +--
 .../java/frc/robot/subsystems/Intake.java     |  25 ++---
 .../java/frc/robot/subsystems/Shooter.java    |  36 ------
 15 files changed, 198 insertions(+), 295 deletions(-)
 create mode 100644 2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/ControlIntake.java
 delete mode 100644 2020Code/src/main/java/frc/robot/commands/GoToBall.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
 delete mode 100644 2020Code/src/main/java/frc/robot/subsystems/BallLimelight.java

diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index 69782b0..2795051 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -34,7 +34,6 @@ public class RobotContainer {
   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,9 @@ public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
     DrivetrainT.setDefaultCommand(new DriveWithJoystick());
+    IntakeT.setDefaultCommand(new ControlIntake());
+    ShooterT.setDefaultCommand(new TurretWithJoystick());
+    IndexerT.setDefaultCommand(new ControlIndexer());
   }
 
   /**
@@ -60,12 +62,12 @@ 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);
 
     //a.whenPressed(new GoToColor());
     //b.whenPressed(new SpinTimes());
     // x.whenPressed(new AutoTest());
-    y.toggleWhenPressed(new TargetPort());
+    leftBumper.toggleWhenPressed(new TargetPort());
   }
 
 
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java b/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
new file mode 100644
index 0000000..fb4161e
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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 ControlIndexer extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ControlIndexer() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    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(1)) {
+      frc.robot.RobotContainer.IndexerT.setMotor(0.8);
+    }
+    else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.6);
+    }
+    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/ControlIntake.java b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
new file mode 100644
index 0000000..66e91d1
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* 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);
+  }
+
+  // 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);
+      }
+    }
+    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;
+    }
+  }
+
+  // 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..416de23 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,15 @@ 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);
   }
 
   // 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 +57,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.
@@ -74,8 +69,6 @@ public void end(boolean interrupted) {
   // 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/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/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..044838b 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -50,13 +50,12 @@ 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
     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) {
+    if(frc.robot.RobotContainer.ShooterLimelightT.limelightHasTarget() && frc.robot.RobotContainer.m_joystick.getRawButton(4)) {
       angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew();
       if (Math.abs(angle) > 0.05) frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
     }
@@ -67,7 +66,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/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
new file mode 100644
index 0000000..2643049
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* 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 TurretWithJoystick extends CommandBase {
+  @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));
+  }
+
+  // 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 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/DiskControl.java b/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
index 57b6a24..e5bde8d 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,22 +20,11 @@ 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);
     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..24bec12 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -12,7 +12,6 @@
 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;
@@ -113,14 +112,9 @@ public Drivetrain() {
   @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());
   }
 
   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;
   }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Indexer.java b/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
index 5dbb4c0..5914b69 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
@@ -17,21 +17,14 @@ public class Indexer extends SubsystemBase {
    */
   TalonSRX indexMotor = new TalonSRX(20);
   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);
-    }
+  }
+
+  public void setMotor(double speed){
+    indexMotor.set(ControlMode.PercentOutput, speed);
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Intake.java b/2020Code/src/main/java/frc/robot/subsystems/Intake.java
index b0ca2b8..d477f52 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Intake.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Intake.java
@@ -27,21 +27,14 @@ public Intake() {
 
   @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/Shooter.java b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
index 4af4734..9fced84 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;
@@ -17,7 +16,6 @@
 import edu.wpi.first.wpiutil.math.MathUtil;
 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;
 
@@ -42,44 +40,10 @@ public Shooter() {
     Shoot2.follow(Shoot1);
     Shoot2.setInverted(false);
     Hood.setBounds(2.0, 1.8, 1.5, 1.2, 1.0);
-    //Turret.setOpenLoopRampRate(5);
   }
 
   @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());
   }
 
   public void setShooterSpeed(double speed) {

From 0902ca70164028980cdf5d79754f6ded5811398d Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Thu, 20 Feb 2020 19:13:04 -0500
Subject: [PATCH 02/23] New auto added, better distance calculation

---
 .../src/main/java/frc/robot/Constants.java    | 15 ++++-
 .../main/java/frc/robot/RobotContainer.java   | 66 ++++++++++++++++++-
 .../java/frc/robot/commands/TargetPort.java   |  3 +-
 .../java/frc/robot/subsystems/Drivetrain.java | 43 ++++++------
 4 files changed, 99 insertions(+), 28 deletions(-)

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/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index 2795051..bdb7ffc 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -7,6 +7,7 @@
 
 package frc.robot;
 
+import java.util.List;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
@@ -14,6 +15,16 @@
 import frc.robot.subsystems.*;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.trajectory.*;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.controller.PIDController;
 // import frc.robot.Constants.*;
 // import edu.wpi.first.wpilibj.util.Color;
 
@@ -25,11 +36,11 @@
  */
 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();
@@ -78,6 +89,55 @@ private void configureButtonBindings() {
    */
   public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
-    return new AutoTest();
+    //return new AutoTest();
+    var autoVoltageConstraint =
+        new DifferentialDriveVoltageConstraint(
+            new SimpleMotorFeedforward(DriveConstants.ksVolts,
+                                       DriveConstants.kvVoltSecondsPerInch,
+                                       DriveConstants.kaVoltSecondsSquaredPerInch),
+                                       DriveConstants.kDriveKinematics,
+                                       10);
+    // Create config for trajectory
+    TrajectoryConfig config =
+        new TrajectoryConfig(DriveConstants.kMaxSpeedInchesPerSecond,
+                             DriveConstants.kMaxAccelerationInchesPerSecondSquared)
+            // Add kinematics to ensure max speed is actually obeyed
+            .setKinematics(DriveConstants.kDriveKinematics)
+            // Apply the voltage constraint
+            .addConstraint(autoVoltageConstraint);
+
+    // An example trajectory to follow.  All units in meters.
+    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+        // Start at the origin facing the +X direction
+        new Pose2d(0, 0, new Rotation2d(0)),
+        // Pass through these two interior waypoints, making an 's' curve path
+        List.of(
+            new Translation2d(40, 40),
+            new Translation2d(80, -40)
+        ),
+        // End 3 meters straight ahead of where we started, facing forward
+        new Pose2d(120, 0, new Rotation2d(0)),
+        // Pass config
+        config
+    );
+
+    RamseteCommand ramseteCommand = new RamseteCommand(
+        exampleTrajectory,
+        DrivetrainT::getPose,
+        new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta),
+        new SimpleMotorFeedforward(DriveConstants.ksVolts,
+                                   DriveConstants.kvVoltSecondsPerInch,
+                                   DriveConstants.kaVoltSecondsSquaredPerInch),
+        DriveConstants.kDriveKinematics,
+        DrivetrainT::getWheelSpeeds,
+        new PIDController(DriveConstants.kPDriveVel, 0, 0),
+        new PIDController(DriveConstants.kPDriveVel, 0, 0),
+        // RamseteCommand passes volts to the callback
+        DrivetrainT::tankDriveVolts,
+        DrivetrainT
+    );
+
+    // Run path following command, then stop at the end.
+    return ramseteCommand.andThen(() -> DrivetrainT.tankDriveVolts(0, 0));
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index 044838b..2412b9c 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -48,7 +48,8 @@ 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 = (91 - 17) * Math.tan(frc.robot.RobotContainer.ShooterLimelightT.getYSkew());
     SmartDashboard.putNumber("Distance to target", distance);
     frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
     // robotVelocity = frc.robot.RobotContainer.DrivetrainT.getVelocity();
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
index 24bec12..24637e4 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -12,10 +12,10 @@
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-// 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;
@@ -33,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;
@@ -47,8 +49,7 @@ 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.
    */
@@ -106,12 +107,16 @@ 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
+    m_odometry.update(Rotation2d.fromDegrees(frc.robot.RobotContainer.NavxT.getHeading()), getLeftEncoders(), getRightEncoders());
   }
 
   public void arcadeDrive(double speed, double rotation) {
@@ -119,27 +124,13 @@ public void arcadeDrive(double speed, double rotation) {
     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() {
@@ -177,4 +168,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());
+  }
 }

From 7b3c40d3ae8a7bf22729baa01a723f47df2bae6e Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Tue, 25 Feb 2020 13:39:19 -0500
Subject: [PATCH 03/23] Updated to new WPILIB, and this code works

---
 2020Code/build.gradle                                         | 2 +-
 2020Code/src/main/java/frc/robot/RobotContainer.java          | 2 +-
 2020Code/src/main/java/frc/robot/commands/ControlIndexer.java | 4 ++--
 .../src/main/java/frc/robot/commands/TurretWithJoystick.java  | 3 +++
 4 files changed, 7 insertions(+), 4 deletions(-)

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/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index bdb7ffc..c166e18 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -120,7 +120,7 @@ public Command getAutonomousCommand() {
         // Pass config
         config
     );
-
+    
     RamseteCommand ramseteCommand = new RamseteCommand(
         exampleTrajectory,
         DrivetrainT::getPose,
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java b/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
index fb4161e..25f1157 100644
--- a/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
@@ -34,10 +34,10 @@ public void initialize() {
   @Override
   public void execute() {
     if(frc.robot.RobotContainer.m_joystick.getRawButton(1)) {
-      frc.robot.RobotContainer.IndexerT.setMotor(0.8);
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.6);
     }
     else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
-      frc.robot.RobotContainer.IndexerT.setMotor(-0.6);
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.9);
     }
     else {
       frc.robot.RobotContainer.IndexerT.setMotor(0);
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
index 2643049..d345581 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -34,6 +34,9 @@ public void initialize() {
   // 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 frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
   }
 
   // Called once the command ends or is interrupted.

From 4e2693448c56a74e20906bda684126a81f4aaa56 Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Wed, 26 Feb 2020 20:57:10 -0500
Subject: [PATCH 04/23] AUTO HOOD ADJUST

---
 .../main/java/frc/robot/RobotContainer.java   |   5 +-
 .../main/java/frc/robot/autos/AutoPathA.java  |  60 +++++++++
 .../frc/robot/commands/ControlIndexer.java    |  57 ---------
 .../frc/robot/commands/ControlIntake.java     |  14 +++
 .../java/frc/robot/commands/TargetPort.java   |  14 ++-
 .../robot/commands/TurretWithJoystick.java    |  11 +-
 .../java/frc/robot/subsystems/Indexer.java    |   8 ++
 .../java/frc/robot/subsystems/Intake.java     |   2 +-
 .../java/frc/robot/subsystems/Lights.java     | 119 ------------------
 .../java/frc/robot/subsystems/Shooter.java    |  27 +++-
 10 files changed, 125 insertions(+), 192 deletions(-)
 create mode 100644 2020Code/src/main/java/frc/robot/autos/AutoPathA.java
 delete mode 100644 2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
 delete mode 100644 2020Code/src/main/java/frc/robot/subsystems/Lights.java

diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index c166e18..c1b60a8 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -60,7 +60,6 @@ public RobotContainer() {
     DrivetrainT.setDefaultCommand(new DriveWithJoystick());
     IntakeT.setDefaultCommand(new ControlIntake());
     ShooterT.setDefaultCommand(new TurretWithJoystick());
-    IndexerT.setDefaultCommand(new ControlIndexer());
   }
 
   /**
@@ -88,7 +87,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
     //return new AutoTest();
     var autoVoltageConstraint =
         new DifferentialDriveVoltageConstraint(
@@ -105,7 +103,6 @@ public Command getAutonomousCommand() {
             .setKinematics(DriveConstants.kDriveKinematics)
             // Apply the voltage constraint
             .addConstraint(autoVoltageConstraint);
-
     // An example trajectory to follow.  All units in meters.
     Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
         // Start at the origin facing the +X direction
@@ -120,7 +117,7 @@ public Command getAutonomousCommand() {
         // Pass config
         config
     );
-    
+
     RamseteCommand ramseteCommand = new RamseteCommand(
         exampleTrajectory,
         DrivetrainT::getPose,
diff --git a/2020Code/src/main/java/frc/robot/autos/AutoPathA.java b/2020Code/src/main/java/frc/robot/autos/AutoPathA.java
new file mode 100644
index 0000000..d120937
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/autos/AutoPathA.java
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* 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.autos;
+
+import frc.robot.commands.*;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import frc.robot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.trajectory.*;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
+import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import java.util.List;
+
+public class AutoPathA extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  public AutoPathA(DifferentialDriveVoltageConstraint autoVoltageConstraint, TrajectoryConfig config) {
+    /*
+    * First part of the auto, does this off the starting line.
+    */
+    Trajectory part1 = TrajectoryGenerator.generateTrajectory(
+      // Start at the origin facing the +X direction
+      new Pose2d(0, 0, new Rotation2d(0)),
+      // Pass through these two interior waypoints, making an 's' curve path
+      List.of(
+          new Translation2d(40, 40),
+          new Translation2d(80, -40)
+      ),
+      new Pose2d(120, 0, new Rotation2d(0)),
+      config
+    );
+    RamseteCommand ramseteCommand1 = new RamseteCommand(
+        part1,
+        frc.robot.RobotContainer.DrivetrainT::getPose,
+        new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta),
+        new SimpleMotorFeedforward(DriveConstants.ksVolts,
+                                   DriveConstants.kvVoltSecondsPerInch,
+                                   DriveConstants.kaVoltSecondsSquaredPerInch),
+        DriveConstants.kDriveKinematics,
+        frc.robot.RobotContainer.DrivetrainT::getWheelSpeeds,
+        new PIDController(DriveConstants.kPDriveVel, 0, 0),
+        new PIDController(DriveConstants.kPDriveVel, 0, 0),
+        frc.robot.RobotContainer.DrivetrainT::tankDriveVolts,
+        frc.robot.RobotContainer.DrivetrainT
+    );
+    addCommands(
+      ramseteCommand1.andThen(() -> frc.robot.RobotContainer.DrivetrainT.tankDriveVolts(0, 0))
+    );
+  }
+}
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java b/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
deleted file mode 100644
index 25f1157..0000000
--- a/2020Code/src/main/java/frc/robot/commands/ControlIndexer.java
+++ /dev/null
@@ -1,57 +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 ControlIndexer extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ControlIndexer() {
-    // Use addRequirements() here to declare subsystem dependencies.
-    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(1)) {
-      frc.robot.RobotContainer.IndexerT.setMotor(-0.6);
-    }
-    else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
-      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/ControlIntake.java b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
index 66e91d1..35aa680 100644
--- a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
@@ -24,6 +24,7 @@ public class ControlIntake extends CommandBase {
   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.
@@ -45,11 +46,24 @@ public void execute() {
         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.9);
+    }
+    else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
+      frc.robot.RobotContainer.IndexerT.setMotor(0.6);
+    }
+    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.
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index 2412b9c..df76463 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -49,16 +49,20 @@ public void initialize() {
   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 = (91 - 17) * Math.tan(frc.robot.RobotContainer.ShooterLimelightT.getYSkew());
+    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);
+    //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(frc.robot.RobotContainer.ShooterLimelightT.limelightHasTarget() && frc.robot.RobotContainer.m_joystick.getRawButton(4)) {
-      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()) {
+      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
+      SmartDashboard.putNumber("Hood Auto", -0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
+      angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + 1.2;
+      frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
     }
   }
 
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
index d345581..a844d93 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -7,12 +7,15 @@
 
 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"})
 
   /**
@@ -28,7 +31,8 @@ public TurretWithJoystick() {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-      frc.robot.RobotContainer.ShooterT.setTurretSpeed(frc.robot.RobotContainer.m_joystick.getRawAxis(0));
+      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.
@@ -37,6 +41,11 @@ 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 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.
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Indexer.java b/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
index 5914b69..cb4ba70 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Indexer.java
@@ -7,24 +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
+    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 d477f52..8b77175 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Intake.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Intake.java
@@ -20,7 +20,7 @@ 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);
   }
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/Shooter.java b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
index 9fced84..55cd351 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
@@ -16,7 +16,7 @@
 import edu.wpi.first.wpiutil.math.MathUtil;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.Servo;
 
 public class Shooter extends SubsystemBase {
@@ -29,11 +29,7 @@ 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() {
@@ -44,6 +40,11 @@ public Shooter() {
 
   @Override
   public void periodic() {
+    rollingAverage[pointer] = ShootEncoder.getRate();
+    if(pointer < 4) pointer++;
+    else pointer = 0;
+    SmartDashboard.putNumber("Shooter AVG", getFlywheelSpeed());
+    SmartDashboard.putNumber("Shooter Direct", ShootEncoder.getRate());
   }
 
   public void setShooterSpeed(double speed) {
@@ -57,5 +58,21 @@ public double getAngle() {
   }
   public void setShooterVoltage(double volt){
     Shoot1.setVoltage(volt);
+    Shoot2.setVoltage(volt);
+    SmartDashboard.putNumber("Shooter Direct", ShootEncoder.getRate());
+  }
+  public void setHood(double value){
+    Hood.set(value);
+  }
+  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;
   }
 }
\ No newline at end of file

From bd6624d83cbbb67ea0a4816347b55de3405d533f Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Thu, 27 Feb 2020 20:16:51 -0500
Subject: [PATCH 05/23] t

---
 .../main/java/frc/robot/RobotContainer.java   |  2 +-
 .../main/java/frc/robot/autos/ThreeBall.java  | 26 ++++++++
 .../java/frc/robot/commands/AutoIntake.java   | 66 +++++++++++++++++++
 .../java/frc/robot/commands/TargetPort.java   |  7 +-
 .../robot/commands/TurretWithJoystick.java    |  3 +-
 .../java/frc/robot/subsystems/Drivetrain.java | 54 ++++++++-------
 6 files changed, 129 insertions(+), 29 deletions(-)
 create mode 100644 2020Code/src/main/java/frc/robot/autos/ThreeBall.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/AutoIntake.java

diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index c1b60a8..4300d79 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -77,7 +77,7 @@ private void configureButtonBindings() {
     //a.whenPressed(new GoToColor());
     //b.whenPressed(new SpinTimes());
     // x.whenPressed(new AutoTest());
-    leftBumper.toggleWhenPressed(new TargetPort());
+    leftBumper.toggleWhenPressed(new TargetPort(false));
   }
 
 
diff --git a/2020Code/src/main/java/frc/robot/autos/ThreeBall.java b/2020Code/src/main/java/frc/robot/autos/ThreeBall.java
new file mode 100644
index 0000000..d0a2bc8
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/autos/ThreeBall.java
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* 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.autos;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.*;
+
+public class ThreeBall extends SequentialCommandGroup {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  public ThreeBall() {
+    addCommands(
+      new AutoIntake(0, 0, true), 
+      new DriveDistance(-100),
+      new ParallelCommandGroup(
+        new TargetPort(true),
+        new AutoIntake(0, -0.9, true)
+      )
+    );
+  }
+}
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoIntake.java b/2020Code/src/main/java/frc/robot/commands/AutoIntake.java
new file mode 100644
index 0000000..ecad1f7
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/AutoIntake.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 AutoIntake extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  boolean prevButton6;
+  double m_intakeSpeed;
+  double m_indexerSpeed;
+  boolean m_piston;
+  public AutoIntake(double intakeSpeed, double indexerSpeed, boolean piston) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(frc.robot.RobotContainer.IntakeT);
+    addRequirements(frc.robot.RobotContainer.IndexerT);
+    m_intakeSpeed = intakeSpeed;
+    m_indexerSpeed = indexerSpeed;
+    m_piston = piston;
+  }
+
+  // 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.setMotor(m_intakeSpeed);
+    frc.robot.RobotContainer.IntakeT.setPiston(m_piston);
+    if(m_intakeSpeed < 0 && frc.robot.RobotContainer.IndexerT.ballAtEnd()){
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.9);
+    }
+    else{
+      frc.robot.RobotContainer.IndexerT.setMotor(m_indexerSpeed);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    frc.robot.RobotContainer.IndexerT.setMotor(0);
+    frc.robot.RobotContainer.IntakeT.setMotor(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index df76463..507cd40 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -29,10 +29,12 @@ public class TargetPort extends CommandBase {
   double ballVelocity;
   double turretAngle;
   long time;
+  boolean m_inAuto;
   PIDController pid = new PIDController(0.1, 0, 0.0);
-  public TargetPort() {
+  public TargetPort(boolean inAuto) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(frc.robot.RobotContainer.ShooterT);
+    m_inAuto = inAuto;
   }
 
   // Called when the command is initially scheduled.
@@ -58,8 +60,9 @@ public void execute() {
     // angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + predictor;
     if(frc.robot.RobotContainer.m_joystick.getRawButton(5)) frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
     else frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
+    if(m_inAuto)frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
     if(frc.robot.RobotContainer.ShooterLimelightT.limelightHasTarget()) {
-      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
+      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000115*(Math.pow(distance, 2)));
       SmartDashboard.putNumber("Hood Auto", -0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
       angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + 1.2;
       frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
index a844d93..753205c 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -38,8 +38,9 @@ public void initialize() {
   // 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));
+    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;
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
index 24637e4..b9d865b 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -82,23 +82,23 @@ public Drivetrain() {
     leftMasterEncoder.setVelocityConversionFactor(encoderFactor);
     leftMaster.setOpenLoopRampRate(0.1);
     
-    leftFollower1 = new CANSparkMax(14, MotorType.kBrushless);
-    leftFollower1.setInverted(true);
-    leftFollowerEncoder1 = leftFollower1.getEncoder();
-    leftFollowerEncoder1.setPositionConversionFactor(encoderFactor);
-    leftFollowerEncoder1.setVelocityConversionFactor(encoderFactor);
-    leftFollower1.setOpenLoopRampRate(0.1);
-
-    leftFollower2 = new CANSparkMax(13, MotorType.kBrushless);
-    leftFollower2.setInverted(true);
-    leftFollowerEncoder2 = leftFollower2.getEncoder();
-    leftFollowerEncoder2.setPositionConversionFactor(encoderFactor);
-    leftFollowerEncoder2.setVelocityConversionFactor(encoderFactor);
-    leftFollower2.setOpenLoopRampRate(0.1);
+    // leftFollower1 = new CANSparkMax(14, MotorType.kBrushless);
+    // leftFollower1.setInverted(true);
+    // leftFollowerEncoder1 = leftFollower1.getEncoder();
+    // leftFollowerEncoder1.setPositionConversionFactor(encoderFactor);
+    // leftFollowerEncoder1.setVelocityConversionFactor(encoderFactor);
+    // leftFollower1.setOpenLoopRampRate(0.1);
+
+    // leftFollower2 = new CANSparkMax(15, MotorType.kBrushless);
+    // leftFollower2.setInverted(true);
+    // leftFollowerEncoder2 = leftFollower2.getEncoder();
+    // leftFollowerEncoder2.setPositionConversionFactor(encoderFactor);
+    // leftFollowerEncoder2.setVelocityConversionFactor(encoderFactor);
+    // leftFollower2.setOpenLoopRampRate(0.1);
     
 
-    leftFollower1.follow(leftMaster);
-    leftFollower2.follow(leftMaster);
+    // leftFollower1.follow(leftMaster);
+    // leftFollower2.follow(leftMaster);
     rightFollower1.follow(rightMaster);
     rightFollower2.follow(rightMaster);
     
@@ -134,7 +134,8 @@ public void tankDriveVolts(double leftVolts, double rightVolts) {
   }
 
   public double getLeftEncoders() {
-    return ((leftMasterEncoder.getPosition() + leftFollowerEncoder1.getPosition() + leftFollowerEncoder2.getPosition())/3);
+    return 1;
+    //((leftMasterEncoder.getPosition() + leftFollowerEncoder1.getPosition() + leftFollowerEncoder2.getPosition())/3);
   }
 
   public double getRightEncoders() {
@@ -149,21 +150,24 @@ public boolean isDrivingAllowed() {
     return drivingEnabled;
   }
   public double getEncoderAverage() {
-    return (getRightEncoders() + getLeftEncoders() / 2);
+    return 1;
+    //(getRightEncoders() + getLeftEncoders() / 2);
   }
   public void resetEncoders() {
-    leftFollowerEncoder2.setPosition(0);
-    leftFollowerEncoder1.setPosition(0);
-    leftMasterEncoder.setPosition(0);
-    rightFollowerEncoder2.setPosition(0);
-    rightFollowerEncoder1.setPosition(0);
-    rightMasterEncoder.setPosition(0);
+    // leftFollowerEncoder2.setPosition(0);
+    // leftFollowerEncoder1.setPosition(0);
+    // leftMasterEncoder.setPosition(0);
+    // rightFollowerEncoder2.setPosition(0);
+    // rightFollowerEncoder1.setPosition(0);
+    // rightMasterEncoder.setPosition(0);
   }
   public double getVelocity() {
-    return ((leftMasterEncoder.getVelocity() + rightMasterEncoder.getVelocity()) / 2);
+    return 1;
+    //((leftMasterEncoder.getVelocity() + rightMasterEncoder.getVelocity()) / 2);
   }
   public double getLeftVelocity() {
-    return leftMasterEncoder.getVelocity();
+    return 1;
+    //leftMasterEncoder.getVelocity();
   }
   public double getRightVelocity() {
     return rightMasterEncoder.getVelocity();

From 14bcfb736ef0ab0c5dc6df7cebf9483af2679037 Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Thu, 27 Feb 2020 20:17:50 -0500
Subject: [PATCH 06/23] Revert "t"

This reverts commit bd6624d83cbbb67ea0a4816347b55de3405d533f.
---
 .../main/java/frc/robot/RobotContainer.java   |  2 +-
 .../main/java/frc/robot/autos/ThreeBall.java  | 26 --------
 .../java/frc/robot/commands/AutoIntake.java   | 66 -------------------
 .../java/frc/robot/commands/TargetPort.java   |  7 +-
 .../robot/commands/TurretWithJoystick.java    |  3 +-
 .../java/frc/robot/subsystems/Drivetrain.java | 54 +++++++--------
 6 files changed, 29 insertions(+), 129 deletions(-)
 delete mode 100644 2020Code/src/main/java/frc/robot/autos/ThreeBall.java
 delete mode 100644 2020Code/src/main/java/frc/robot/commands/AutoIntake.java

diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index 4300d79..c1b60a8 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -77,7 +77,7 @@ private void configureButtonBindings() {
     //a.whenPressed(new GoToColor());
     //b.whenPressed(new SpinTimes());
     // x.whenPressed(new AutoTest());
-    leftBumper.toggleWhenPressed(new TargetPort(false));
+    leftBumper.toggleWhenPressed(new TargetPort());
   }
 
 
diff --git a/2020Code/src/main/java/frc/robot/autos/ThreeBall.java b/2020Code/src/main/java/frc/robot/autos/ThreeBall.java
deleted file mode 100644
index d0a2bc8..0000000
--- a/2020Code/src/main/java/frc/robot/autos/ThreeBall.java
+++ /dev/null
@@ -1,26 +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.autos;
-
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.commands.*;
-
-public class ThreeBall extends SequentialCommandGroup {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  public ThreeBall() {
-    addCommands(
-      new AutoIntake(0, 0, true), 
-      new DriveDistance(-100),
-      new ParallelCommandGroup(
-        new TargetPort(true),
-        new AutoIntake(0, -0.9, true)
-      )
-    );
-  }
-}
\ No newline at end of file
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoIntake.java b/2020Code/src/main/java/frc/robot/commands/AutoIntake.java
deleted file mode 100644
index ecad1f7..0000000
--- a/2020Code/src/main/java/frc/robot/commands/AutoIntake.java
+++ /dev/null
@@ -1,66 +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 AutoIntake extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  boolean prevButton6;
-  double m_intakeSpeed;
-  double m_indexerSpeed;
-  boolean m_piston;
-  public AutoIntake(double intakeSpeed, double indexerSpeed, boolean piston) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(frc.robot.RobotContainer.IntakeT);
-    addRequirements(frc.robot.RobotContainer.IndexerT);
-    m_intakeSpeed = intakeSpeed;
-    m_indexerSpeed = indexerSpeed;
-    m_piston = piston;
-  }
-
-  // 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.setMotor(m_intakeSpeed);
-    frc.robot.RobotContainer.IntakeT.setPiston(m_piston);
-    if(m_intakeSpeed < 0 && frc.robot.RobotContainer.IndexerT.ballAtEnd()){
-      frc.robot.RobotContainer.IndexerT.setMotor(-0.9);
-    }
-    else{
-      frc.robot.RobotContainer.IndexerT.setMotor(m_indexerSpeed);
-    }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    frc.robot.RobotContainer.IndexerT.setMotor(0);
-    frc.robot.RobotContainer.IntakeT.setMotor(0);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index 507cd40..df76463 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -29,12 +29,10 @@ public class TargetPort extends CommandBase {
   double ballVelocity;
   double turretAngle;
   long time;
-  boolean m_inAuto;
   PIDController pid = new PIDController(0.1, 0, 0.0);
-  public TargetPort(boolean inAuto) {
+  public TargetPort() {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(frc.robot.RobotContainer.ShooterT);
-    m_inAuto = inAuto;
   }
 
   // Called when the command is initially scheduled.
@@ -60,9 +58,8 @@ public void execute() {
     // angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + predictor;
     if(frc.robot.RobotContainer.m_joystick.getRawButton(5)) frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
     else frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
-    if(m_inAuto)frc.robot.RobotContainer.ShooterT.setShooterVoltage(9);
     if(frc.robot.RobotContainer.ShooterLimelightT.limelightHasTarget()) {
-      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000115*(Math.pow(distance, 2)));
+      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
       SmartDashboard.putNumber("Hood Auto", -0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
       angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + 1.2;
       frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
index 753205c..a844d93 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -38,9 +38,8 @@ public void initialize() {
   // 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));
+    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;
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
index b9d865b..24637e4 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -82,23 +82,23 @@ public Drivetrain() {
     leftMasterEncoder.setVelocityConversionFactor(encoderFactor);
     leftMaster.setOpenLoopRampRate(0.1);
     
-    // leftFollower1 = new CANSparkMax(14, MotorType.kBrushless);
-    // leftFollower1.setInverted(true);
-    // leftFollowerEncoder1 = leftFollower1.getEncoder();
-    // leftFollowerEncoder1.setPositionConversionFactor(encoderFactor);
-    // leftFollowerEncoder1.setVelocityConversionFactor(encoderFactor);
-    // leftFollower1.setOpenLoopRampRate(0.1);
-
-    // leftFollower2 = new CANSparkMax(15, MotorType.kBrushless);
-    // leftFollower2.setInverted(true);
-    // leftFollowerEncoder2 = leftFollower2.getEncoder();
-    // leftFollowerEncoder2.setPositionConversionFactor(encoderFactor);
-    // leftFollowerEncoder2.setVelocityConversionFactor(encoderFactor);
-    // leftFollower2.setOpenLoopRampRate(0.1);
+    leftFollower1 = new CANSparkMax(14, MotorType.kBrushless);
+    leftFollower1.setInverted(true);
+    leftFollowerEncoder1 = leftFollower1.getEncoder();
+    leftFollowerEncoder1.setPositionConversionFactor(encoderFactor);
+    leftFollowerEncoder1.setVelocityConversionFactor(encoderFactor);
+    leftFollower1.setOpenLoopRampRate(0.1);
+
+    leftFollower2 = new CANSparkMax(13, MotorType.kBrushless);
+    leftFollower2.setInverted(true);
+    leftFollowerEncoder2 = leftFollower2.getEncoder();
+    leftFollowerEncoder2.setPositionConversionFactor(encoderFactor);
+    leftFollowerEncoder2.setVelocityConversionFactor(encoderFactor);
+    leftFollower2.setOpenLoopRampRate(0.1);
     
 
-    // leftFollower1.follow(leftMaster);
-    // leftFollower2.follow(leftMaster);
+    leftFollower1.follow(leftMaster);
+    leftFollower2.follow(leftMaster);
     rightFollower1.follow(rightMaster);
     rightFollower2.follow(rightMaster);
     
@@ -134,8 +134,7 @@ public void tankDriveVolts(double leftVolts, double rightVolts) {
   }
 
   public double getLeftEncoders() {
-    return 1;
-    //((leftMasterEncoder.getPosition() + leftFollowerEncoder1.getPosition() + leftFollowerEncoder2.getPosition())/3);
+    return ((leftMasterEncoder.getPosition() + leftFollowerEncoder1.getPosition() + leftFollowerEncoder2.getPosition())/3);
   }
 
   public double getRightEncoders() {
@@ -150,24 +149,21 @@ public boolean isDrivingAllowed() {
     return drivingEnabled;
   }
   public double getEncoderAverage() {
-    return 1;
-    //(getRightEncoders() + getLeftEncoders() / 2);
+    return (getRightEncoders() + getLeftEncoders() / 2);
   }
   public void resetEncoders() {
-    // leftFollowerEncoder2.setPosition(0);
-    // leftFollowerEncoder1.setPosition(0);
-    // leftMasterEncoder.setPosition(0);
-    // rightFollowerEncoder2.setPosition(0);
-    // rightFollowerEncoder1.setPosition(0);
-    // rightMasterEncoder.setPosition(0);
+    leftFollowerEncoder2.setPosition(0);
+    leftFollowerEncoder1.setPosition(0);
+    leftMasterEncoder.setPosition(0);
+    rightFollowerEncoder2.setPosition(0);
+    rightFollowerEncoder1.setPosition(0);
+    rightMasterEncoder.setPosition(0);
   }
   public double getVelocity() {
-    return 1;
-    //((leftMasterEncoder.getVelocity() + rightMasterEncoder.getVelocity()) / 2);
+    return ((leftMasterEncoder.getVelocity() + rightMasterEncoder.getVelocity()) / 2);
   }
   public double getLeftVelocity() {
-    return 1;
-    //leftMasterEncoder.getVelocity();
+    return leftMasterEncoder.getVelocity();
   }
   public double getRightVelocity() {
     return rightMasterEncoder.getVelocity();

From 90c0beb0fd6c15a60401a519d3027ba78b2c02d4 Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Fri, 28 Feb 2020 20:57:09 -0500
Subject: [PATCH 07/23] Working AUTO!!!

---
 2020Code/src/main/java/frc/robot/Robot.java   |  4 +
 .../main/java/frc/robot/RobotContainer.java   | 65 +---------------
 .../main/java/frc/robot/autos/AutoPathA.java  | 60 ---------------
 .../frc/robot/commands/AutoControlIntake.java | 58 ++++++++++++++
 .../frc/robot/commands/DriveDistance.java     |  2 +-
 .../frc/robot/commands/DriveDistanceSlow.java | 62 +++++++++++++++
 .../java/frc/robot/commands/SetClimb.java     | 54 +++++++++++++
 .../frc/robot/commands/SixBallSimple.java     | 41 ++++++++++
 .../frc/robot/commands/TargetPortAuto.java    | 75 +++++++++++++++++++
 .../frc/robot/commands/ThreeBallClose.java    | 31 ++++++++
 .../java/frc/robot/commands/ThreeBallFar.java | 31 ++++++++
 .../frc/robot/commands/TurretToAngle.java     | 54 +++++++++++++
 .../main/java/frc/robot/subsystems/Climb.java | 11 ++-
 .../frc/robot/subsystems/DiskControl.java     |  2 +-
 .../java/frc/robot/subsystems/Drivetrain.java |  6 ++
 .../java/frc/robot/subsystems/Shooter.java    | 14 ++--
 16 files changed, 439 insertions(+), 131 deletions(-)
 delete mode 100644 2020Code/src/main/java/frc/robot/autos/AutoPathA.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/SetClimb.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/ThreeBallClose.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/ThreeBallFar.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/TurretToAngle.java

diff --git a/2020Code/src/main/java/frc/robot/Robot.java b/2020Code/src/main/java/frc/robot/Robot.java
index d0ea75e..aad43a0 100644
--- a/2020Code/src/main/java/frc/robot/Robot.java
+++ b/2020Code/src/main/java/frc/robot/Robot.java
@@ -70,6 +70,7 @@ public void robotPeriodic() {
    */
   @Override
   public void disabledInit() {
+    frc.robot.RobotContainer.ShooterT.zeroTurretAngle();
   }
 
   @Override
@@ -81,6 +82,8 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
+    frc.robot.RobotContainer.ShooterT.zeroTurretAngle();
+    frc.robot.RobotContainer.ClimbT.setClimb(false);
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
@@ -98,6 +101,7 @@ 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
diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index c1b60a8..94e5f15 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -7,7 +7,6 @@
 
 package frc.robot;
 
-import java.util.List;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
@@ -15,16 +14,6 @@
 import frc.robot.subsystems.*;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import frc.robot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.trajectory.*;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj2.command.RamseteCommand;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.controller.PIDController;
 // import frc.robot.Constants.*;
 // import edu.wpi.first.wpilibj.util.Color;
 
@@ -73,11 +62,13 @@ private void configureButtonBindings() {
     //final JoystickButton b = new JoystickButton(m_joystick, 2);
     // final JoystickButton x = new JoystickButton(m_joystick, 3);
     final JoystickButton leftBumper = new JoystickButton(m_joystick, 4);
+    final JoystickButton climbButton = new JoystickButton(m_driverController, 8);
 
     //a.whenPressed(new GoToColor());
     //b.whenPressed(new SpinTimes());
-    // x.whenPressed(new AutoTest());
+    //x.whenPressed(new AutoTest());
     leftBumper.toggleWhenPressed(new TargetPort());
+    climbButton.whenPressed(new SetClimb(true));
   }
 
 
@@ -87,54 +78,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    //return new AutoTest();
-    var autoVoltageConstraint =
-        new DifferentialDriveVoltageConstraint(
-            new SimpleMotorFeedforward(DriveConstants.ksVolts,
-                                       DriveConstants.kvVoltSecondsPerInch,
-                                       DriveConstants.kaVoltSecondsSquaredPerInch),
-                                       DriveConstants.kDriveKinematics,
-                                       10);
-    // Create config for trajectory
-    TrajectoryConfig config =
-        new TrajectoryConfig(DriveConstants.kMaxSpeedInchesPerSecond,
-                             DriveConstants.kMaxAccelerationInchesPerSecondSquared)
-            // Add kinematics to ensure max speed is actually obeyed
-            .setKinematics(DriveConstants.kDriveKinematics)
-            // Apply the voltage constraint
-            .addConstraint(autoVoltageConstraint);
-    // An example trajectory to follow.  All units in meters.
-    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-        // Start at the origin facing the +X direction
-        new Pose2d(0, 0, new Rotation2d(0)),
-        // Pass through these two interior waypoints, making an 's' curve path
-        List.of(
-            new Translation2d(40, 40),
-            new Translation2d(80, -40)
-        ),
-        // End 3 meters straight ahead of where we started, facing forward
-        new Pose2d(120, 0, new Rotation2d(0)),
-        // Pass config
-        config
-    );
-
-    RamseteCommand ramseteCommand = new RamseteCommand(
-        exampleTrajectory,
-        DrivetrainT::getPose,
-        new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta),
-        new SimpleMotorFeedforward(DriveConstants.ksVolts,
-                                   DriveConstants.kvVoltSecondsPerInch,
-                                   DriveConstants.kaVoltSecondsSquaredPerInch),
-        DriveConstants.kDriveKinematics,
-        DrivetrainT::getWheelSpeeds,
-        new PIDController(DriveConstants.kPDriveVel, 0, 0),
-        new PIDController(DriveConstants.kPDriveVel, 0, 0),
-        // RamseteCommand passes volts to the callback
-        DrivetrainT::tankDriveVolts,
-        DrivetrainT
-    );
-
-    // Run path following command, then stop at the end.
-    return ramseteCommand.andThen(() -> DrivetrainT.tankDriveVolts(0, 0));
+    return new SixBallSimple();
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/autos/AutoPathA.java b/2020Code/src/main/java/frc/robot/autos/AutoPathA.java
deleted file mode 100644
index d120937..0000000
--- a/2020Code/src/main/java/frc/robot/autos/AutoPathA.java
+++ /dev/null
@@ -1,60 +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.autos;
-
-import frc.robot.commands.*;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
-import frc.robot.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.trajectory.*;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj2.command.RamseteCommand;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import java.util.List;
-
-public class AutoPathA extends SequentialCommandGroup {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  public AutoPathA(DifferentialDriveVoltageConstraint autoVoltageConstraint, TrajectoryConfig config) {
-    /*
-    * First part of the auto, does this off the starting line.
-    */
-    Trajectory part1 = TrajectoryGenerator.generateTrajectory(
-      // Start at the origin facing the +X direction
-      new Pose2d(0, 0, new Rotation2d(0)),
-      // Pass through these two interior waypoints, making an 's' curve path
-      List.of(
-          new Translation2d(40, 40),
-          new Translation2d(80, -40)
-      ),
-      new Pose2d(120, 0, new Rotation2d(0)),
-      config
-    );
-    RamseteCommand ramseteCommand1 = new RamseteCommand(
-        part1,
-        frc.robot.RobotContainer.DrivetrainT::getPose,
-        new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta),
-        new SimpleMotorFeedforward(DriveConstants.ksVolts,
-                                   DriveConstants.kvVoltSecondsPerInch,
-                                   DriveConstants.kaVoltSecondsSquaredPerInch),
-        DriveConstants.kDriveKinematics,
-        frc.robot.RobotContainer.DrivetrainT::getWheelSpeeds,
-        new PIDController(DriveConstants.kPDriveVel, 0, 0),
-        new PIDController(DriveConstants.kPDriveVel, 0, 0),
-        frc.robot.RobotContainer.DrivetrainT::tankDriveVolts,
-        frc.robot.RobotContainer.DrivetrainT
-    );
-    addCommands(
-      ramseteCommand1.andThen(() -> frc.robot.RobotContainer.DrivetrainT.tankDriveVolts(0, 0))
-    );
-  }
-}
\ No newline at end of file
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..8f69c95
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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);
+    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/DriveDistance.java b/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
index 367aaaa..f1efe9c 100644
--- a/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
+++ b/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
@@ -24,7 +24,7 @@ public class DriveDistance extends CommandBase {
    * @param subsystem The subsystem used by this command.
    */
   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);
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..c8f87d2
--- /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.05, 0.005, 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(2, 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/SetClimb.java b/2020Code/src/main/java/frc/robot/commands/SetClimb.java
new file mode 100644
index 0000000..72ba9b9
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/SetClimb.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.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 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..f54170d
--- /dev/null
+++ b/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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)
+      ),
+      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 TargetPortAuto(false, false).withTimeout(0.1),
+      new AutoControlIntake(-0.6, 0, true).withTimeout(0.1),
+      new DriveDistanceSlow(180).withTimeout(7),
+      new AutoControlIntake(0, 0, true).withTimeout(0.1),
+      new DriveDistance(-180).withTimeout(3),
+      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 TargetPortAuto(false, false).withTimeout(0.1)
+    );
+  }
+}
\ No newline at end of file
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..6857c69
--- /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.00569*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..0c11288
--- /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.06, 0.09, 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(2, 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/subsystems/Climb.java b/2020Code/src/main/java/frc/robot/subsystems/Climb.java
index 1cfc015..fe9cfa1 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Climb.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Climb.java
@@ -9,19 +9,28 @@
 
 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.getAButton()) Winch.set(-1);
+    else 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 e5bde8d..3304cbc 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
@@ -25,7 +25,7 @@ public class DiskControl extends SubsystemBase {
     private Solenoid piston;
 
   public DiskControl() {
-    piston = new Solenoid(30, 2);
+    piston = new Solenoid(30, 3);
     addChild("piston", piston);
   }
 
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
index 24637e4..4c280a0 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -55,6 +55,7 @@ public class Drivetrain extends SubsystemBase {
    */
   public Drivetrain() {
     rightMaster = new CANSparkMax(10, MotorType.kBrushless);
+    rightMaster.restoreFactoryDefaults();
     rightMaster.setInverted(true);
     rightMasterEncoder = rightMaster.getEncoder();
     rightMasterEncoder.setPositionConversionFactor(encoderFactor);
@@ -62,6 +63,7 @@ public Drivetrain() {
     rightMaster.setOpenLoopRampRate(0.1);
 
     rightFollower1 = new CANSparkMax(11, MotorType.kBrushless);
+    rightFollower1.restoreFactoryDefaults();
     rightFollower1.setInverted(true);
     rightFollowerEncoder1 = rightFollower1.getEncoder();
     rightFollowerEncoder1.setPositionConversionFactor(encoderFactor);
@@ -69,6 +71,7 @@ public Drivetrain() {
     rightFollower1.setOpenLoopRampRate(0.1);
 
     rightFollower2 = new CANSparkMax(12, MotorType.kBrushless);
+    rightFollower2.restoreFactoryDefaults();
     rightFollower2.setInverted(true);
     rightFollowerEncoder2 = rightFollower2.getEncoder();
     rightFollowerEncoder2.setPositionConversionFactor(encoderFactor);
@@ -76,6 +79,7 @@ public Drivetrain() {
     rightFollower2.setOpenLoopRampRate(0.1);
 
     leftMaster = new CANSparkMax(15, MotorType.kBrushless);
+    leftMaster.restoreFactoryDefaults();
     leftMaster.setInverted(true);
     leftMasterEncoder = leftMaster.getEncoder();
     leftMasterEncoder.setPositionConversionFactor(encoderFactor);
@@ -83,6 +87,7 @@ public Drivetrain() {
     leftMaster.setOpenLoopRampRate(0.1);
     
     leftFollower1 = new CANSparkMax(14, MotorType.kBrushless);
+    leftFollower1.restoreFactoryDefaults();
     leftFollower1.setInverted(true);
     leftFollowerEncoder1 = leftFollower1.getEncoder();
     leftFollowerEncoder1.setPositionConversionFactor(encoderFactor);
@@ -90,6 +95,7 @@ public Drivetrain() {
     leftFollower1.setOpenLoopRampRate(0.1);
 
     leftFollower2 = new CANSparkMax(13, MotorType.kBrushless);
+    leftFollower2.restoreFactoryDefaults();
     leftFollower2.setInverted(true);
     leftFollowerEncoder2 = leftFollower2.getEncoder();
     leftFollowerEncoder2.setPositionConversionFactor(encoderFactor);
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Shooter.java b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
index 55cd351..c1b3dd8 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
@@ -36,15 +36,12 @@ public Shooter() {
     Shoot2.follow(Shoot1);
     Shoot2.setInverted(false);
     Hood.setBounds(2.0, 1.8, 1.5, 1.2, 1.0);
+    TurretEncoder.setPosition(0);
   }
 
   @Override
   public void periodic() {
-    rollingAverage[pointer] = ShootEncoder.getRate();
-    if(pointer < 4) pointer++;
-    else pointer = 0;
-    SmartDashboard.putNumber("Shooter AVG", getFlywheelSpeed());
-    SmartDashboard.putNumber("Shooter Direct", ShootEncoder.getRate());
+    SmartDashboard.putNumber("Shooter angle", getAngle());
   }
 
   public void setShooterSpeed(double speed) {
@@ -54,7 +51,7 @@ public void setTurretSpeed(double speed) {
     Turret.set(MathUtil.clamp(-speed * 0.1, -0.1, 0.1));
   }
   public double getAngle() {
-    return TurretEncoder.getPosition();
+    return TurretEncoder.getPosition() * 90 / 8.0238;
   }
   public void setShooterVoltage(double volt){
     Shoot1.setVoltage(volt);
@@ -62,7 +59,7 @@ public void setShooterVoltage(double volt){
     SmartDashboard.putNumber("Shooter Direct", ShootEncoder.getRate());
   }
   public void setHood(double value){
-    Hood.set(value);
+    Hood.set(MathUtil.clamp(value, 0, 0.61));
   }
   public double getHood(){
     return Hood.get();
@@ -75,4 +72,7 @@ public double getFlywheelSpeed(){
     total /= 5;
     return total;
   }
+  public void zeroTurretAngle(){
+    TurretEncoder.setPosition(0);
+  }
 }
\ No newline at end of file

From f60642f336b11f37ac88c6687f4d9c65759552f6 Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Tue, 3 Mar 2020 19:14:53 -0500
Subject: [PATCH 08/23] T

---
 .../java/frc/robot/commands/AutoControlIntake.java   |  6 +++++-
 .../main/java/frc/robot/commands/ControlIntake.java  |  2 +-
 .../java/frc/robot/commands/DriveDistanceSlow.java   |  2 +-
 .../main/java/frc/robot/commands/SixBallSimple.java  | 12 +++++++-----
 .../src/main/java/frc/robot/commands/TargetPort.java |  4 ++--
 .../main/java/frc/robot/commands/TargetPortAuto.java |  2 +-
 .../main/java/frc/robot/commands/TurretToAngle.java  |  4 ++--
 .../java/frc/robot/commands/TurretWithJoystick.java  |  2 +-
 .../main/java/frc/robot/subsystems/DiskControl.java  |  2 +-
 .../main/java/frc/robot/subsystems/Drivetrain.java   |  2 +-
 .../java/frc/robot/subsystems/ShooterLimelight.java  |  3 ++-
 11 files changed, 24 insertions(+), 17 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java b/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
index 8f69c95..7f4c871 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
@@ -41,7 +41,11 @@ public void initialize() {
   @Override
   public void execute() {
     frc.robot.RobotContainer.IntakeT.setPiston(m_intakeState);
-    frc.robot.RobotContainer.IndexerT.setMotor(m_indexerSpeed);
+    if(m_intakeSpeed < 0 && frc.robot.RobotContainer.IndexerT.ballAtEnd()){
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.9);
+    } else{
+      frc.robot.RobotContainer.IndexerT.setMotor(m_indexerSpeed);
+    }
     frc.robot.RobotContainer.IntakeT.setMotor(m_intakeSpeed);
   }
 
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
index 35aa680..3aff88f 100644
--- a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
@@ -53,7 +53,7 @@ public void execute() {
       prevButton6 = false;
     }
     if(frc.robot.RobotContainer.m_joystick.getRawButton(1)) {
-      frc.robot.RobotContainer.IndexerT.setMotor(-0.9);
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.7);
     }
     else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
       frc.robot.RobotContainer.IndexerT.setMotor(0.6);
diff --git a/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java b/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
index c8f87d2..614fda2 100644
--- a/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
+++ b/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
@@ -24,7 +24,7 @@ public class DriveDistanceSlow extends CommandBase {
    * @param subsystem The subsystem used by this command.
    */
   double m_distance = 0;
-  PIDController pid = new PIDController(0.05, 0.005, 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);
diff --git a/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java b/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
index f54170d..a33cae5 100644
--- a/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
+++ b/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
@@ -16,20 +16,22 @@ public SixBallSimple() {
     addCommands(
       new ParallelCommandGroup(
         new AutoControlIntake(0, 0, true).withTimeout(1),
-        new TurretToAngle(150)
+        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(3),
-        new AutoControlIntake(0, -0.8, true).withTimeout(3)
+        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(180).withTimeout(7),
       new AutoControlIntake(0, 0, true).withTimeout(0.1),
-      new DriveDistance(-180).withTimeout(3),
-      new TargetPortAuto(false, true).withTimeout(2),
+      new ParallelCommandGroup(
+        new DriveDistance(-180).withTimeout(3),
+        new TargetPortAuto(false, true).withTimeout(3)
+      ),
       new TargetPortAuto(true, true).withTimeout(0.5),
       new ParallelCommandGroup(
         new TargetPortAuto(true, true).withTimeout(3),
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index df76463..31cdee3 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -59,8 +59,8 @@ public void execute() {
     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()) {
-      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
-      SmartDashboard.putNumber("Hood Auto", -0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
+      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00549*distance-0.0000117*(Math.pow(distance, 2)));
+      SmartDashboard.putNumber("Hood Auto", -0.137+0.00569*distance-0.0000118*(Math.pow(distance, 2)));
       angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + 1.2;
       frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
     }
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
index 6857c69..c6e718e 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
@@ -52,7 +52,7 @@ 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.00569*distance-0.0000113*(Math.pow(distance, 2)));
+    frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000110*(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));
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java b/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
index 0c11288..7676c53 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
@@ -22,7 +22,7 @@ public class TurretToAngle extends CommandBase {
    * @param subsystem The subsystem used by this command.
    */
   double m_angle;
-  PIDController pid = new PIDController(0.06, 0.09, 0.0);
+  PIDController pid = new PIDController(0.07, 0.05, 0.0);
   public TurretToAngle(double angle) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(frc.robot.RobotContainer.ShooterT);
@@ -32,7 +32,7 @@ public TurretToAngle(double angle) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pid.setTolerance(2, 10);
+    pid.setTolerance(5, 10);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
index a844d93..eaa8ac4 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -38,7 +38,7 @@ public void initialize() {
   // 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));
+    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 frc.robot.RobotContainer.ShooterT.setShooterVoltage(0);
     frc.robot.RobotContainer.ShooterT.setHood(hoodOut);
diff --git a/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java b/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
index 3304cbc..d61238a 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/DiskControl.java
@@ -25,7 +25,7 @@ public class DiskControl extends SubsystemBase {
     private Solenoid piston;
 
   public DiskControl() {
-    piston = new Solenoid(30, 3);
+    piston = new Solenoid(0, 3);
     addChild("piston", piston);
   }
 
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
index 4c280a0..36bde53 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -61,7 +61,7 @@ public Drivetrain() {
     rightMasterEncoder.setPositionConversionFactor(encoderFactor);
     rightMasterEncoder.setVelocityConversionFactor(encoderFactor);
     rightMaster.setOpenLoopRampRate(0.1);
-
+  
     rightFollower1 = new CANSparkMax(11, MotorType.kBrushless);
     rightFollower1.restoreFactoryDefaults();
     rightFollower1.setInverted(true);
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

From c8961c559c76915c2a03dd53df9e11d82f8761b1 Mon Sep 17 00:00:00 2001
From: NormallyNormal <33298484+NormallyNormal@users.noreply.github.com>
Date: Tue, 3 Mar 2020 20:29:59 -0500
Subject: [PATCH 09/23] WORKING 1.5 over auto

---
 .../main/java/frc/robot/commands/AutoControlIntake.java   | 8 ++++++--
 .../src/main/java/frc/robot/commands/ControlIntake.java   | 4 ++--
 2020Code/src/main/java/frc/robot/commands/TargetPort.java | 4 ++--
 .../src/main/java/frc/robot/commands/TargetPortAuto.java  | 2 +-
 .../src/main/java/frc/robot/commands/TurretToAngle.java   | 2 +-
 2020Code/src/main/java/frc/robot/subsystems/Shooter.java  | 1 +
 6 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java b/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
index 7f4c871..384efd7 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoControlIntake.java
@@ -42,8 +42,12 @@ public void initialize() {
   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.9);
-    } else{
+      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);
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
index 3aff88f..892bca2 100644
--- a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
@@ -53,10 +53,10 @@ public void execute() {
       prevButton6 = false;
     }
     if(frc.robot.RobotContainer.m_joystick.getRawButton(1)) {
-      frc.robot.RobotContainer.IndexerT.setMotor(-0.7);
+      frc.robot.RobotContainer.IndexerT.setMotor(-0.8);
     }
     else if(frc.robot.RobotContainer.m_joystick.getRawButton(8)){
-      frc.robot.RobotContainer.IndexerT.setMotor(0.6);
+      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);
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index 31cdee3..81ae752 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -59,8 +59,8 @@ public void execute() {
     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()) {
-      frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00549*distance-0.0000117*(Math.pow(distance, 2)));
-      SmartDashboard.putNumber("Hood Auto", -0.137+0.00569*distance-0.0000118*(Math.pow(distance, 2)));
+      if(!frc.robot.RobotContainer.m_joystick.getRawButton(1)) frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00569*distance-0.0000113*(Math.pow(distance, 2)));
+      SmartDashboard.putNumber("Hood Auto", -0.137+0.00489*distance-0.0000118*(Math.pow(distance, 2)));
       angle = frc.robot.RobotContainer.ShooterLimelightT.getXSkew() + 1.2;
       frc.robot.RobotContainer.ShooterT.setTurretSpeed(pid.calculate(angle, 0));
     }
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
index c6e718e..f4d8fb2 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
@@ -52,7 +52,7 @@ 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.00569*distance-0.0000110*(Math.pow(distance, 2)));
+    frc.robot.RobotContainer.ShooterT.setHood(-0.137+0.00489*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));
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java b/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
index 7676c53..dadb4a2 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretToAngle.java
@@ -22,7 +22,7 @@ public class TurretToAngle extends CommandBase {
    * @param subsystem The subsystem used by this command.
    */
   double m_angle;
-  PIDController pid = new PIDController(0.07, 0.05, 0.0);
+  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);
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Shooter.java b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
index c1b3dd8..3af86d5 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Shooter.java
@@ -51,6 +51,7 @@ public void setTurretSpeed(double speed) {
     Turret.set(MathUtil.clamp(-speed * 0.1, -0.1, 0.1));
   }
   public double getAngle() {
+    SmartDashboard.putNumber("Shooter angle", TurretEncoder.getPosition() * 90 / 8.0238);
     return TurretEncoder.getPosition() * 90 / 8.0238;
   }
   public void setShooterVoltage(double volt){

From a03e7fb9af04899d14321e9ed0fcfdac4eb5a1c2 Mon Sep 17 00:00:00 2001
From: Natalie <bubumk10@gmail.com>
Date: Wed, 4 Mar 2020 21:00:39 -0500
Subject: [PATCH 10/23] COMP READY!

---
 2020Code/src/main/java/frc/robot/Robot.java               | 2 +-
 2020Code/src/main/java/frc/robot/RobotContainer.java      | 8 +++++---
 .../src/main/java/frc/robot/commands/ControlIntake.java   | 2 +-
 2020Code/src/main/java/frc/robot/commands/TargetPort.java | 3 +--
 .../src/main/java/frc/robot/commands/TargetPortAuto.java  | 2 +-
 2020Code/src/main/java/frc/robot/subsystems/Climb.java    | 3 +--
 6 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/Robot.java b/2020Code/src/main/java/frc/robot/Robot.java
index aad43a0..4c835fb 100644
--- a/2020Code/src/main/java/frc/robot/Robot.java
+++ b/2020Code/src/main/java/frc/robot/Robot.java
@@ -107,7 +107,7 @@ public void teleopInit() {
     // 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 94e5f15..4d62bb0 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -62,13 +62,15 @@ private void configureButtonBindings() {
     //final JoystickButton b = new JoystickButton(m_joystick, 2);
     // final JoystickButton x = new JoystickButton(m_joystick, 3);
     final JoystickButton leftBumper = new JoystickButton(m_joystick, 4);
-    final JoystickButton climbButton = new JoystickButton(m_driverController, 8);
+    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());
     leftBumper.toggleWhenPressed(new TargetPort());
-    climbButton.whenPressed(new SetClimb(true));
+    climbButtonUp.whenPressed(new SetClimb(true));
+    climbButtonDown.whenPressed(new SetClimb(false));
   }
 
 
@@ -78,6 +80,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return new SixBallSimple();
+    return new SixBallSimple().withTimeout(20);
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
index 892bca2..6e2c105 100644
--- a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
@@ -36,7 +36,7 @@ public void initialize() {
   @Override
   public void execute() {
     if(frc.robot.RobotContainer.m_joystick.getRawButton(2)) {
-      frc.robot.RobotContainer.IntakeT.setMotor(-0.6);
+      frc.robot.RobotContainer.IntakeT.setMotor(-0.8);
     }
     else {
       if(frc.robot.RobotContainer.m_joystick.getRawButton(3)) {
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPort.java b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
index 81ae752..8f5352d 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPort.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPort.java
@@ -59,8 +59,7 @@ public void execute() {
     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.00569*distance-0.0000113*(Math.pow(distance, 2)));
-      SmartDashboard.putNumber("Hood Auto", -0.137+0.00489*distance-0.0000118*(Math.pow(distance, 2)));
+      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));
     }
diff --git a/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
index f4d8fb2..bc14770 100644
--- a/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
+++ b/2020Code/src/main/java/frc/robot/commands/TargetPortAuto.java
@@ -52,7 +52,7 @@ 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.00489*distance-0.0000113*(Math.pow(distance, 2)));
+    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));
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Climb.java b/2020Code/src/main/java/frc/robot/subsystems/Climb.java
index fe9cfa1..3207a1b 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Climb.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Climb.java
@@ -25,8 +25,7 @@ public Climb() {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    if(frc.robot.RobotContainer.m_driverController.getAButton()) Winch.set(-1);
-    else if(frc.robot.RobotContainer.m_driverController.getBButton()) Winch.set(1);
+    if(frc.robot.RobotContainer.m_driverController.getBButton()) Winch.set(1);
     else Winch.set(0);
   }
 

From e06d08d8d203a7bbb9f604ce0073b58e5e2c19e2 Mon Sep 17 00:00:00 2001
From: Natalie <bubumk10@gmail.com>
Date: Sun, 8 Mar 2020 14:55:40 -0400
Subject: [PATCH 11/23] upload

---
 2020Code/src/main/java/frc/robot/Robot.java          |  2 ++
 2020Code/src/main/java/frc/robot/RobotContainer.java |  2 +-
 .../main/java/frc/robot/commands/ControlIntake.java  |  2 +-
 .../java/frc/robot/commands/DriveDistanceSlow.java   |  2 +-
 .../src/main/java/frc/robot/commands/SetClimb.java   |  2 --
 .../main/java/frc/robot/commands/SixBallSimple.java  |  4 ++--
 .../java/frc/robot/commands/TurretWithJoystick.java  |  1 +
 .../main/java/frc/robot/subsystems/Drivetrain.java   | 12 ++++++------
 .../main/java/frc/robot/subsystems/Pneumatics.java   |  7 +++++++
 9 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/Robot.java b/2020Code/src/main/java/frc/robot/Robot.java
index 4c835fb..1072452 100644
--- a/2020Code/src/main/java/frc/robot/Robot.java
+++ b/2020Code/src/main/java/frc/robot/Robot.java
@@ -71,6 +71,7 @@ public void robotPeriodic() {
   @Override
   public void disabledInit() {
     frc.robot.RobotContainer.ShooterT.zeroTurretAngle();
+    frc.robot.RobotContainer.PneumaticsT.setCompressor(true);
   }
 
   @Override
@@ -84,6 +85,7 @@ public void disabledPeriodic() {
   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)
diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index 4d62bb0..0ae9061 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -80,6 +80,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return new SixBallSimple().withTimeout(20);
+    return new ThreeBallFar().withTimeout(20);
   }
 }
diff --git a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
index 6e2c105..892bca2 100644
--- a/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
+++ b/2020Code/src/main/java/frc/robot/commands/ControlIntake.java
@@ -36,7 +36,7 @@ public void initialize() {
   @Override
   public void execute() {
     if(frc.robot.RobotContainer.m_joystick.getRawButton(2)) {
-      frc.robot.RobotContainer.IntakeT.setMotor(-0.8);
+      frc.robot.RobotContainer.IntakeT.setMotor(-0.6);
     }
     else {
       if(frc.robot.RobotContainer.m_joystick.getRawButton(3)) {
diff --git a/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java b/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
index 614fda2..98a64a7 100644
--- a/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
+++ b/2020Code/src/main/java/frc/robot/commands/DriveDistanceSlow.java
@@ -34,7 +34,7 @@ public DriveDistanceSlow(double distance) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pid.setTolerance(2, 10);
+    pid.setTolerance(5, 10);
     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/SetClimb.java b/2020Code/src/main/java/frc/robot/commands/SetClimb.java
index 72ba9b9..9c07148 100644
--- a/2020Code/src/main/java/frc/robot/commands/SetClimb.java
+++ b/2020Code/src/main/java/frc/robot/commands/SetClimb.java
@@ -7,9 +7,7 @@
 
 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.
diff --git a/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java b/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
index a33cae5..5b4cd86 100644
--- a/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
+++ b/2020Code/src/main/java/frc/robot/commands/SixBallSimple.java
@@ -26,10 +26,10 @@ public SixBallSimple() {
       ),
       new TargetPortAuto(false, false).withTimeout(0.1),
       new AutoControlIntake(-0.6, 0, true).withTimeout(0.1),
-      new DriveDistanceSlow(180).withTimeout(7),
+      new DriveDistanceSlow(110).withTimeout(5),
       new AutoControlIntake(0, 0, true).withTimeout(0.1),
       new ParallelCommandGroup(
-        new DriveDistance(-180).withTimeout(3),
+        new DriveDistance(-110).withTimeout(3),
         new TargetPortAuto(false, true).withTimeout(3)
       ),
       new TargetPortAuto(true, true).withTimeout(0.5),
diff --git a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
index eaa8ac4..753205c 100644
--- a/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
+++ b/2020Code/src/main/java/frc/robot/commands/TurretWithJoystick.java
@@ -40,6 +40,7 @@ public void initialize() {
   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;
diff --git a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
index 36bde53..b30072a 100644
--- a/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/2020Code/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -60,7 +60,7 @@ public Drivetrain() {
     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();
@@ -68,7 +68,7 @@ public Drivetrain() {
     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();
@@ -76,7 +76,7 @@ public Drivetrain() {
     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();
@@ -84,7 +84,7 @@ public Drivetrain() {
     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();
@@ -92,7 +92,7 @@ public Drivetrain() {
     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();
@@ -100,7 +100,7 @@ public Drivetrain() {
     leftFollowerEncoder2 = leftFollower2.getEncoder();
     leftFollowerEncoder2.setPositionConversionFactor(encoderFactor);
     leftFollowerEncoder2.setVelocityConversionFactor(encoderFactor);
-    leftFollower2.setOpenLoopRampRate(0.1);
+    leftFollower2.setOpenLoopRampRate(0.25);
     
 
     leftFollower1.follow(leftMaster);
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();
+  }
 }

From 40c2e3f60e5a41ca60354899702f00c79854734d Mon Sep 17 00:00:00 2001
From: Natalie <bubumk10@gmail.com>
Date: Sat, 13 Mar 2021 15:17:26 -0500
Subject: [PATCH 12/23] changed some PID stuff

changed a few things in PID and worked on auto -Nia
---
 .../main/java/frc/robot/RobotContainer.java   |  2 +-
 .../java/frc/robot/commands/AutoTest.java     | 59 ++++++++++++++++++-
 .../java/frc/robot/commands/DriveArc.java     |  2 +
 .../frc/robot/commands/DriveDistance.java     |  3 +-
 4 files changed, 61 insertions(+), 5 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/RobotContainer.java b/2020Code/src/main/java/frc/robot/RobotContainer.java
index 0ae9061..f602cc0 100644
--- a/2020Code/src/main/java/frc/robot/RobotContainer.java
+++ b/2020Code/src/main/java/frc/robot/RobotContainer.java
@@ -80,6 +80,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return new ThreeBallFar().withTimeout(20);
+    return new AutoTest(); //.withTimeout(20);
   }
 }
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/DriveArc.java b/2020Code/src/main/java/frc/robot/commands/DriveArc.java
index 416de23..6bc4ca4 100644
--- a/2020Code/src/main/java/frc/robot/commands/DriveArc.java
+++ b/2020Code/src/main/java/frc/robot/commands/DriveArc.java
@@ -42,6 +42,7 @@ public void initialize() {
     frc.robot.RobotContainer.DrivetrainT.allowDrive(false);
     frc.robot.RobotContainer.DrivetrainT.resetEncoders();
     output = 0.6 / (m_radius+wheelDistance/2) * (m_radius-wheelDistance/2);
+    System.out.println("hello world");
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -64,6 +65,7 @@ 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.
diff --git a/2020Code/src/main/java/frc/robot/commands/DriveDistance.java b/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
index f1efe9c..ceed2cd 100644
--- a/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
+++ b/2020Code/src/main/java/frc/robot/commands/DriveDistance.java
@@ -23,6 +23,7 @@ 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.05, 0.005, 0);
   public DriveDistance(double distance) {
@@ -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();

From b673d17dfd150b11f12d43d904100d4b4af83a7a Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Mon, 5 Apr 2021 17:36:46 -0400
Subject: [PATCH 13/23] Slalom Path AutoTest

Slalom Path AutoTest
---
 .../java/frc/robot/commands/AutoTest2.java    | 32 +++++++++++++++++
 .../java/frc/robot/commands/AutoTest3.java    | 34 +++++++++++++++++++
 .../frc/robot/commands/BarrelRunTest.java     | 29 ++++++++++++++++
 3 files changed, 95 insertions(+)
 create mode 100644 2020Code/src/main/java/frc/robot/commands/AutoTest2.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/AutoTest3.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java

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..66ab7a5
--- /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 = 125; //scale 106%
+  
+    public AutoTest2() {
+        addCommands(
+            new DriveArc(55, -90), //90 degree circle counterclockwise
+            new DriveArc(15, 90), //90 degree circle clockwise
+            new DriveDistance(125), //move forward 125 inches
+            new DriveArc(45, 90),  //90 degree circle clockwise
+            new DriveArc(45, 360), //full circle 360 degrees 
+            new DriveArc(75, -90), //90 degree circle counterclockwise
+            new DriveDistance(125), //move forward 125 inches
+            new DriveArc(15, 90), //90 degree circle clockwise
+            new DriveArc(55, -90)  //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..128aeee
--- /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(35, -90), //90 degree circle counterclowise
+           new DriveArc(10,170), //170 degrees clockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(30, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(10, 180), //180 degrees clockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(35, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(100, 180), //180 degrees clockwise
+           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..12e715b
--- /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, 360), //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

From 763ab5c02c659211f1c4d753fd4bb145c85951b5 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Mon, 5 Apr 2021 17:41:40 -0400
Subject: [PATCH 14/23] Slalom and Bounce Path AutoTests

Slalom and Bounce Path AutoTests
---
 .../java/frc/robot/commands/AutoTest2.java    | 32 -----------------
 .../java/frc/robot/commands/AutoTest3.java    | 34 -------------------
 .../frc/robot/commands/BarrelRunTest.java     | 29 ----------------
 3 files changed, 95 deletions(-)
 delete mode 100644 2020Code/src/main/java/frc/robot/commands/AutoTest2.java
 delete mode 100644 2020Code/src/main/java/frc/robot/commands/AutoTest3.java
 delete mode 100644 2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
deleted file mode 100644
index 66ab7a5..0000000
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
+++ /dev/null
@@ -1,32 +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.SequentialCommandGroup;
-
-
-public class AutoTest2 extends SequentialCommandGroup {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-
-    public static final double DriveDist = 125; //scale 106%
-  
-    public AutoTest2() {
-        addCommands(
-            new DriveArc(55, -90), //90 degree circle counterclockwise
-            new DriveArc(15, 90), //90 degree circle clockwise
-            new DriveDistance(125), //move forward 125 inches
-            new DriveArc(45, 90),  //90 degree circle clockwise
-            new DriveArc(45, 360), //full circle 360 degrees 
-            new DriveArc(75, -90), //90 degree circle counterclockwise
-            new DriveDistance(125), //move forward 125 inches
-            new DriveArc(15, 90), //90 degree circle clockwise
-            new DriveArc(55, -90)  //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
deleted file mode 100644
index 128aeee..0000000
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
+++ /dev/null
@@ -1,34 +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.SequentialCommandGroup;
-
-
-public class AutoTest3 extends SequentialCommandGroup {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-
-    public static final double DriveDistance = 65; 
-  
-    public AutoTest3() {
-        addCommands(
-           new DriveArc(35, -90), //90 degree circle counterclowise
-           new DriveArc(10,170), //170 degrees clockwise
-           new DriveDistance(65), //drive forward 65 inches
-           new DriveArc(30, -180), //180 degrees counterclockwise
-           new DriveDistance(65), //drive forward 65 inches
-           new DriveArc(10, 180), //180 degrees clockwise
-           new DriveDistance(65), //drive forward 65 inches
-           new DriveArc(35, -180), //180 degrees counterclockwise
-           new DriveDistance(65), //drive forward 65 inches
-           new DriveArc(100, 180), //180 degrees clockwise
-           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
deleted file mode 100644
index 12e715b..0000000
--- a/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java
+++ /dev/null
@@ -1,29 +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.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, 360), //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

From 5fd7c78ac01fe41e88992bcc24dce5195ebe1c07 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Mon, 5 Apr 2021 17:46:55 -0400
Subject: [PATCH 15/23] Revert "Slalom and Bounce Path AutoTests"

This reverts commit 763ab5c02c659211f1c4d753fd4bb145c85951b5.
---
 .../java/frc/robot/commands/AutoTest2.java    | 32 +++++++++++++++++
 .../java/frc/robot/commands/AutoTest3.java    | 34 +++++++++++++++++++
 .../frc/robot/commands/BarrelRunTest.java     | 29 ++++++++++++++++
 3 files changed, 95 insertions(+)
 create mode 100644 2020Code/src/main/java/frc/robot/commands/AutoTest2.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/AutoTest3.java
 create mode 100644 2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java

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..66ab7a5
--- /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 = 125; //scale 106%
+  
+    public AutoTest2() {
+        addCommands(
+            new DriveArc(55, -90), //90 degree circle counterclockwise
+            new DriveArc(15, 90), //90 degree circle clockwise
+            new DriveDistance(125), //move forward 125 inches
+            new DriveArc(45, 90),  //90 degree circle clockwise
+            new DriveArc(45, 360), //full circle 360 degrees 
+            new DriveArc(75, -90), //90 degree circle counterclockwise
+            new DriveDistance(125), //move forward 125 inches
+            new DriveArc(15, 90), //90 degree circle clockwise
+            new DriveArc(55, -90)  //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..128aeee
--- /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(35, -90), //90 degree circle counterclowise
+           new DriveArc(10,170), //170 degrees clockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(30, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(10, 180), //180 degrees clockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(35, -180), //180 degrees counterclockwise
+           new DriveDistance(65), //drive forward 65 inches
+           new DriveArc(100, 180), //180 degrees clockwise
+           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..12e715b
--- /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, 360), //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

From b4a4180bf747dc89dd01890a8c6c3d3e476d4533 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Mon, 5 Apr 2021 17:52:10 -0400
Subject: [PATCH 16/23] Update AutoTest3.java

---
 2020Code/src/main/java/frc/robot/commands/AutoTest3.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
index 128aeee..42716ea 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
@@ -23,11 +23,11 @@ public AutoTest3() {
            new DriveDistance(65), //drive forward 65 inches
            new DriveArc(30, -180), //180 degrees counterclockwise
            new DriveDistance(65), //drive forward 65 inches
-           new DriveArc(10, 180), //180 degrees clockwise
+           new TurnAngle(180), //Turn 180 degrees
            new DriveDistance(65), //drive forward 65 inches
            new DriveArc(35, -180), //180 degrees counterclockwise
            new DriveDistance(65), //drive forward 65 inches
-           new DriveArc(100, 180), //180 degrees clockwise
+           new TurnAngle(180), //Turn 180 degrees
            new DriveArc(35, -90) //90 degrees counterclockwise
             ); 
         }   

From 5fc4b5ea9233f433f73a40bbc9e384ce3f0855ca Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Mon, 5 Apr 2021 18:22:49 -0400
Subject: [PATCH 17/23] Update AutoTest3.java

---
 2020Code/src/main/java/frc/robot/commands/AutoTest3.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
index 42716ea..59acaa2 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
@@ -19,9 +19,9 @@ public class AutoTest3 extends SequentialCommandGroup {
     public AutoTest3() {
         addCommands(
            new DriveArc(35, -90), //90 degree circle counterclowise
-           new DriveArc(10,170), //170 degrees clockwise
+           new TurnAngle(170), //170 degrees clockwise
            new DriveDistance(65), //drive forward 65 inches
-           new DriveArc(30, -180), //180 degrees counterclockwise
+           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

From 59070ec2bbed262089565ec6c1b2b262e4f5e4a8 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Tue, 6 Apr 2021 10:15:02 -0400
Subject: [PATCH 18/23] AutoTest2

Slalom Path
---
 .../java/frc/robot/commands/AutoTest2.java    | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
index 66ab7a5..ea0ec3b 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
@@ -14,19 +14,19 @@
 public class AutoTest2 extends SequentialCommandGroup {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
 
-    public static final double DriveDist = 125; //scale 106%
+    public static final double DriveDist = 100; 
   
     public AutoTest2() {
         addCommands(
-            new DriveArc(55, -90), //90 degree circle counterclockwise
-            new DriveArc(15, 90), //90 degree circle clockwise
-            new DriveDistance(125), //move forward 125 inches
-            new DriveArc(45, 90),  //90 degree circle clockwise
-            new DriveArc(45, 360), //full circle 360 degrees 
-            new DriveArc(75, -90), //90 degree circle counterclockwise
-            new DriveDistance(125), //move forward 125 inches
-            new DriveArc(15, 90), //90 degree circle clockwise
-            new DriveArc(55, -90)  //90 degree circle counterclockwise
+            new DriveArc(55, -68), //90 degree circle counterclockwise
+            new DriveArc(15, 59), //90 degree circle clockwise
+            new DriveDistance(90), //move forward 125 inches
+            new DriveArc(45, 65),  //90 degree circle clockwise
+            new DriveArc(45, 280), //full circle 360 degrees 
+            new DriveArc(55, -68), //90 degree circle counterclockwise
+            new DriveDistance(120), //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

From cbd09b0f76b8efad9f84b944f7c5c7f85972b834 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Tue, 6 Apr 2021 10:46:21 -0400
Subject: [PATCH 19/23] Changes to Slalom and Bounce Paths

Changes to Slalom and Bounce Paths
---
 2020Code/src/main/java/frc/robot/commands/AutoTest2.java | 6 +++---
 2020Code/src/main/java/frc/robot/commands/AutoTest3.java | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
index ea0ec3b..18eae64 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
@@ -20,11 +20,11 @@ public AutoTest2() {
         addCommands(
             new DriveArc(55, -68), //90 degree circle counterclockwise
             new DriveArc(15, 59), //90 degree circle clockwise
-            new DriveDistance(90), //move forward 125 inches
+            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(55, -68), //90 degree circle counterclockwise
-            new DriveDistance(120), //move forward 125 inches
+            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
             ); 
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
index 59acaa2..b52d163 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
@@ -18,14 +18,14 @@ public class AutoTest3 extends SequentialCommandGroup {
   
     public AutoTest3() {
         addCommands(
-           new DriveArc(35, -90), //90 degree circle counterclowise
-           new TurnAngle(170), //170 degrees clockwise
+           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(35, -180), //180 degrees counterclockwise
+           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

From 1b217077f75ad513c75f0701cf62ac5af88934d5 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Tue, 6 Apr 2021 11:00:05 -0400
Subject: [PATCH 20/23] Revert "Changes to Slalom and Bounce Paths"

This reverts commit cbd09b0f76b8efad9f84b944f7c5c7f85972b834.
---
 2020Code/src/main/java/frc/robot/commands/AutoTest2.java | 6 +++---
 2020Code/src/main/java/frc/robot/commands/AutoTest3.java | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
index 18eae64..ea0ec3b 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
@@ -20,11 +20,11 @@ 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 DriveDistance(90), //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(55, -68), //90 degree circle counterclockwise
+            new DriveDistance(120), //move forward 125 inches
             new DriveArc(15, 59), //90 degree circle clockwise
             new DriveArc(55, -68)  //90 degree circle counterclockwise
             ); 
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
index b52d163..59acaa2 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
@@ -18,14 +18,14 @@ public class AutoTest3 extends SequentialCommandGroup {
   
     public AutoTest3() {
         addCommands(
-           new DriveArc(45, -90), //90 degree circle counterclowise
-           new TurnAngle(171), //170 degrees clockwise
+           new DriveArc(35, -90), //90 degree circle counterclowise
+           new TurnAngle(170), //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 DriveArc(35, -180), //180 degrees counterclockwise
            new DriveDistance(65), //drive forward 65 inches
            new TurnAngle(180), //Turn 180 degrees
            new DriveArc(35, -90) //90 degrees counterclockwise

From e74c88f42768ac9bab0a094f3c4f331358518b8b Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Tue, 6 Apr 2021 11:00:33 -0400
Subject: [PATCH 21/23] Revert "Revert "Changes to Slalom and Bounce Paths""

This reverts commit 1b217077f75ad513c75f0701cf62ac5af88934d5.
---
 2020Code/src/main/java/frc/robot/commands/AutoTest2.java | 6 +++---
 2020Code/src/main/java/frc/robot/commands/AutoTest3.java | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
index ea0ec3b..18eae64 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest2.java
@@ -20,11 +20,11 @@ public AutoTest2() {
         addCommands(
             new DriveArc(55, -68), //90 degree circle counterclockwise
             new DriveArc(15, 59), //90 degree circle clockwise
-            new DriveDistance(90), //move forward 125 inches
+            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(55, -68), //90 degree circle counterclockwise
-            new DriveDistance(120), //move forward 125 inches
+            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
             ); 
diff --git a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
index 59acaa2..b52d163 100644
--- a/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
+++ b/2020Code/src/main/java/frc/robot/commands/AutoTest3.java
@@ -18,14 +18,14 @@ public class AutoTest3 extends SequentialCommandGroup {
   
     public AutoTest3() {
         addCommands(
-           new DriveArc(35, -90), //90 degree circle counterclowise
-           new TurnAngle(170), //170 degrees clockwise
+           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(35, -180), //180 degrees counterclockwise
+           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

From 9b13c7f8afc507d8070bc9d45b829b6c3ec00c91 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Tue, 6 Apr 2021 19:15:53 -0400
Subject: [PATCH 22/23] BarrelRunTest

BarrelRunTest
---
 .DS_Store                                        | Bin 0 -> 6148 bytes
 .../java/frc/robot/commands/BarrelRunTest.java   |   2 +-
 2 files changed, 1 insertion(+), 1 deletion(-)
 create mode 100644 .DS_Store

diff --git a/.DS_Store b/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..222501b65f2c103d59498c590baecb0291510ee3
GIT binary patch
literal 6148
zcmeHKO>Wab6n@hN>QEt4gp@A5L1Gc2q%Dn5g%l<2A`(JKL9hUn+I47LTu+oZ7zjb$
za0oa65=Y?#9E1bFH*W@T64*w7=2>sP@yz@2e6rtoh)A?1L7k{XL=IG8;S!qPh?-Zr
zBx`0wO$v_DCP@MHJCQtDvbF=30n5PO#(@5~cc?~6bKkEYNgofZ3c~^P#y1vze1~mw
z_I^4Z8vD=wFbc;>t@cH%trsp_+;ECTr+C?U+8xNWn|R4&-10_``7R9lzUe=ZGWyx~
z{D)yb?Uk;zWt@0n+#lu4pF9fVK&CA@iGwl6x$h+d_;R9NX?r@o)u>n8o&DOZ;!Ycl
zTE*Sl-Ji`Ir+mGBr*nM%>h<)^?CpDA3JDZ0mMaE-zz1}e7W$$)iX$1HVov#OEfPUD
zsYB;9L>U}WLVXHUDa=v%30`?B%S#cebfSwWVx=7CIgJs~!#Ee#U(rN)A=*_Mz_y3H
zBV0Xrg($~+bekHqOI3;x?{xl&#JhzUn{*dzXwwnhqb}Mi*71^(1+T9QUR%gJpb0(0
z6(PTmOdg&gfd3ihB*pU)_@(-od#Mo6%4!)fufDUn+O#&CWxz7<4=|wWg9}yIG?;1R
z(t(DL0Kfu<S<qL%49dqf*ff}F#0*4eSD<zk=7=G*JH}mO*EE=E)b1qA;X|0r!kkb9
zXGeUO%}F#h+SW2)8JK5aUEh|}{(to6^Z&e)y|WBh2L3AsMB%V|*us*`-nz6nwb#1P
rW2h2kmuX}dG-f;Ij@pX1p)8nlRR`EKm}$fa#Qq2{4YsiitdxQ8;4!T!

literal 0
HcmV?d00001

diff --git a/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java b/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java
index 12e715b..5fc5f1b 100644
--- a/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java
+++ b/2020Code/src/main/java/frc/robot/commands/BarrelRunTest.java
@@ -18,7 +18,7 @@ public class BarrelRunTest extends SequentialCommandGroup {
     public BarrelRunTest() {
         addCommands(
                 new DriveDistance(90), //move forward 7.5 feet
-                new DriveArc(30, 360), //do a full circle clockwise
+                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

From 2807c417fbcc297dad01753adb70fb30c6138c73 Mon Sep 17 00:00:00 2001
From: SahilP113 <65868166+SahilP113@users.noreply.github.com>
Date: Tue, 4 May 2021 18:16:35 -0400
Subject: [PATCH 23/23] Create ChooseAutoTest.java

Code to choose an auto test.
---
 .../frc/robot/commands/ChooseAutoTest.java    | 34 +++++++++++++++++++
 1 file changed, 34 insertions(+)
 create mode 100644 2020Code/src/main/java/frc/robot/commands/ChooseAutoTest.java

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);
+        
+    }
+}
+
+
+