From a14f94fc91ad7f9dfe56d2638a4239913805073f Mon Sep 17 00:00:00 2001
From: Sean-Weber <803074@seq.org>
Date: Sat, 22 May 2021 11:20:38 -0700
Subject: [PATCH 01/11] Add Servo for Limelight/toggle button

---
 .../java/org/team199/robot2021/Constants.java  |  2 ++
 .../org/team199/robot2021/RobotContainer.java  |  2 +-
 .../team199/robot2021/subsystems/Intake.java   | 18 +++++++++++++++++-
 3 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/team199/robot2021/Constants.java b/src/main/java/org/team199/robot2021/Constants.java
index 2d53310..2705139 100644
--- a/src/main/java/org/team199/robot2021/Constants.java
+++ b/src/main/java/org/team199/robot2021/Constants.java
@@ -235,6 +235,8 @@ public static final class Controller {
             // intake/feeder
             public static final int kIntakeButton = X;
             public static final int kOuttakeButton = Y;
+            // limelight toggle
+            public static final int kLimelightToggleButton = 0; //TODO: set button
             //public static final int kRegurgitateButton = B;
         }
     }
diff --git a/src/main/java/org/team199/robot2021/RobotContainer.java b/src/main/java/org/team199/robot2021/RobotContainer.java
index fb43399..028bc2f 100644
--- a/src/main/java/org/team199/robot2021/RobotContainer.java
+++ b/src/main/java/org/team199/robot2021/RobotContainer.java
@@ -193,7 +193,7 @@ private void configureButtonBindingsController() {
         new JoystickButton(controller, Constants.OI.Controller.B).whenPressed(new InstantCommand(() -> { SmartDashboard.putBoolean("Field Oriented", !SmartDashboard.getBoolean("Field Oriented", true)); }));
         // Intake toggle button
         new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake));
-
+        new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> {intake.changeServoStatus(!intake.isLimelightSearching());}));
         // Power cell regurgitate button
         //new JoystickButton(controller, Constants.OI.Controller.kRegurgitateButton).whileHeld(new Regurgitate(intake, feeder));
 
diff --git a/src/main/java/org/team199/robot2021/subsystems/Intake.java b/src/main/java/org/team199/robot2021/subsystems/Intake.java
index d4199e3..7be50fa 100644
--- a/src/main/java/org/team199/robot2021/subsystems/Intake.java
+++ b/src/main/java/org/team199/robot2021/subsystems/Intake.java
@@ -11,17 +11,21 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.lib.MotorControllerFactory;
-
+import edu.wpi.first.wpilibj.Servo;
 public class Intake extends SubsystemBase {
     // TODO: find good values and then set to final
     private static double kIntakeSpeed = .6;
     private static double kSlowSpeed = -0.1;
+    private static double kLimeBottomAngle = 0; //TODO: set correct angle
+    private static double kLimeTopAngle = 0; //TODO: set correct angle
 
     private final CANSparkMax rollerMotor = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kIntakeRoller);
+    private final Servo limelightServo = new Servo(0); //TODO: set proper channel
     private final DoubleSolenoid intakePistons1 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[2], Constants.DrivePorts.kIntakePistons[3]);
     private final DoubleSolenoid intakePistons2 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[0], Constants.DrivePorts.kIntakePistons[1]);
 
     private boolean deployed = false;
