lime align and april align working mostly

src/main/java/frc4388/robot/subsystems/Limelight.java

@@ -42,16 +42,16 @@ public class Limelight extends SubsystemBase {
 
   public void setToLimePipeline() {
     cam.setPipelineIndex(1);
+    setLEDs(true);
   }
 
   public void setToAprilPipeline() {
     cam.setPipelineIndex(0);
+    setLEDs(false);
   }
 
   // ! might need to find midpoint instead of entire target
   public PhotonTrackedTarget getAprilPoint() {
-    setToAprilPipeline();
-
     if (!cam.isConnected()) return null;
 
     PhotonPipelineResult result = cam.getLatestResult();
@@ -61,7 +61,7 @@ public class Limelight extends SubsystemBase {
     return result.getBestTarget();
   }
 
-  public double getDistanceToApril() {
+  public double getDistanceToApril() {    
     PhotonTrackedTarget aprilPoint = getAprilPoint();
     if (aprilPoint == null) return -1;
 
@@ -73,8 +73,6 @@ public class Limelight extends SubsystemBase {
   }
 
   public PhotonTrackedTarget getLowestTape() {
-    setToLimePipeline();
-
     if (!cam.isConnected()) return null;
 
     PhotonPipelineResult result = cam.getLatestResult();
@@ -93,15 +91,7 @@ public class Limelight extends SubsystemBase {
     return lowest;
   }
 
-  public int getNumTapes() {
-    setToLimePipeline();
-
-    PhotonPipelineResult result = cam.getLatestResult();
-
-    return result.getTargets().size();
-  }
-
-  public double getDistanceToTape() {
+  public double getDistanceToTape() {    
     PhotonTrackedTarget tapePoint = getLowestTape();
     if (tapePoint == null) return -1;
 

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -73,10 +73,10 @@ public class SwerveDrive extends SubsystemBase {
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-      Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);