aim and approach by purple vision proof of concept

src/main/java/frc/robot/Robot.java

@@ -40,8 +40,10 @@ public class Robot extends TimedRobot {
   private final ArmSubsystem arm = new ArmSubsystem(hardware.armLeft, hardware.armRight);
   private final IntakeSubsystem intake =
       new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor);
-  private final SwerveSubsystem swerve = new SwerveSubsystem();
+  private final SwerveSubsystem swerve = new SwerveSubsystem(queuer);
+
   private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon);
+
   private final Limelight leftLimelight =
       new Limelight("left", RobotConfig.get().vision().interpolatedVisionSet().leftSet);
   private final Limelight rightLimelight =

src/main/java/frc/robot/purple/Purple.java

@@ -0,0 +1,34 @@
+package frc.robot.purple;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.vision.LimelightHelpers;
+
+public class Purple {
+  private static final double X_ADJUST_P = 0.1;
+
+  public static ChassisSpeeds getPurpleAdjustmentRobotRelative() {
+    var tx = LimelightHelpers.getTX("asdasd");
+
+
+    if (tx == 0) {
+      return new ChassisSpeeds();
+    }
+
+    var error = getXError(tx);
+
+    var adjustment = getRobotAdjustmentRobotRelative(error);
+
+    return adjustment;
+  }
+
+  private static double getXError(double tx) {
+    return -tx;
+  }
+
+  private static ChassisSpeeds getRobotAdjustmentRobotRelative(double xError) {
+    // adjust side to side, don't care about forward/backward, don't care about rotation
+    double xTranslation = X_ADJUST_P * xError;
+    ChassisSpeeds translation = new ChassisSpeeds(xTranslation, 0, 0);
+    return translation;
+  }
+}

src/main/java/frc/robot/swerve/SnapUtil.java

@@ -20,6 +20,62 @@ public static double getSubwooferAngle() {
     return FmsSubsystem.isRedAlliance() ? 0 : (180.0);
   }
 
+  public static double getReef(int reefNum) {
+    switch (reefNum) {
+      case 1 -> {
+        return getReefAngle1();
+      }
+      case 2 -> {
+        return getReefAngle2();
+      }
+      case 3 -> {
+        return getReefAngle3();
+      }
+      case 4 -> {
+        return getReefAngle4();
+      }
+      case 5 -> {
+        return getReefAngle5();
+      }
+      case 6 -> {
+        return getReefAngle6();
+      }
+      default -> {
+        return 0.0;
+      }
+    }
+  }
+
+  public static double getReefAngle1() {
+    // return whatever the amp angle is
+    return FmsSubsystem.isRedAlliance() ? 0.0 : (180.0);
+  }
+
+  public static double getReefAngle2() {
+    // return whatever the amp angle is
+    return FmsSubsystem.isRedAlliance() ? 60.0 : (240.0);
+  }
+
+  public static double getReefAngle3() {
+    // return whatever the amp angle is
+    return FmsSubsystem.isRedAlliance() ? 120.0 : (300.0);
+  }
+
+  public static double getReefAngle4() {
+    // return whatever the amp angle is
+    return FmsSubsystem.isRedAlliance() ? 180.0 : (0.0);
+  }
+
+  public static double getReefAngle5() {
+    // return whatever the amp angle is
+    return FmsSubsystem.isRedAlliance() ? 240.0 : (60.0);
+  }
+
+  public static double getReefAngle6() {
+    // return whatever the amp angle is
+    return FmsSubsystem.isRedAlliance() ? 300.0 : (120.0);
+  }
+
   private static final List<Double> RED_STAGE_ANGLES = List.of(0.0, 120.0, -120.0);
   private static final List<Double> BLUE_STAGE_ANGLES =
       List.of(0.0 + 180.0, 120.0 - 180.0, -120 + 180.0);

src/main/java/frc/robot/swerve/SwerveState.java

@@ -6,5 +6,11 @@ public enum SwerveState {
   AUTO,
   AUTO_SNAPS,
   INTAKE_ASSIST_TELEOP,
-  INTAKE_ASSIST_AUTO;
+  INTAKE_ASSIST_AUTO,
+  PURPLE_ASSIST_TELEOP,
+  PURPLE_ASSIST_AUTO,
+  PURPLE_SNAP_TELEOP,
+  PURPLE_SNAP_AUTO,
+  PURPLE_APPROACH_TELEOP,
+  PURPLE_APPROACH_AUTO;
 }

src/main/java/frc/robot/swerve/SwerveSubsystem.java

@@ -12,16 +12,20 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import com.ctre.phoenix6.signals.InvertedValue;
 import dev.doglog.DogLog;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