+    private boolean isLimelightSearching = false;
 
     /**
      * Vectored intake that rolls balls through the bumper gap and into feeder.
@@ -77,5 +81,17 @@ public void doTheFlop() {
     public boolean isDeployed() {
         return deployed;
     }
+    public boolean isLimelightSearching() {
+        return isLimelightSearching;
+    }
+    public void changeServoStatus(boolean status) {
+        isLimelightSearching = status;
+        if (isLimelightSearching() == false) {
+            limelightServo.setAngle(kLimeBottomAngle); // TODO: set correct angle
+        } else {
+            limelightServo.setAngle(kLimeTopAngle); // TODO: set correct angle
+        }
+    }
+
 
 }
\ No newline at end of file

From 14ea07d175ede209027b2a521e6957532304a7fe Mon Sep 17 00:00:00 2001
From: Dean Brettle <dean@brettle.com>
Date: Wed, 9 Jun 2021 19:50:12 -0700
Subject: [PATCH 02/11] fix SetLimelightSearching/add LimelightServo Constants

---
 src/main/java/org/team199/robot2021/Constants.java |  3 ++-
 .../java/org/team199/robot2021/RobotContainer.java |  2 +-
 .../org/team199/robot2021/subsystems/Intake.java   | 14 +++++++-------
 3 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/src/main/java/org/team199/robot2021/Constants.java b/src/main/java/org/team199/robot2021/Constants.java
index 2705139..911ea57 100644
--- a/src/main/java/org/team199/robot2021/Constants.java
+++ b/src/main/java/org/team199/robot2021/Constants.java
@@ -153,6 +153,7 @@ public static final class DrivePorts {
 
         public static final int kAutoPathSwitch1Port = 0;
         public static final int kAutoPathSwitch2Port = 1;
+        public static final int kLimelightServoPort = 0; //TODO: set proper channel
     }
 
     /**
@@ -236,7 +237,7 @@ public static final class Controller {
             public static final int kIntakeButton = X;
             public static final int kOuttakeButton = Y;
             // limelight toggle
-            public static final int kLimelightToggleButton = 0; //TODO: set button
+            public static final int kLimelightToggleButton = LB; //Limelight Toggle button set to Left Button
             //public static final int kRegurgitateButton = B;
         }
     }
diff --git a/src/main/java/org/team199/robot2021/RobotContainer.java b/src/main/java/org/team199/robot2021/RobotContainer.java
index 028bc2f..ce93c7f 100644
--- a/src/main/java/org/team199/robot2021/RobotContainer.java
+++ b/src/main/java/org/team199/robot2021/RobotContainer.java
@@ -193,7 +193,7 @@ private void configureButtonBindingsController() {
         new JoystickButton(controller, Constants.OI.Controller.B).whenPressed(new InstantCommand(() -> { SmartDashboard.putBoolean("Field Oriented", !SmartDashboard.getBoolean("Field Oriented", true)); }));
         // Intake toggle button
         new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake));
-        new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> {intake.changeServoStatus(!intake.isLimelightSearching());}));
+        new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> {intake.setLimelightSearching(!intake.isLimelightSearching());}));
         // Power cell regurgitate button
         //new JoystickButton(controller, Constants.OI.Controller.kRegurgitateButton).whileHeld(new Regurgitate(intake, feeder));
 
diff --git a/src/main/java/org/team199/robot2021/subsystems/Intake.java b/src/main/java/org/team199/robot2021/subsystems/Intake.java
index 7be50fa..8774c4d 100644
--- a/src/main/java/org/team199/robot2021/subsystems/Intake.java
+++ b/src/main/java/org/team199/robot2021/subsystems/Intake.java
@@ -16,11 +16,11 @@ public class Intake extends SubsystemBase {
     // TODO: find good values and then set to final
     private static double kIntakeSpeed = .6;
     private static double kSlowSpeed = -0.1;
-    private static double kLimeBottomAngle = 0; //TODO: set correct angle
-    private static double kLimeTopAngle = 0; //TODO: set correct angle
+    private static double kLimeBottomAngleDegs = 0; //TODO: set correct angle
+    private static double kLimeTopAngleDegs = 0; //TODO: set correct angle
 
     private final CANSparkMax rollerMotor = MotorControllerFactory.createSparkMax(Constants.DrivePorts.kIntakeRoller);
-    private final Servo limelightServo = new Servo(0); //TODO: set proper channel
+    private final Servo limelightServo = new Servo(Constants.DrivePorts.kLimelightServoPort); 
     private final DoubleSolenoid intakePistons1 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[2], Constants.DrivePorts.kIntakePistons[3]);
     private final DoubleSolenoid intakePistons2 = new DoubleSolenoid(Constants.DrivePorts.kIntakePistons[0], Constants.DrivePorts.kIntakePistons[1]);
 
@@ -84,12 +84,12 @@ public boolean isDeployed() {
     public boolean isLimelightSearching() {
         return isLimelightSearching;
     }
-    public void changeServoStatus(boolean status) {
+    public void setLimelightSearching(boolean status) {
         isLimelightSearching = status;
-        if (isLimelightSearching() == false) {
-            limelightServo.setAngle(kLimeBottomAngle); // TODO: set correct angle
+        if (isLimelightSearching() == true) {
+            limelightServo.setAngle(kLimeBottomAngleDegs); // TODO: set correct angle
         } else {
-            limelightServo.setAngle(kLimeTopAngle); // TODO: set correct angle
+            limelightServo.setAngle(kLimeTopAngleDegs); // TODO: set correct angle
         }
     }
 

From 9ba78a261323519114125a961034e320b56b0d5f Mon Sep 17 00:00:00 2001
From: Dean Brettle <dean@brettle.com>
Date: Wed, 9 Jun 2021 20:49:28 -0700
Subject: [PATCH 03/11] Change Limelight Toggle Button

---
 src/main/java/org/team199/robot2021/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/team199/robot2021/Constants.java b/src/main/java/org/team199/robot2021/Constants.java
index 911ea57..4767c3a 100644
--- a/src/main/java/org/team199/robot2021/Constants.java
+++ b/src/main/java/org/team199/robot2021/Constants.java
@@ -237,7 +237,7 @@ public static final class Controller {
             public static final int kIntakeButton = X;
             public static final int kOuttakeButton = Y;
             // limelight toggle
-            public static final int kLimelightToggleButton = LB; //Limelight Toggle button set to Left Button
+            public static final int kLimelightToggleButton = LT; //Limelight Toggle button set to Left Button
             //public static final int kRegurgitateButton = B;
         }
     }

From 17bb2bc6317f0ed636e16622c23987e8314ff5ae Mon Sep 17 00:00:00 2001
From: Dean Brettle <dean@brettle.com>
Date: Wed, 21 Jul 2021 20:48:52 -0700
Subject: [PATCH 04/11] add changing pipeline

---
 src/main/java/org/team199/robot2021/RobotContainer.java    | 5 ++++-
 src/main/java/org/team199/robot2021/subsystems/Intake.java | 4 ++++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/team199/robot2021/RobotContainer.java b/src/main/java/org/team199/robot2021/RobotContainer.java
index ce93c7f..3603725 100644
--- a/src/main/java/org/team199/robot2021/RobotContainer.java
+++ b/src/main/java/org/team199/robot2021/RobotContainer.java
@@ -193,7 +193,10 @@ private void configureButtonBindingsController() {
         new JoystickButton(controller, Constants.OI.Controller.B).whenPressed(new InstantCommand(() -> { SmartDashboard.putBoolean("Field Oriented", !SmartDashboard.getBoolean("Field Oriented", true)); }));
         // Intake toggle button
         new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake));
-        new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> {intake.setLimelightSearching(!intake.isLimelightSearching());}));
+        new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> {
+            intake.setLimelightSearching(!intake.isLimelightSearching()); 
+        
+        }));
         // Power cell regurgitate button
         //new JoystickButton(controller, Constants.OI.Controller.kRegurgitateButton).whileHeld(new Regurgitate(intake, feeder));
 
diff --git a/src/main/java/org/team199/robot2021/subsystems/Intake.java b/src/main/java/org/team199/robot2021/subsystems/Intake.java
index 8774c4d..7b72ef4 100644
--- a/src/main/java/org/team199/robot2021/subsystems/Intake.java
+++ b/src/main/java/org/team199/robot2021/subsystems/Intake.java
@@ -88,9 +88,13 @@ public void setLimelightSearching(boolean status) {
         isLimelightSearching = status;
         if (isLimelightSearching() == true) {
             limelightServo.setAngle(kLimeBottomAngleDegs); // TODO: set correct angle
+            NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(1);
         } else {
             limelightServo.setAngle(kLimeTopAngleDegs); // TODO: set correct angle
+            NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0);
+
         }
+
     }
 
 

From f107a812c9a5dbfe1933b2e8ce9e77ddf33b7172 Mon Sep 17 00:00:00 2001
From: Sean-Weber <803074@seq.org>
Date: Thu, 22 Jul 2021 20:17:15 -0700
Subject: [PATCH 05/11] import NetworkTableInstance

---
 src/main/java/org/team199/robot2021/subsystems/Intake.java | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/main/java/org/team199/robot2021/subsystems/Intake.java b/src/main/java/org/team199/robot2021/subsystems/Intake.java
index 7b72ef4..70b379c 100644
--- a/src/main/java/org/team199/robot2021/subsystems/Intake.java
+++ b/src/main/java/org/team199/robot2021/subsystems/Intake.java
@@ -12,6 +12,9 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.lib.MotorControllerFactory;
 import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+
 public class Intake extends SubsystemBase {
     // TODO: find good values and then set to final
     private static double kIntakeSpeed = .6;

From 928931807704b1fead193585e9eca216cbe17f77 Mon Sep 17 00:00:00 2001
From: Bamboozlejpg <kevin@bachelorclan.com>
Date: Wed, 8 Sep 2021 19:08:02 -0700
Subject: [PATCH 06/11] Begin creating system for auto-ball pickup

---
 lib199                                        |  2 +-
 .../java/org/team199/robot2021/Constants.java |  5 ++
 .../commands/BallSearchAndPickup.java         | 71 +++++++++++++++++++
 .../robot2021/commands/RotateAngle.java       | 45 ++++++++++++
 4 files changed, 122 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
 create mode 100644 src/main/java/org/team199/robot2021/commands/RotateAngle.java

diff --git a/lib199 b/lib199
index a2a01f6..a7dea30 160000
--- a/lib199
+++ b/lib199
@@ -1 +1 @@
-Subproject commit a2a01f6e6c1338f86e01ee6e86b8a13da554ceaa
+Subproject commit a7dea3012bb9c120a16ef09e0eeb03f7a8b278e8
diff --git a/src/main/java/org/team199/robot2021/Constants.java b/src/main/java/org/team199/robot2021/Constants.java
index 4767c3a..2ef8b3c 100644
--- a/src/main/java/org/team199/robot2021/Constants.java
+++ b/src/main/java/org/team199/robot2021/Constants.java
@@ -106,8 +106,13 @@ public static final class DriveConstants {
         // TODO: Find experimental value for current draw
         public static final double intakeCurrentDraw = 8.0;
 
+        // TODO: change these to current for 2021 robot
         public static final double cameraHeight = Units.inchesToMeters(40.0);
         public static final double cameraMountingAngleDeg = 29.5;
+
+        // TODO: Find distances between intake and limelight
+        public static final double xLimeDistIntake = 0.0;
+        public static final double zLimeDistIntake = 0.0;
     }
 
     public static final class DrivePorts {
diff --git a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
new file mode 100644
index 0000000..52e7561
--- /dev/null
+++ b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.team199.robot2021.commands;
+
+import frc.robot.lib.Limelight;
+import org.team199.robot2021.subsystems.Drivetrain;
+import org.team199.robot2021.subsystems.Feeder;
+import org.team199.robot2021.subsystems.Intake;
+import org.team199.robot2021.subsystems.Shooter;
+import org.team199.robot2021.Constants;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class BallSearchAndPickup extends CommandBase {
+  private Drivetrain drivetrain;
+  private Intake intake;
+  private Feeder feeder;
+  private Shooter shooter;
+  private Limelight lime;
+
+  /** Creates a new BallSearchAndPickup. */
+  public BallSearchAndPickup(Drivetrain drivetrain, Intake intake, Feeder feeder, Shooter shooter, 
+  Limelight lime) {
+    this.lime = lime;
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain, intake, feeder, shooter);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    //Search for Balls
+    double[] distComponents = lime.determineObjectDist(Constants.DriveConstants.cameraHeight, 0.0, Constants.DriveConstants.cameraMountingAngleDeg);
+    double xOffset = Constants.DriveConstants.xLimeDistIntake;
+    double zOffset = Constants.DriveConstants.zLimeDistIntake;
+    //(Make sure to subtract the horizontal dist. b/t limelight and center of intake from the left right dist)
+    distComponents[0] = distComponents[0] - xOffset;
+    distComponents[1] = distComponents[1] - zOffset;
+    //Find angle and distance from balls using trigonometry and getObjectDistance method in Limelight class (0 is forward, 1 is strafe)
+    double angle = Math.atan2(distComponents[0], distComponents[1]);
+    double distance = Math.hypot(distComponents[0], distComponents[1]);
+    //Rotate Robot along angle till it's facing ball
+      //Create new command to do this where isfinished checks if the goal angle is the same as the current angle
+    //Run the intake
+    //Drive robot forward the distance plus a little bit
+    //Congrats you're done
+  }
+
+  // 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/src/main/java/org/team199/robot2021/commands/RotateAngle.java b/src/main/java/org/team199/robot2021/commands/RotateAngle.java
new file mode 100644
index 0000000..281793b
--- /dev/null
+++ b/src/main/java/org/team199/robot2021/commands/RotateAngle.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.team199.robot2021.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import org.team199.robot2021.subsystems.Drivetrain;
+
+public class RotateAngle extends CommandBase {
+  private Drivetrain dt;
+  private double goalAngle;
+  private double currentAngle;
+  /** Creates a new RotateAngle. */
+  public RotateAngle(Drivetrain dt, double goalAngle) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.dt = dt;
+    this.goalAngle = goalAngle;
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    //TODO: determine speed angle rotates
+    dt.drive(0, 0, 0.1);
+    //TODO: use getHeading to find the angle we want the drivetrain to rotate. 
+    //TODO: Change goalAngle to turnAngle, with goalAngle being the angle we want the drivetrian to rotate
+    }
+
+  // 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() {
+    // TODO: Determine margin of error
+    return (Math.abs(goalAngle - currentAngle) < 5);
+  }
+}

