target alignment basically completed

src/main/java/frc4388/robot/RobotContainer.java

@@ -9,8 +9,10 @@ package frc4388.robot;
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.AutoBalance;
@@ -22,6 +24,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.JoystickRecorder;
 import frc4388.robot.commands.LimeAlign;
 import frc4388.robot.commands.PlaybackChooser;
+import frc4388.robot.commands.RotateToAngle;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController; 
 
@@ -47,7 +50,7 @@ public class RobotContainer {
 
     public final Claw m_robotClaw = new Claw(m_robotMap.servo);
 
-    public final Limelight m_limeLight = new Limelight();
+    public final Limelight m_robotLimeLight = new Limelight();
 
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -57,7 +60,22 @@ public class RobotContainer {
     public SendableChooser<Command> chooser = new SendableChooser<>();
     
     private Command noAuto = new InstantCommand();
+
+    // private SequentialCommandGroup alignToTarget = new SequentialCommandGroup(
+    //     new RotateToAngle(m_robotSwerveDrive, m_robotLimeLight, 0.0, true),
+    //     new LimeAlign(m_robotSwerveDrive, m_robotLimeLight)
+    // );
+
+    private ConditionalCommand alignToTarget = new ConditionalCommand(
+        new SequentialCommandGroup(
+            new RotateToAngle(m_robotSwerveDrive, 0.0),
+            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight)
+        ),
+        noAuto,
+        () -> m_robotLimeLight.numTargets() <= 2
+    );
     
+
     // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
     
     // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
@@ -146,31 +164,21 @@ public class RobotContainer {
         //     .onFalse(new InstantCommand());
 
         // * Operator Buttons
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-        //     // .onTrue(new InstantCommand(() -> System.out.println("Claw Button")));
-        //     .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
-
-        
 
         new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
-
-        
+            .onTrue(new InstantCommand(() -> m_robotLimeLight.toggleLEDs(), m_robotLimeLight));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-            .whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight));
+            .onTrue(alignToTarget);
         
-            new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
-        
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
-
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw));
+            
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw));
     }
 
     /**

src/main/java/frc4388/robot/commands/LimeAlign.java

@@ -17,7 +17,7 @@ public class LimeAlign extends PelvicInflammatoryDisease {
   Limelight lime;
 
   public LimeAlign(SwerveDrive drive, Limelight lime) {
-    super(0.7, 0.1, 0.0, 0.0, 0);
+    super(0.7, 0.4, 0.0, 0.0, 0.04);
 
     this.drive = drive;
     this.lime = lime;
@@ -27,10 +27,15 @@ public class LimeAlign extends PelvicInflammatoryDisease {
 
   @Override
   public double getError() {
+
+    if (lime.numTargets() > 2) {
+      return 0.0;
+    }
+
     double err = 0.0;
 
     try {
-      err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); 
+      err = lime.getLowestTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); 
     } catch (NullPointerException ex) {}
     
     return err;

src/main/java/frc4388/robot/commands/RotateToAngle.java

@@ -0,0 +1,34 @@
+// 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 frc4388.robot.commands;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class RotateToAngle extends PelvicInflammatoryDisease {
+
+  SwerveDrive drive;
+  double targetAngle;
+
+  /** Creates a new RotateToAngle. */
+  public RotateToAngle(SwerveDrive drive, double targetAngle) {
+    super(0.3, 0.0, 0.0, 0.0, 1);
+    
+    this.drive = drive;
+    this.targetAngle = targetAngle;
+
+    addRequirements(drive);
+  }
+
+  @Override
+  public double getError() {
+    return targetAngle - drive.getGyroAngle();
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);    
+  }
+}

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() {