+import frc.robot.FieldUtil;
 import frc.robot.config.RobotConfig;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.generated.TunerConstants;
 import frc.robot.intake_assist.IntakeAssistManager;
+import frc.robot.purple.Purple;
+import frc.robot.queuer.QueuerSubsystem;
 import frc.robot.util.ControllerHelpers;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
@@ -79,9 +83,12 @@ private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber)
   private final SwerveModule backLeft = drivetrain.getModule(2);
   private final SwerveModule backRight = drivetrain.getModule(3);
 
+  private final ChassisSpeeds approachSpeed = new ChassisSpeeds(0, 1, 0);
+
   private double lastSimTime;
   private Notifier simNotifier = null;
 
+  public final QueuerSubsystem queuer;
   private boolean slowEnoughToShoot = false;
   private SwerveDriveState drivetrainState = new SwerveDriveState();
   private ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds();
@@ -127,8 +134,10 @@ public void setSnapToAngle(double angle) {
     }
   }
 
-  public SwerveSubsystem() {
+  public SwerveSubsystem(QueuerSubsystem queuer) {
     super(SubsystemPriority.SWERVE, SwerveState.TELEOP);
+    this.queuer = queuer;
+
     driveToAngle.HeadingController = RobotConfig.get().swerve().snapController();
     driveToAngle.HeadingController.enableContinuousInput(-Math.PI, Math.PI);
     driveToAngle.HeadingController.setTolerance(0.02);
@@ -186,6 +195,26 @@ public void setRobotRelativeAutoSpeeds(ChassisSpeeds speeds) {
             speeds, Rotation2d.fromDegrees(drivetrainPigeon.getYaw().getValueAsDouble())));
   }
 
+  public boolean atReef(SwerveState currentState) {
+    // check if we are close to the coral (x & y) and rotated mostly correct
+    return switch (currentState) {
+      case PURPLE_APPROACH_AUTO, PURPLE_APPROACH_TELEOP ->
+          MathUtil.isNear(FieldUtil.getSpeakerPose().getX(), getDrivetrainState().Pose.getX(), 3)
+              && MathUtil.isNear(
+                  FieldUtil.getSpeakerPose().getY(), getDrivetrainState().Pose.getY(), 3);
+
+      case PURPLE_ASSIST_AUTO, PURPLE_ASSIST_TELEOP -> queuer.hasNote();
+
+      case PURPLE_SNAP_AUTO, PURPLE_SNAP_TELEOP ->
+          MathUtil.isNear(
+              goalSnapAngle,
+              MathUtil.inputModulus(drivetrain.getPigeon2().getYaw().getValue(), -180, 180),
+              2);
+
+      default -> false;
+    };
+  }
+
   @Override
   protected SwerveState getNextState(SwerveState currentState) {
     // Ensure that we are in an auto state during auto, and a teleop state during teleop
@@ -195,8 +224,24 @@ protected SwerveState getNextState(SwerveState currentState) {
           DriverStation.isAutonomous()
               ? SwerveState.INTAKE_ASSIST_AUTO
               : SwerveState.INTAKE_ASSIST_TELEOP;
+      case PURPLE_ASSIST_AUTO, PURPLE_ASSIST_TELEOP ->
+          DriverStation.isAutonomous()
+              ? SwerveState.PURPLE_ASSIST_AUTO
+              : SwerveState.PURPLE_ASSIST_TELEOP;
       case AUTO_SNAPS, TELEOP_SNAPS ->
           DriverStation.isAutonomous() ? SwerveState.AUTO_SNAPS : SwerveState.TELEOP_SNAPS;
+      case PURPLE_SNAP_AUTO ->
+          atReef(currentState) ? SwerveState.PURPLE_APPROACH_AUTO : SwerveState.PURPLE_SNAP_AUTO;
+      case PURPLE_SNAP_TELEOP ->
+          atReef(currentState)
+              ? SwerveState.PURPLE_APPROACH_TELEOP
+              : SwerveState.PURPLE_SNAP_TELEOP;
+      case PURPLE_APPROACH_AUTO ->
+          atReef(currentState) ? SwerveState.PURPLE_ASSIST_AUTO : SwerveState.PURPLE_APPROACH_AUTO;
+      case PURPLE_APPROACH_TELEOP ->
+          atReef(currentState)
+              ? SwerveState.PURPLE_ASSIST_TELEOP
+              : SwerveState.PURPLE_APPROACH_TELEOP;
     };
   }
 
@@ -290,6 +335,25 @@ private void sendSwerveRequest() {
                   .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
         }
       }