From 0cebc4846d193f1a427fb7c6c12fc72bde34ff31 Mon Sep 17 00:00:00 2001
From: Sean Weber <803074@seq.org>
Date: Fri, 10 Sep 2021 15:14:04 -0700
Subject: [PATCH 07/11] add ball sense and pick up functionality

---
 lib199                                        |  2 +-
 .../commands/BallSearchAndPickup.java         | 39 +++++----------
 .../robot2021/commands/DriveDistance.java     | 49 +++++++++++++++++++
 .../robot2021/commands/RotateAngle.java       | 29 ++++++++---
 4 files changed, 83 insertions(+), 36 deletions(-)
 create mode 100644 src/main/java/org/team199/robot2021/commands/DriveDistance.java

diff --git a/lib199 b/lib199
index a7dea30..a2a01f6 160000
--- a/lib199
+++ b/lib199
@@ -1 +1 @@
-Subproject commit a7dea3012bb9c120a16ef09e0eeb03f7a8b278e8
+Subproject commit a2a01f6e6c1338f86e01ee6e86b8a13da554ceaa
diff --git a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
index 52e7561..6dd6ad2 100644
--- a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
+++ b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
@@ -10,12 +10,15 @@
 import org.team199.robot2021.subsystems.Intake;
 import org.team199.robot2021.subsystems.Shooter;
 import org.team199.robot2021.Constants;
