Usb vision

src/main/deploy/choreo/Path1.traj

@@ -131,4 +131,4 @@
   "splits":[0]
  },
  "events":[]
-}
+}

src/main/deploy/choreo/Path2.traj

@@ -50,4 +50,4 @@
   "splits":[0]
  },
  "events":[]
-}
+}

src/main/deploy/choreo/Path3.traj

@@ -37,4 +37,4 @@
   "splits":[0]
  },
  "events":[]
-}
+}

src/main/deploy/choreo/choreo.chor

@@ -75,4 +75,4 @@
   }
  },
  "generationFeatures":[]
-}
+}

src/main/java/frc/robot/Constants.java

@@ -241,6 +241,15 @@ public static final SparkFlexConfig MOTOR_CONFIG() {
   }
 
   public class VisionConstants {
+    public static final String ROOT_TABLE = "Vision";
+
+    public static final Transform3d robotToCamera =
+        new Transform3d(
+            Units.Inches.of(8.375),
+            Units.Inches.of(15),
+            Units.Inches.of(19.25),
+            new Rotation3d(Units.Degrees.of(30), Units.Degrees.of(0), (Units.Degrees.of(0))));
+
     // AprilTag layout
     public static AprilTagFieldLayout aprilTagLayout =
         AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

src/main/java/frc/robot/RobotContainer.java

@@ -41,6 +41,7 @@
 import frc.robot.subsystems.intake.IntakeIO;
 import frc.robot.subsystems.intake.IntakeIOSim;
 import frc.robot.subsystems.shooter.*;
+import frc.robot.subsystems.usb_vision.*;
 import java.util.function.Supplier;
 
 /**
@@ -57,6 +58,7 @@ public class RobotContainer {
   private final Intake intake;
   private final Hopper hopper;
   private final Shooter shooter;
+  private final USBVision vision;
   // private final Vision vision;
 
   private final SuperStateMachine superStateMachine;
@@ -75,8 +77,9 @@ public RobotContainer() {
         drive = new Drive(new DriveModules(true), new GyroIOPigeon2(), controller);
         elevator = new Elevator(new ElevatorIO() {});
         intake = new Intake(new IntakeIO() {});
-        hopper = new Hopper(new HopperIO() {});
         shooter = new Shooter(new ShooterIO() {});
+        hopper = new Hopper(new HopperIO() {});
+        vision = new USBVision(new USBVisionIOReal() {}, drive::addVisionMeasurement);
         // vision =
         //     new Vision(
         //         drive::addVisionMeasurement,
@@ -90,6 +93,7 @@ public RobotContainer() {
         intake = new Intake(new IntakeIOSim());
         shooter = new Shooter(new ShooterIOSim());
         hopper = new Hopper(new HopperIOSim());
+        vision = new USBVision(new USBVisionIO() {}, drive::addVisionMeasurement);
         // vision =
         //     new Vision(
         //         drive::addVisionMeasurement,
@@ -104,6 +108,7 @@ public RobotContainer() {
         intake = new Intake(new IntakeIO() {});
         shooter = new Shooter(new ShooterIO() {});
         hopper = new Hopper(new HopperIO() {});
+        vision = new USBVision(new USBVisionIO() {}, drive::addVisionMeasurement);
         // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
         break;
     }

src/main/java/frc/robot/subsystems/usb_vision/USBVision.java

@@ -0,0 +1,93 @@
+// 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 frc.robot.subsystems.usb_vision;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.team2930.TunableNumberGroup;
+import frc.lib.team6328.LoggedTunableNumber;
+import frc.robot.Constants.VisionConstants;
+import org.littletonrobotics.junction.Logger;
+
+public class USBVision extends SubsystemBase {
+  // Logging
+
+  // private static final LoggerGroup logGroup = LoggerGroup.build(VisionConstants.ROOT_TABLE);
+
+  // private static final LoggerEntry.Bool logOnTarget = logGroup.buildBoolean("onTarget");
+
+  // Tunable numbers
+
+  private static final TunableNumberGroup group =
+      new TunableNumberGroup(VisionConstants.ROOT_TABLE);
+
+  private final LoggedTunableNumber angleTolerance = group.build("angleTolerance", 0.1);
+
+  private final USBVisionIO io;
+  private final USBVisionInputsAutoLogged inputs = new USBVisionInputsAutoLogged();
+  private final VisionConsumer consumer;
+
+  /** Creates a new Vision Subsystem. */
+  public USBVision(USBVisionIO io, VisionConsumer consumer) {
+    this.io = io;
+    this.consumer = consumer;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs(VisionConstants.ROOT_TABLE, inputs);
+
+    for (var observation : inputs.poseObservations) {
+      double stdDevFactor =
+          Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
+      double linearStdDev = VisionConstants.linearStdDevBaseline * stdDevFactor;
+      double angularStdDev = VisionConstants.angularStdDevBaseline * stdDevFactor;
+
+      consumer.accept(
+          observation.pose().toPose2d(),
+          observation.timestamp(),
+          VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev));
+    }
+
+    // Updating tunable numbers
+    var hc = hashCode();
+    if (angleTolerance.hasChanged(hc)) {
+      setAngleTolerance();
+    }
+  }
+
+  // Setters
+
+  public void setAngleTolerance() {
+    io.setAngleTolerance(angleTolerance.get());
+  }
+
+  // Getters
+
+  // public boolean isOnTarget() {
+  //   return inputs.rotToTarget.getZ <= angleTolerance.get() * (Math.PI/180.0);
+  // }
+
+  // public Rotation3d getRotateToTarget() {
+  //   return inputs.rotatePose;
+  // }
+
+  // public Pose3d getDistToTarget() {
+  //   return inputs.targetPose;
+  // }
+
+  @FunctionalInterface
+  public static interface VisionConsumer {
+    public void accept(
+        Pose2d visionRobotPoseMeters,
+        double timestampSeconds,
+        Matrix<N3, N1> visionMeasurementStdDevs);
+  }
+}

src/main/java/frc/robot/subsystems/usb_vision/USBVisionIO.java

@@ -0,0 +1,38 @@
+package frc.robot.subsystems.usb_vision;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface USBVisionIO {
+
+  public static record PoseObservation(
+      double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance) {}
+
+  public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+
+  /** Contains all of the input data received from hardware. */
+  @AutoLog
+  public class USBVisionInputs {
+    public TargetObservation latestTargetObservation =
+        new TargetObservation(new Rotation2d(), new Rotation2d());
+    public PoseObservation[] poseObservations = new PoseObservation[0];
+
+    // public Pose3d robotVisionPose;
+
+    // public Pose3d targetPose;
+    // public double targetX;
+    // public double targetY;
+    // public double targetZ;
+
+    // public Rotation3d rotatePose;
+    // public double rotateX;
+    // public double rotateY;
+    // public double rotateZ;
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(USBVisionInputs inputs) {}
+
+  public default void setAngleTolerance(double angle) {}
+}