target alignment basically completed

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

@@ -156,15 +156,7 @@ public class Arm extends SubsystemBase {
         // double x = Math.cos(Math.toRadians(degrees));
     }
 
-    boolean soft_limits = true;
     public void killSoftLimits() {
         resetTeleSoftLimit();
-        var pivot_soft = m_pivot.getSelectedSensorPosition();
-        var tele_soft  = m_tele.getSelectedSensorPosition();
-        
-        m_pivot.configForwardSoftLimitEnable(!soft_limits);
-        m_pivot.configReverseSoftLimitEnable(!soft_limits);
-
-        soft_limits = !soft_limits;
     }
-}
+}

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

@@ -95,8 +95,29 @@ public class Limelight extends SubsystemBase {
     return result.getBestTarget();
   }
 
-  private double getPointAngle(Point point) {
-    return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS);
+  public PhotonTrackedTarget getLowestTargetPoint() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
+
+    PhotonTrackedTarget lowest = points.get(0);
+    for (PhotonTrackedTarget point : points) {
+      if (point.getPitch() < lowest.getPitch()) {
+        lowest = point;
+      }
+    }
+
+    return lowest;
+  }
+
+  public int numTargets() {
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    return result.getTargets().size();
   }
 
   public double getHorizontalDistanceToTarget(boolean high) {

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

@@ -101,6 +101,19 @@ public class SwerveDrive extends SubsystemBase {
     }
   }
 
+  public boolean rotateToTarget(double angle) {
+    double currentAngle = getGyroAngle();
+    double error = angle - currentAngle;
+
+    driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
+
+    if (Math.abs(angle - getGyroAngle()) < 5.0) {
+      return true;
+    }
+
+    return false;
+  }
+
   public double getGyroAngle() {
     return gyro.getAngle();
   }
@@ -122,7 +135,8 @@ public class SwerveDrive extends SubsystemBase {
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    // This method will be called once per scheduler run\
+    SmartDashboard.putNumber("Gyro", getGyroAngle());
   }
 
   public void shiftDown() {