+import org.team199.robot2021.commands.RotateAngle;
+import org.team199.robot2021.commands.DriveDistance;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-public class BallSearchAndPickup extends CommandBase {
+public class BallSearchAndPickup extends SequentialCommandGroup {
   private Drivetrain drivetrain;
   private Intake intake;
   private Feeder feeder;
@@ -28,12 +31,8 @@ public BallSearchAndPickup(Drivetrain drivetrain, Intake intake, Feeder feeder,
     this.lime = lime;
     this.drivetrain = drivetrain;
     addRequirements(drivetrain, intake, feeder, shooter);
-    // Use addRequirements() here to declare subsystem dependencies.
-  }
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
+    // Use addRequirements() here to declare subsystem dependencies.
     //Search for Balls
     double[] distComponents = lime.determineObjectDist(Constants.DriveConstants.cameraHeight, 0.0, Constants.DriveConstants.cameraMountingAngleDeg);
     double xOffset = Constants.DriveConstants.xLimeDistIntake;
@@ -44,28 +43,14 @@ public void initialize() {
     //Find angle and distance from balls using trigonometry and getObjectDistance method in Limelight class (0 is forward, 1 is strafe)
     double angle = Math.atan2(distComponents[0], distComponents[1]);
     double distance = Math.hypot(distComponents[0], distComponents[1]);
+    RotateAngle rotate = new RotateAngle(drivetrain, angle);
+    DriveDistance drive = new DriveDistance(drivetrain, distance);
     //Rotate Robot along angle till it's facing ball
       //Create new command to do this where isfinished checks if the goal angle is the same as the current angle
-    //Run the intake
-    //Drive robot forward the distance plus a little bit
-    //Congrats you're done
-  }
-
-  // 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) {
-
+    //Run the intak
+    addCommands(rotate, new InstantCommand(() -> intake.deploy()), new InstantCommand(() -> intake.intake()), drive);
   }
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+  // Called when the command is initially scheduled.
+  
 }
