Vision additions w/ testing (probs not right but getting there...)

src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java

@@ -6,10 +6,12 @@
 import java.util.List;
 import java.util.Map;
 
+import org.littletonrobotics.junction.Logger;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import edu.wpi.first.math.estimator.PoseEstimator;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 
 public class VisionMap {
 
@@ -45,8 +47,7 @@ public VisionMap(final List<CameraSource> visionSources) {
      * @param <T>       Estimator wheel type.
      * @param estimator The WPIlib estimator object.
      */
-    public <T> void updateData(Data<T> data) {
-        data.targets.clear();
+    public <T> void updateData(Data data) {
         for (var source : this.visionSources) {
             var results = source.camera.getAllUnreadResults();
             if (!results.isEmpty()) {
@@ -58,10 +59,14 @@ public <T> void updateData(Data<T> data) {
                             est.timestampSeconds);
                 });
                 // Now copy the targets that are found
-                // data.targets.add(List.copyOf(latestResult.targets));
+                if (latestResult.targets.size() != 0) {
+                    data.targets.clear();
+                }
                 for (var target : latestResult.targets) {
                     int targetID = target.getFiducialId();
+                    Logger.recordOutput("target" + targetID, target.bestCameraToTarget);
                     target.bestCameraToTarget = target.bestCameraToTarget.plus(source.robotToCam);
+                    Logger.recordOutput("targetOffset" + targetID, target.bestCameraToTarget);
                     if (data.targets.containsKey(target.getFiducialId())) {
                         data.targets.get(targetID).add(target);
                     } else {
@@ -74,8 +79,8 @@ public <T> void updateData(Data<T> data) {
         }
     }
 
-    public static class Data<T> {
-        public PoseEstimator<T> estimator;
+    public static class Data {
+        public SwerveDrivePoseEstimator estimator;
         public Map<Integer, List<PhotonTrackedTarget>> targets = new HashMap<>();
     }
 }

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

@@ -134,9 +134,8 @@ public void configureButtonBindings() {
         driveController.back().onTrue(commandSequences.resetAll());
         driveController.leftBumper()
                 .whileTrue(drive.robotCentricDrive());
-        driveController.rightBumper().whileTrue(drive.aimAtReefCenter());
-        driveController.rightTrigger().whileTrue(drive.alignToReefBranch(Branch.RIGHT_BRANCH));
-        driveController.leftTrigger().whileTrue(drive.alignToReefBranch(Branch.LEFT_BRANCH));
+        driveController.rightTrigger().whileTrue(drive.moveToBranch(Branch.RIGHT_BRANCH));
+        driveController.leftTrigger().whileTrue(drive.moveToBranch(Branch.LEFT_BRANCH));
 
         copilotController.a().onTrue(commandSequences.intake());
 

src/main/java/frc/robot/Vision.java

@@ -131,6 +131,9 @@ public Transform2d pickBestReefLocation(Map<Integer, List<PhotonTrackedTarget>>
         // Find the closest april tag target
         // For now we only look at a single camera's data
         // In the future we may want to combine the pose data for the april tags?
+        if (targets.size() == 0) {
+            return new Transform2d();
+        }
         var closestTarget = Collections.min(
                 targets.entrySet(),
                 Comparator.comparing((Map.Entry<Integer, List<PhotonTrackedTarget>> entry) -> {

src/main/java/frc/robot/maps/ColdStart.java

@@ -27,6 +27,7 @@
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -117,8 +118,12 @@ public SwerveDriveMap getDriveMap() {
     public VisionMap getVisionMap() {
 
         return new VisionMap(
-                new CameraSource("FLCamera", new Transform3d()),
-                new CameraSource("FRCamera", new Transform3d()));
+                new CameraSource("FLCamera",
+                        new Transform3d(Units.inchesToMeters(9.43), Units.inchesToMeters(10.72),
+                                Units.inchesToMeters(8.24), new Rotation3d(0, -68, -16.76))),
+                new CameraSource("FRCamera", new Transform3d(Units.inchesToMeters(-10.72), Units.inchesToMeters(9.43),
+                        Units.inchesToMeters(8.24),
+                        new Rotation3d(0, Units.degreesToRadians(-68), Units.degreesToRadians(16.76)))));
     }
 
     @Override

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

@@ -31,13 +31,14 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Vision;
+import frc.robot.FieldConstants.Reef;
 import frc.robot.Vision.Branch;
 
 public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
 
     public final SwerveDriveKinematics kinematics;
     private final VisionMap visionMap;
-    private final VisionMap.Data<SwerveDrivePoseEstimator> visionData = new VisionMap.Data<>();
+    private final VisionMap.Data visionData = new VisionMap.Data();
     private final Vision vision = new Vision();
 
     private final double maxDriveSpeedMetersPerSecond;
@@ -83,6 +84,8 @@ public Drive(SwerveDriveMap map, DoubleSupplier xSpeed, DoubleSupplier ySpeed, D
                 VecBuilder.fill(0.02, 0.02, 0.01),
                 VecBuilder.fill(0.1, 0.1, 0.01));
 
+        visionData.estimator = estimator;
+
         AutoBuilder.configure(estimator::getEstimatedPosition,
                 this::setPose,
                 this::getSpeeds,
@@ -179,6 +182,8 @@ public void periodic() {
         Logger.recordOutput("Estimator Pose", estimator.getEstimatedPosition());
         Logger.recordOutput("Pose Angle", estimator.getEstimatedPosition().getRotation());
         Logger.recordOutput("Robot Rotation Gyro", getMap().gyro.getRotation2d());
+        Logger.recordOutput("Target Branch", targetBranch);
+
     }
 
     private double calculateRotationSpeed(double targetAngleDegrees) {
@@ -209,13 +214,18 @@ private void periodicMove(final double xSpeed, final double ySpeed, final double
             rotationSpeed = calculateRotateSpeedToTarget(aimTarget::get);
         }
         if (targetBranch != Branch.NONE) {
-            alignToReefBranch(targetBranch);
             vision.filterReefTags(isBlueAlliance, visionData.targets);
+            Transform2d reefLocation = vision.pickBestReefLocation(visionData.targets);
             Transform2d robotToBranch = vision
-                    .adjustTranslationForBranch(vision.pickBestReefLocation(visionData.targets), targetBranch);
-            translateXSpeed = translationPID_X.calculate(robotToBranch.getX());
-            translateYSpeed = translationPID_Y.calculate(robotToBranch.getY());
-            rotationSpeed = rotationPID.calculate(robotToBranch.getRotation().getDegrees());
+                    .adjustTranslationForBranch(reefLocation, targetBranch);
+            Logger.recordOutput("Reef Location Transform2d", estimator.getEstimatedPosition().plus(reefLocation));
+            Logger.recordOutput("Robot To Branch Transform2d", estimator.getEstimatedPosition().plus(robotToBranch));
+            Pose2d robotPose = estimator.getEstimatedPosition();
+            translateXSpeed = translationPID_X.calculate(robotPose.getX(), robotPose.getX() + robotToBranch.getX());
+            translateYSpeed = translationPID_Y.calculate(robotPose.getY(), robotPose.getY() + robotToBranch.getY());
+            rotationSpeed = rotationPID.calculate(robotPose.getRotation().getDegrees(),
+                    robotPose.getRotation().getDegrees() - robotToBranch.getRotation().getDegrees());
+            Logger.recordOutput("Robot To Branch Angle", robotToBranch.getRotation().getDegrees());
         }
 
         move(translateXSpeed, translateYSpeed, rotationSpeed, isRobotCentric);