barge auton vision bug, auton delays/offsets, l3 eject speed, earlier elevator raise in auto barge

Author: ds12a

src/main/deploy/choreo/bargeToI.traj

@@ -117,7 +117,7 @@ public void initialize() {
       robotStateSubsystem.clearCoral();
     }
 
-    visionSubsystem.setIsAuto(false);
+    // visionSubsystem.setIsAuto(false);
     robotStateSubsystem.setAutoAlgaeLevel(algaeLevel);
 
     isServoing = false;

src/main/java/frc/robot/commands/auton/DriveAlgaeAutonServoCommand.java

@@ -29,5 +29,5 @@ public class AutonConstants {
       new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180));
 
   public static final Pose2d kMiddleBargeStart =
-      new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180));
+      new Pose2d(7.1, 3.7209, Rotation2d.fromDegrees(180));
 }

src/main/java/frc/robot/constants/AutonConstants.java

@@ -7,8 +7,11 @@ public class BargeAlignConstants {
 
   public static final double kBlueEjectAlgaeX = 7.67; // 7.72
   public static final double kRedEjectAlgaeX = 9.86; // 9.81
-  public static final double kBlueRaiseElevatorX = 7.02; // 7.22
-  public static final double kRedRaiseElevatorX = 10.51; // 10.31
+  public static final double kBlueRaiseElevatorX = 6.82; // 7.02
+  public static final double kRedRaiseElevatorX = 10.71; // 10.51
+
+  public static final double kBlueUnsafeX = 7.02;
+  public static final double kRedUnsafeX = 10.51;
 
   public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0);
   public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0);

src/main/java/frc/robot/constants/BargeAlignConstants.java

@@ -8,7 +8,7 @@ public class RobotStateConstants {
   public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
   public static final double kAlgaeRetreatDistance = 0;
 
-  public static final double kBlueBargeSafeX = 7; // 7.6
+  public static final double kBlueBargeSafeX = 7.02; // 7.6
   public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
 
   public static final double kCoralEjectTimer = 0.25;

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -282,7 +282,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance)),
-            new ArrayList<>(Arrays.asList(-0.0175, 0.0, 0.0)),
+            new ArrayList<>(Arrays.asList(-0.0075, -0.0075, -0.0075)),
             'j',
             false,
             AutonConstants.kNonProcessorMid);
@@ -366,12 +366,12 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             tagAlignSubsystem,
             visionSubsystem,
             new ArrayList<String>(Arrays.asList("startBargeToG", "bargeToE", "bargeToI")),
-            new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
+            new ArrayList<Double>(Arrays.asList(-0.00475, 0.0, 0.0)),
             new ArrayList<Double>(Arrays.asList(0.0, 0.0, 0.0)),
             new ArrayList<String>(Arrays.asList("GToBarge", "EToBarge", "IToNearBarge")),
             new ArrayList<>(Arrays.asList(ScoringLevel.L2, ScoringLevel.L3, ScoringLevel.L3)),
             AutonConstants.kMiddleBargeStart,
-            0.5);
+            0.0);
       }
 
       case 0x12 -> {
@@ -550,7 +550,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance,
                     PathHandlerConstants.kLoadLightDistance)),
-            new ArrayList<>(Arrays.asList(0.0, 0.0, 0.0)),
+            new ArrayList<>(Arrays.asList(-0.0075, -0.0075, -0.0075)),
             'e',
             true,
             AutonConstants.kProcessorMid);

src/main/java/frc/robot/subsystems/auto/AutoSwitch.java

@@ -105,7 +105,8 @@ public void eject(ScoringLevel level) {
     // setSpeed(CoralConstants.kEjectingSpeed);
 
     switch (level) {
-      case L1, L2, L3 -> setPct(0.8);
+      case L1, L2 -> setPct(0.8);
+      case L3 -> setPct(0.75);
       case L4 -> setPct(1);
     }
     setState(CoralState.EJECTING);

src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java

@@ -83,8 +83,17 @@ private boolean isSafe() {
   private boolean shouldRaiseElevator() {
     double poseX = driveSubsystem.getPoseMeters().getX();
     return isOnBlueSide
-        ? poseX > BargeAlignConstants.kBlueRaiseElevatorX
-        : poseX < BargeAlignConstants.kRedRaiseElevatorX;
+        ? (poseX > BargeAlignConstants.kBlueRaiseElevatorX
+            && poseX <= BargeAlignConstants.kBlueUnsafeX)
+        : (poseX < BargeAlignConstants.kRedRaiseElevatorX
+            && poseX >= BargeAlignConstants.kRedUnsafeX);
+  }
+
+  private boolean shouldDriveBackwards() {
+    double poseX = driveSubsystem.getPoseMeters().getX();
+    return isOnBlueSide
+        ? poseX > BargeAlignConstants.kBlueUnsafeX
+        : poseX < BargeAlignConstants.kRedUnsafeX;
   }
 
   private boolean shouldEjectAlgae() {
@@ -131,6 +140,8 @@ public void periodic() {
         Logger.recordOutput("BargeAlign/Vomega", vOmega);
         if (shouldRaiseElevator()) {
           setState(BargeAlignStates.RAISE_ELEV);
+        } else if (shouldDriveBackwards()) {
+          setState(BargeAlignStates.REVERSE);
         }
       }
       case RAISE_ELEV -> {
@@ -146,14 +157,18 @@ public void periodic() {
         }
       }
       case REVERSE -> {
-        if (isOnBlueSide) {
-          driveSubsystem.move(
-              -vX, getYStickReading(), BargeAlignConstants.kBlueRaiseElevatorX, true);
-          if (shouldRaiseElevator()) {
-            setState(BargeAlignStates.RAISE_ELEV);
-          }
-        } else {
-          driveSubsystem.move(vX, getYStickReading(), BargeAlignConstants.kRedRaiseElevatorX, true);
+        double vOmega =
+            driveOmega.calculate(
+                driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians());
+
+        driveSubsystem.move(-vX, getYStickReading(), vOmega, true);
+
+        Logger.recordOutput("BargeAlign/Vx", vX);
+        Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError());
+        Logger.recordOutput("BargeAlign/Vomega", vOmega);
+
+        if (shouldRaiseElevator()) {
+          setState(BargeAlignStates.RAISE_ELEV);
         }
       }
       case FINISHED -> {}

src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java

@@ -136,7 +136,7 @@ public VisionSubsystem(DriveSubsystem driveSubsystem) {
     // Initialize our udpSubscribers
     udpSubscriber[0] = new UdpSubscriber(5802, cams[0]);
     udpSubscriber[1] = new UdpSubscriber(5803, cams[2]);
-    udpSubscriber[2] = new UdpSubscriber(5804, cams[1], cams[3], cams[4]);
+    udpSubscriber[2] = new UdpSubscriber(5804, cams[1], cams[3]);
   }
   // Setter Methods
   public void setVisionUpdating(boolean updating) {
@@ -443,7 +443,8 @@ public void periodic() {
             }
           }
 
-          if (acceptUpdates[i] || seeGoodTag) {
+          if (((acceptUpdates[i] || seeGoodTag) && (i == 0 || i == 2))
+              || ((i == 1 || i == 3) && !ignoreRearCams && !isAuto)) {
             timeSinceLastUpdate = getSeconds();
             validResults.add(new Pair<WallEyeResult, Integer>(cams[i].getResults(), i));
             lastResult[i] = (WallEyeTagResult) cams[i].getResults();