+      case PURPLE_SNAP_TELEOP -> {
+        if (teleopSpeeds.omegaRadiansPerSecond == 0) {
+          drivetrain.setControl(
+              driveToAngle
+                  .withVelocityX(teleopSpeeds.vxMetersPerSecond)
+                  .withVelocityY(teleopSpeeds.vyMetersPerSecond)
+                  .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
+                  .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+
+        } else {
+          drivetrain.setControl(
+              drive
+                  .withVelocityX(teleopSpeeds.vxMetersPerSecond)
+                  .withVelocityY(teleopSpeeds.vyMetersPerSecond)
+                  .withRotationalRate(teleopSpeeds.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+        }
+      }
+
       case INTAKE_ASSIST_TELEOP -> {
         var intakeAssistTeleopSpeeds =
             IntakeAssistManager.getRobotRelativeAssistSpeeds(0, teleopSpeeds);
@@ -302,6 +366,16 @@ private void sendSwerveRequest() {
                 .withRotationalRate(intakeAssistTeleopSpeeds.omegaRadiansPerSecond)
                 .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
       }
+      case PURPLE_ASSIST_TELEOP -> {
+        var purpleAssistTeleopSpeeds = Purple.getPurpleAdjustmentRobotRelative();
+
+        drivetrain.setControl(
+            drive
+                .withVelocityX(purpleAssistTeleopSpeeds.vxMetersPerSecond)
+                .withVelocityY(purpleAssistTeleopSpeeds.vyMetersPerSecond)
+                .withRotationalRate(purpleAssistTeleopSpeeds.omegaRadiansPerSecond)
+                .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+      }
       case AUTO ->
           drivetrain.setControl(
               drive
@@ -316,6 +390,13 @@ private void sendSwerveRequest() {
                   .withVelocityY(autoSpeeds.vyMetersPerSecond)
                   .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
                   .withDriveRequestType(DriveRequestType.Velocity));
+      case PURPLE_SNAP_AUTO ->
+          drivetrain.setControl(
+              driveToAngle
+                  .withVelocityX(autoSpeeds.vxMetersPerSecond)
+                  .withVelocityY(autoSpeeds.vyMetersPerSecond)
+                  .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
+                  .withDriveRequestType(DriveRequestType.Velocity));
 
       case INTAKE_ASSIST_AUTO -> {
         var intakeAssistAutoSpeeds =
@@ -328,6 +409,31 @@ private void sendSwerveRequest() {
                 .withRotationalRate(intakeAssistAutoSpeeds.omegaRadiansPerSecond)
                 .withDriveRequestType(DriveRequestType.Velocity));
       }
+      case PURPLE_ASSIST_AUTO -> {
+        var purpleAssistTeleopSpeeds = Purple.getPurpleAdjustmentRobotRelative();
+
+        drivetrain.setControl(
+            drive
+                .withVelocityX(purpleAssistTeleopSpeeds.vxMetersPerSecond)
+                .withVelocityY(purpleAssistTeleopSpeeds.vyMetersPerSecond)
+                .withRotationalRate(purpleAssistTeleopSpeeds.omegaRadiansPerSecond)
+                .withDriveRequestType(DriveRequestType.Velocity));
+      }
+      case PURPLE_APPROACH_AUTO ->
+          drivetrain.setControl(
+              drive
+                  .withVelocityX(approachSpeed.vxMetersPerSecond)
+                  .withVelocityY(approachSpeed.vyMetersPerSecond)
+                  .withRotationalRate(approachSpeed.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.Velocity));
+      case PURPLE_APPROACH_TELEOP ->
+          drivetrain.setControl(
+              drive
+                  .withVelocityX(approachSpeed.vxMetersPerSecond)
+                  .withVelocityY(approachSpeed.vyMetersPerSecond)
+                  .withRotationalRate(approachSpeed.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.Velocity));
+      default -> throw new IllegalArgumentException("Unexpected value: " + getState());
     }
   }
 
@@ -337,9 +443,9 @@ public void setState(SwerveState newState) {
 
   public void setSnapsEnabled(boolean newValue) {
     switch (getState()) {
-      case TELEOP, TELEOP_SNAPS, INTAKE_ASSIST_TELEOP ->
+      case TELEOP, TELEOP_SNAPS, INTAKE_ASSIST_TELEOP, PURPLE_ASSIST_TELEOP ->
           setStateFromRequest(newValue ? SwerveState.TELEOP_SNAPS : SwerveState.TELEOP);
-      case AUTO, AUTO_SNAPS, INTAKE_ASSIST_AUTO ->
+      case AUTO, AUTO_SNAPS, INTAKE_ASSIST_AUTO, PURPLE_ASSIST_AUTO ->
           setStateFromRequest(newValue ? SwerveState.AUTO_SNAPS : SwerveState.AUTO);
     }
   }