Seed start position in auto

src/main/deploy/auto/1PieceBarge.json

@@ -6,5 +6,12 @@
     "L4"
   ],
   "intakeLocation": "CoralStationRight",
-  "intakeAfterLastScore": false
+  "intakeAfterLastScore": false,
+  "startSeededPositionTranslation": {
+    "x": 11.7,
+    "y": 4
+  },
+  "startSeededPositionRotation": {
+    "degrees": 0
+  }
 }

src/main/deploy/auto/1PieceLeft.json

@@ -6,5 +6,12 @@
     "L4"
   ],
   "intakeLocation": "CoralStationLeft",
-  "intakeAfterLastScore": false
+  "intakeAfterLastScore": false,
+  "startSeededPositionTranslation": {
+    "x": 11.7,
+    "y": 1.5
+  },
+  "startSeededPositionRotation": {
+    "degrees": -40
+  }
 }

src/main/deploy/auto/2PieceLeft.json

@@ -7,5 +7,12 @@
     "L4",
     "L4"
   ],
-  "intakeLocation": "CoralStationLeft"
+  "intakeLocation": "CoralStationLeft",
+  "startSeededPositionTranslation": {
+    "x": 11.7,
+    "y": 1.5
+  },
+  "startSeededPositionRotation": {
+    "degrees": -40
+  }
 }

src/main/deploy/auto/2PieceRight.json

@@ -7,5 +7,12 @@
     "L4",
     "L4"
   ],
-  "intakeLocation": "CoralStationRight"
+  "intakeLocation": "CoralStationRight",
+  "startSeededPositionTranslation": {
+    "x": 11.7,
+    "y": 6.5
+  },
+  "startSeededPositionRotation": {
+    "degrees": 40
+  }
 }

src/main/deploy/auto/3PieceLeft.json

@@ -9,5 +9,12 @@
     "L4",
     "L4"
   ],
-  "intakeLocation": "CoralStationLeft"
+  "intakeLocation": "CoralStationLeft",
+  "startSeededPositionTranslation": {
+    "x": 11.7,
+    "y": 1.5
+  },
+  "startSeededPositionRotation": {
+    "degrees": -40
+  }
 }

src/main/deploy/auto/3PieceRight.json

@@ -9,5 +9,12 @@
     "L4",
     "L4"
   ],
-  "intakeLocation": "CoralStationRight"
+  "intakeLocation": "CoralStationRight",
+  "startSeededPositionTranslation": {
+    "x": 11.7,
+    "y": 6.5
+  },
+  "startSeededPositionRotation": {
+    "degrees": 40
+  }
 }

src/main/deploy/auto/4PieceLeft.json

@@ -11,5 +11,12 @@
     "L4",
     "L4"
   ],
-  "intakeLocation": "CoralStationLeft"
+  "intakeLocation": "CoralStationLeft",
+  "startSeededPositionTranslation": {
+    "x": 11.7,
+    "y": 1.5
+  },
+  "startSeededPositionRotation": {
+    "degrees": -40
+  }
 }

src/main/deploy/auto/ExampleAutoPath.json

@@ -7,5 +7,10 @@
     "L1",
     "L1"
   ],
-  "intakeLocation": "CoralStationRight"
+  "intakeLocation": "CoralStationRight",
+  "startSeededPosition": {
+    "x": 11.7,
+    "y": 6.5,
+    "degrees": 40
+  }
 }

src/main/deploy/constants/comp/FeatureFlags.json

@@ -1,6 +1,6 @@
 {
   "runDrive": true,
-  "runVision": true,
+  "runVision": false,
   "runScoring": true,
   "runClimb": true,
   "runRamp": true

src/main/deploy/pathplanner/paths/New New New Path.path

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.341931818174691,
+        "y": 6.502940340914731
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.574119318174691,
+        "y": 6.173877840914731
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.619744318174691,
+        "y": 6.502940340914731
+      },
+      "prevControl": {
+        "x": 11.317753813036703,
+        "y": 5.944528914045714
+      },
+      "nextControl": {
+        "x": 9.92173482331268,
+        "y": 7.061351767783748
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 11.198096590901963,
+        "y": 2.2550426136420048
+      },
+      "prevControl": {
+        "x": 10.639687499992872,
+        "y": 2.544218750005635
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.46003552397868663,
+      "rotationDegrees": -140.0
+    },
+    {
+      "waypointRelativePos": 0.5648312611012444,
+      "rotationDegrees": -40.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -53.67317404787988
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -154.85521436932117
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/New New Path.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 11.66978181537829,
-        "y": 6.890164987664473
+        "x": 11.67,
+        "y": 6.89
       },
       "prevControl": null,
       "nextControl": {
-        "x": 12.66978181537829,
-        "y": 6.890164987664473
+        "x": 12.484432232602588,
+        "y": 6.482997988367408
       },
       "isLocked": false,
       "linkedName": null