diff --git a/src/main/java/org/team199/robot2021/commands/DriveDistance.java b/src/main/java/org/team199/robot2021/commands/DriveDistance.java
new file mode 100644
index 0000000..24ab004
--- /dev/null
+++ b/src/main/java/org/team199/robot2021/commands/DriveDistance.java
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.team199.robot2021.commands;
+
+import org.team199.robot2021.subsystems.Drivetrain;
+
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveDistance extends CommandBase {
+  private Drivetrain drivetrain;
+  private double distance;
+  private Translation2d initDist;
+  private double hasDriven;
+  /** Creates a new DriveDistance. */
+  public DriveDistance(Drivetrain drivetrain, double distance) {
+    this.drivetrain = drivetrain;
+    this.distance = distance;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drivetrain.drive(1, 0, 0);
+    //Sets initial Distance
+    initDist = drivetrain.getOdometry().getPoseMeters().getTranslation();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    //Keep Going forward (Set speed of wheels or whatever that is)
+    drivetrain.drive(1,0,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() {
+    hasDriven = drivetrain.getOdometry().getPoseMeters().getTranslation().getDistance(initDist);
+    return (hasDriven >= distance);
+  }
+}
diff --git a/src/main/java/org/team199/robot2021/commands/RotateAngle.java b/src/main/java/org/team199/robot2021/commands/RotateAngle.java
index 281793b..cf72b28 100644
--- a/src/main/java/org/team199/robot2021/commands/RotateAngle.java
+++ b/src/main/java/org/team199/robot2021/commands/RotateAngle.java
@@ -8,14 +8,15 @@
 import org.team199.robot2021.subsystems.Drivetrain;
 
 public class RotateAngle extends CommandBase {
-  private Drivetrain dt;
-  private double goalAngle;
+  private Drivetrain drivetrain;
+  private double rotateAngle;
   private double currentAngle;
+  private double goalAngle;
   /** Creates a new RotateAngle. */
-  public RotateAngle(Drivetrain dt, double goalAngle) {
+  public RotateAngle(Drivetrain drivetrain, double rotateAngle) {
     // Use addRequirements() here to declare subsystem dependencies.
-    this.dt = dt;
-    this.goalAngle = goalAngle;
+    this.drivetrain = drivetrain;
+    this.rotateAngle = rotateAngle;
 
   }
 
@@ -23,18 +24,30 @@ public RotateAngle(Drivetrain dt, double goalAngle) {
   @Override
   public void initialize() {
     //TODO: determine speed angle rotates
-    dt.drive(0, 0, 0.1);
+    //Rotates based on if rotateAngle is positive or negative
+    //TODO: define goalAngle as the final angle based on odometry values
+    //TODO: ensure getHeading returns an angle
+    goalAngle = drivetrain.getHeading() *Math.PI/180 + rotateAngle;
+    if (Math.abs(rotateAngle) != rotateAngle) {
+      drivetrain.drive(0, 0, -0.1);
+    } else {
+      drivetrain.drive(0, 0, 0.1);
+    }
     //TODO: use getHeading to find the angle we want the drivetrain to rotate. 
     //TODO: Change goalAngle to turnAngle, with goalAngle being the angle we want the drivetrian to rotate
     }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    currentAngle = drivetrain.getHeading()*Math.PI/180;
+  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    drivetrain.drive(0, 0 , 0);
+  }
 
   // Returns true when the command should end.
   @Override

From 4b6eeb4ed6d87ac78a345048385e9e3dacb23c9b Mon Sep 17 00:00:00 2001
From: 0sani <jackahansen18@gmail.com>
Date: Fri, 10 Sep 2021 15:54:35 -0700
Subject: [PATCH 08/11] incorportated BallSearchAndPickup button

---
 .../java/org/team199/robot2021/RobotContainer.java     | 10 +++++++---
 .../robot2021/commands/BallSearchAndPickup.java        |  4 ++--
 .../java/org/team199/robot2021/subsystems/Intake.java  |  2 +-
 3 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/team199/robot2021/RobotContainer.java b/src/main/java/org/team199/robot2021/RobotContainer.java
index 3603725..4d51c26 100644
--- a/src/main/java/org/team199/robot2021/RobotContainer.java
+++ b/src/main/java/org/team199/robot2021/RobotContainer.java
@@ -28,11 +28,14 @@
 
 import org.team199.lib.RobotPath;
 import org.team199.robot2021.Constants.OI;
+import org.team199.robot2021.commands.BallSearchAndPickup;
 import org.team199.robot2021.commands.GalacticSearchCommand;
 import org.team199.robot2021.commands.TeleopDrive;
 import org.team199.robot2021.commands.ToggleIntake;
 import org.team199.robot2021.subsystems.Drivetrain;
+import org.team199.robot2021.subsystems.Feeder;
 import org.team199.robot2021.subsystems.Intake;
+import org.team199.robot2021.subsystems.Shooter;
 
 import frc.robot.lib.Limelight;
 
@@ -46,9 +49,9 @@
 public class RobotContainer {
     final Drivetrain drivetrain = new Drivetrain();
     private final Limelight lime = new Limelight();
-    //private final Shooter shooter = new Shooter(lime);
+    private final Shooter shooter = new Shooter(lime);
     private final Intake intake = new Intake();
-    //private final Feeder feeder = new Feeder();
+    private final Feeder feeder = new Feeder();
     private final Joystick leftJoy = new Joystick(Constants.OI.LeftJoy.port);
     private final Joystick rightJoy = new Joystick(Constants.OI.RightJoy.port);
     private final Joystick controller = new Joystick(Constants.OI.Controller.port);
@@ -192,7 +195,8 @@ private void configureButtonBindingsController() {
         new JoystickButton(controller, Constants.OI.Controller.A).whenPressed(new InstantCommand(() -> {drivetrain.resetHeading();}));
         new JoystickButton(controller, Constants.OI.Controller.B).whenPressed(new InstantCommand(() -> { SmartDashboard.putBoolean("Field Oriented", !SmartDashboard.getBoolean("Field Oriented", true)); }));
         // Intake toggle button
-        new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake));
+        // new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new ToggleIntake(intake));
+        new JoystickButton(controller, Constants.OI.Controller.kIntakeButton).whenPressed(new BallSearchAndPickup(drivetrain, intake, feeder, shooter, lime));
         new JoystickButton(controller, Constants.OI.Controller.kLimelightToggleButton).whenPressed(new InstantCommand(() -> {
             intake.setLimelightSearching(!intake.isLimelightSearching()); 
         
diff --git a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
index 6dd6ad2..bb8ef6f 100644
--- a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
+++ b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
@@ -47,8 +47,8 @@ public BallSearchAndPickup(Drivetrain drivetrain, Intake intake, Feeder feeder,
     DriveDistance drive = new DriveDistance(drivetrain, distance);
     //Rotate Robot along angle till it's facing ball
       //Create new command to do this where isfinished checks if the goal angle is the same as the current angle
-    //Run the intak
-    addCommands(rotate, new InstantCommand(() -> intake.deploy()), new InstantCommand(() -> intake.intake()), drive);
+    //Run the intake
+    addCommands(rotate, new InstantCommand(() -> intake.deploy()), new InstantCommand(() -> intake.intake()), drive, new InstantCommand(() -> intake.stop()), new InstantCommand(() -> intake.retract()));
   }
 
   // Called when the command is initially scheduled.
diff --git a/src/main/java/org/team199/robot2021/subsystems/Intake.java b/src/main/java/org/team199/robot2021/subsystems/Intake.java
index 70b379c..98ca611 100644
--- a/src/main/java/org/team199/robot2021/subsystems/Intake.java
+++ b/src/main/java/org/team199/robot2021/subsystems/Intake.java
@@ -89,7 +89,7 @@ public boolean isLimelightSearching() {
     }
     public void setLimelightSearching(boolean status) {
         isLimelightSearching = status;
-        if (isLimelightSearching() == true) {
+        if (isLimelightSearching) {
             limelightServo.setAngle(kLimeBottomAngleDegs); // TODO: set correct angle
             NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(1);
         } else {

From d078ab0799c984a8d208a1103e920c1ebd49656a Mon Sep 17 00:00:00 2001
From: Bamboozlejpg <kevin@bachelorclan.com>
Date: Fri, 10 Sep 2021 16:00:08 -0700
Subject: [PATCH 09/11] Add Check to find balls in ballsearch

---
 lib199                                        |  2 +-
 .../commands/BallSearchAndPickup.java         | 45 ++++++++++++-------
 2 files changed, 29 insertions(+), 18 deletions(-)

diff --git a/lib199 b/lib199
index a2a01f6..a7dea30 160000
--- a/lib199
+++ b/lib199
@@ -1 +1 @@
-Subproject commit a2a01f6e6c1338f86e01ee6e86b8a13da554ceaa
+Subproject commit a7dea3012bb9c120a16ef09e0eeb03f7a8b278e8
diff --git a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
index 6dd6ad2..d2fc34a 100644
--- a/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
+++ b/src/main/java/org/team199/robot2021/commands/BallSearchAndPickup.java
@@ -15,6 +15,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -31,24 +32,34 @@ public BallSearchAndPickup(Drivetrain drivetrain, Intake intake, Feeder feeder,
     this.lime = lime;
     this.drivetrain = drivetrain;
     addRequirements(drivetrain, intake, feeder, shooter);
+    RotateAngle check = new RotateAngle(drivetrain, Math.PI/5);
 
-    // Use addRequirements() here to declare subsystem dependencies.
-    //Search for Balls
-    double[] distComponents = lime.determineObjectDist(Constants.DriveConstants.cameraHeight, 0.0, Constants.DriveConstants.cameraMountingAngleDeg);
-    double xOffset = Constants.DriveConstants.xLimeDistIntake;
-    double zOffset = Constants.DriveConstants.zLimeDistIntake;
-    //(Make sure to subtract the horizontal dist. b/t limelight and center of intake from the left right dist)
-    distComponents[0] = distComponents[0] - xOffset;
-    distComponents[1] = distComponents[1] - zOffset;
-    //Find angle and distance from balls using trigonometry and getObjectDistance method in Limelight class (0 is forward, 1 is strafe)
-    double angle = Math.atan2(distComponents[0], distComponents[1]);
-    double distance = Math.hypot(distComponents[0], distComponents[1]);
-    RotateAngle rotate = new RotateAngle(drivetrain, angle);
-    DriveDistance drive = new DriveDistance(drivetrain, distance);
-    //Rotate Robot along angle till it's facing ball
-      //Create new command to do this where isfinished checks if the goal angle is the same as the current angle
-    //Run the intak
-    addCommands(rotate, new InstantCommand(() -> intake.deploy()), new InstantCommand(() -> intake.intake()), drive);
+    for (int i = 0; i < 10; i++) {
+      addCommands(check);
+      if (SmartDashboard.getNumber("Found Vision Target", 0) != 0) {
+        break;
+      }
+    }
+
+    if (SmartDashboard.getNumber("Found Vision Target", 0) != 0) {
+      // Use addRequirements() here to declare subsystem dependencies.
+      //Search for Balls
+      double[] distComponents = lime.determineObjectDist(Constants.DriveConstants.cameraHeight, 0.0, Constants.DriveConstants.cameraMountingAngleDeg);
+      double xOffset = Constants.DriveConstants.xLimeDistIntake;
+      double zOffset = Constants.DriveConstants.zLimeDistIntake;
+      //(Make sure to subtract the horizontal dist. b/t limelight and center of intake from the left right dist)
+      distComponents[0] = distComponents[0] - xOffset;
+      distComponents[1] = distComponents[1] - zOffset;
+      //Find angle and distance from balls using trigonometry and getObjectDistance method in Limelight class (0 is forward, 1 is strafe)
+      double angle = Math.atan2(distComponents[0], distComponents[1]);
+      double distance = Math.hypot(distComponents[0], distComponents[1]);
+      RotateAngle rotate = new RotateAngle(drivetrain, angle);
+      DriveDistance drive = new DriveDistance(drivetrain, distance);
+      //Rotate Robot along angle till it's facing ball
+        //Create new command to do this where isfinished checks if the goal angle is the same as the current angle
+      //Run the intak
+      addCommands(rotate, new InstantCommand(() -> intake.deploy()), new InstantCommand(() -> intake.intake()), drive);
+    }
   }
 
   // Called when the command is initially scheduled.

From 99379910f4ed9b764355787ab74868911b65d4b8 Mon Sep 17 00:00:00 2001
From: Bamboozlejpg <kevin@bachelorclan.com>
Date: Fri, 10 Sep 2021 16:12:29 -0700
Subject: [PATCH 10/11] Add search functionality to autonomous

---
 .../team199/robot2021/commands/AutoShootAndDrive.java  | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java b/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java
index 3223427..a2f51ce 100644
--- a/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java
+++ b/src/main/java/org/team199/robot2021/commands/AutoShootAndDrive.java
@@ -23,8 +23,9 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh
         TimeOfFlight shooterDistanceSensor = feeder.getShooterDistanceSensor();
         ShooterHorizontalAim aim = new ShooterHorizontalAim(drivetrain, lime);
         AutoShoot shoot = new AutoShoot(feeder, shooter, shooterDistanceSensor, 3);
+        BallSearchAndPickup search = new BallSearchAndPickup(drivetrain, intake, feeder, shooter, lime);
 
-        /*
+        
         addCommands(
             aim,
             new InstantCommand(() -> { 
@@ -36,7 +37,7 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh
                 intake.intake();
                 intake.doTheFlop();
             }, intake),
-            path.getPathCommand(),
+            path.getPathCommand(true, true),
             new InstantCommand(() -> {
                 intake.retract();
                 intake.stop();
@@ -45,8 +46,9 @@ public AutoShootAndDrive(Drivetrain drivetrain, Intake intake, Feeder feeder, Sh
             new InstantCommand(() -> { 
                 SmartDashboard.putNumber("Shooter.kTargetSpeed", linearInterpol.calculate(drivetrain.getOdometry().getPoseMeters().getTranslation().getDistance(target))); 
             }),
-            shoot
+            shoot, 
+            search
         );
-        */
+        
     }
 }
\ No newline at end of file

From 441f3cd102849d8cbbab48b2cbb650b9e8270725 Mon Sep 17 00:00:00 2001
From: 0sani <jackahansen18@gmail.com>
Date: Wed, 15 Sep 2021 16:32:13 -0700
Subject: [PATCH 11/11] Added paths (not to main this time)

---
 src/main/deploy/PathWeaver/Paths/CBall1.path | 3 +++
 src/main/deploy/PathWeaver/Paths/LBall1.path | 3 +++
 src/main/deploy/PathWeaver/Paths/RBall1.path | 3 +++
 3 files changed, 9 insertions(+)
 create mode 100644 src/main/deploy/PathWeaver/Paths/CBall1.path
 create mode 100644 src/main/deploy/PathWeaver/Paths/LBall1.path
 create mode 100644 src/main/deploy/PathWeaver/Paths/RBall1.path

diff --git a/src/main/deploy/PathWeaver/Paths/CBall1.path b/src/main/deploy/PathWeaver/Paths/CBall1.path
new file mode 100644
index 0000000..055cc34
--- /dev/null
+++ b/src/main/deploy/PathWeaver/Paths/CBall1.path
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+3.39,-2.4,1.0,0.0,true,false,
+5.0,-0.7,1.0,0.0,true,false,
diff --git a/src/main/deploy/PathWeaver/Paths/LBall1.path b/src/main/deploy/PathWeaver/Paths/LBall1.path
new file mode 100644
index 0000000..913f451
--- /dev/null
+++ b/src/main/deploy/PathWeaver/Paths/LBall1.path
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+3.39,-3.4,1.0,0.0,true,false,
+5.0,-0.7,1.0,0.0,true,false,
diff --git a/src/main/deploy/PathWeaver/Paths/RBall1.path b/src/main/deploy/PathWeaver/Paths/RBall1.path
new file mode 100644
index 0000000..7b11d7e
--- /dev/null
+++ b/src/main/deploy/PathWeaver/Paths/RBall1.path
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+3.39,-1.4,1.0,0.0,true,false,
+5.0,-0.7,1.0,0.0,true,false,