@@ -25,7 +25,7 @@
       },
       "nextControl": {
         "x": 17.47167326274671,
-        "y": 2.930455258018086
+        "y": 2.9304552580180863
       },
       "isLocked": false,
       "linkedName": null

src/main/java/frc/robot/BuildConstants.java

@@ -5,12 +5,14 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025-Robot-Code";
   public static final String VERSION = "unspecified";
+
   public static final int GIT_REVISION = 79;
   public static final String GIT_SHA = "67be46969451c2dd5f8c8669e318a4947be378a5";
   public static final String GIT_DATE = "2025-04-03 20:22:38 EDT";
   public static final String GIT_BRANCH = "comp/CHCMP";
   public static final String BUILD_DATE = "2025-04-06 10:07:35 EDT";
   public static final long BUILD_UNIX_TIME = 1743948455918L;
+
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/frc/robot/StrategyManager.java

@@ -1,5 +1,7 @@
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.DoubleSubscriber;
@@ -8,13 +10,16 @@
 import edu.wpi.first.networktables.StringPublisher;
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.strategies.AutoIntake;
 import frc.robot.commands.strategies.AutoScore;
 import frc.robot.constants.AutoStrategy;
 import frc.robot.constants.AutoStrategyContainer.Action;
 import frc.robot.constants.AutoStrategyContainer.ActionType;
+import frc.robot.constants.FeatureFlags;
+import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.Drive.DesiredLocation;
 import frc.robot.subsystems.scoring.ScoringSubsystem;
@@ -435,6 +440,22 @@ public void autonomousInit(AutoStrategy strategy) {
     this.currentCommand = null;
     this.currentAction = null;
 
+    if (!FeatureFlags.synced.getObject().runVision
+        || drive.getVisionMeasurementCount()
+            < VisionConstants.synced.getObject().minimumVisionMeasurementInitCount) {
+      Pose2d startPose =
+          new Pose2d(strategy.startSeededPositionTranslation, strategy.startSeededPositionRotation);
+      if (DriverStation.getAlliance().isPresent()
+          && DriverStation.getAlliance().get() == Alliance.Blue) {
+        startPose =
+            new Pose2d(
+                17.3736 - startPose.getX(),
+                7.9248 - startPose.getY(),
+                new Rotation2d(startPose.getRotation().getRadians() - Math.PI / 2));
+      }
+      drive.setPose(startPose);
+    }
+
     actions.clear();
     this.addActionsFromAutoStrategy(strategy);
     System.out.println("New actions loaded: " + String.valueOf(actions.size()));

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

@@ -1,6 +1,8 @@
 package frc.robot.constants;
 
 import coppercore.parameter_tools.json.JSONExclude;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import frc.robot.subsystems.drive.Drive.DesiredLocation;
 import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget;
 import java.util.List;
@@ -11,4 +13,6 @@ public class AutoStrategy {
   public List<FieldTarget> scoringLevels = null;
   public final DesiredLocation intakeLocation = null;
   public final Boolean intakeAfterLastScore = true;
+  public final Translation2d startSeededPositionTranslation = null;
+  public final Rotation2d startSeededPositionRotation = null;
 }

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

@@ -24,4 +24,6 @@ public class VisionConstants {
           Units.inchesToMeters(-5.5),
           Units.inchesToMeters(12.0),
           new Rotation3d(0, 0, 0));
+
+  public final Integer minimumVisionMeasurementInitCount = 10;
 }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -123,6 +123,8 @@ public class Drive implements DriveTemplate {
 
   private ChassisSpeeds goalSpeeds = new ChassisSpeeds();
 
+  private int visionValueCount = 0;
+
   public ProfiledPIDController angleController =
       new ProfiledPIDController(
           JsonConstants.drivetrainConstants.rotationAlignKp,
@@ -390,6 +392,11 @@ public void autonomousInit() {
     }
   }
 
+  @AutoLogOutput(key = "Vision/MeasurementCount")
+  public int getVisionMeasurementCount() {
+    return visionValueCount;
+  }
+
   /** remove algae coral stack obstacles for on the fly */
   public void teleopInit() {
     localADStar.setDynamicObstacles(List.of(), getPose().getTranslation());
@@ -1063,6 +1070,7 @@ public void addVisionMeasurement(
       Pose2d visionRobotPoseMeters,
       double timestampSeconds,
       Matrix<N3, N1> visionMeasurementStdDevs) {
+    visionValueCount += 1;
     poseEstimator.addVisionMeasurement(
         visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs);
   }