Initial Vision Subsystem

.vscode/settings.json

@@ -26,5 +26,7 @@
     },
   ],
   "java.test.defaultConfig": "WPIlibUnitTests",
-  "java.compile.nullAnalysis.mode": "automatic"
+  "java.compile.nullAnalysis.mode": "automatic",
+  "java.compile.nullAnalysis.mode": "automatic",
+  "java.debug.settings.onBuildFailureProceed": true
 }

build.gradle

@@ -79,7 +79,8 @@ dependencies {
 
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
-
+
+    implementation 'org.strykeforce:wallEYE:2024.1.1'    
 
 }
 

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,17 @@
+{
+  "HALProvider": {
+    "RoboRIO": {
+      "RoboRIO Input": {
+        "open": true
+      }
+    }
+  },
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

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

@@ -6,10 +6,14 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.vision.VisionSubsystem;
 
 public class RobotContainer {
 
+  VisionSubsystem visionSubsystem;
+
   public RobotContainer() {
+    VisionSubsystem visionSubsystem = new VisionSubsystem();
     configureBindings();
   }
 

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

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "crescendo";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 26;
-  public static final String GIT_SHA = "fa73e25bfd3b20699d59393a555615536b928697";
-  public static final String GIT_DATE = "2024-01-14 14:35:09 EST";
-  public static final String GIT_BRANCH = "Initial-Pivot";
-  public static final String BUILD_DATE = "2024-01-14 14:40:34 EST";
-  public static final long BUILD_UNIX_TIME = 1705261234507L;
+  public static final int GIT_REVISION = 29;
+  public static final String GIT_SHA = "e8a0b0279ffaa14bc0eef5607a2958b9cc54b71f";
+  public static final String GIT_DATE = "2024-01-14 14:33:16 EST";
+  public static final String GIT_BRANCH = "Initial-Vision";
+  public static final String BUILD_DATE = "2024-01-14 14:43:32 EST";
+  public static final long BUILD_UNIX_TIME = 1705261412536L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -0,0 +1,23 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+
+public class VisionConstants {
+  public static final int kNumCams = 1;
+  public static final double kMaxTimeNoVision = 5;
+  public static final int kResultsForWheels = 5;
+  public static final double kTimeToDecayDev = 3;
+  public static final double kStdDevDecayCoeff = -0.005;
+  public static final double kMinStdDev = 0.01;
+  public static final double kMaxAmbig = 0.15;
+
+  // Increase these numbers to trust global measurements from vision less. This matrix is in the
+  // form [x, y, theta]ᵀ, with units in meters and radians.
+  // Vision Odometry Standard devs
+  public static Matrix<N3, N1> kVisionMeasurementStdDevs =
+      VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5000));
+}

src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

@@ -1,3 +1,181 @@
 package frc.robot.subsystems.vision;
 
-public class VisionSubsystem {}
+import WallEye.WallEyeCam;
+import WallEye.WallEyeResult;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.VisionConstants;
+import java.util.ArrayList;
+
+public class VisionSubsystem extends SubsystemBase {
+
+  // Private Variables
+  WallEyeCam[] cams;
+  Translation3d[] offsets = {new Translation3d(0, 0, 0), new Translation3d(0, 0, 0)};
+  Rotation3d[] rotsOff = {new Rotation3d(0, 0, 0), new Rotation3d(0, 0, 0)};
+  String[] names = {"1", "2"};
+  int[] camIndex = {1, 2};
+  ArrayList<Pair<WallEyeResult, Integer>> validResults = new ArrayList<>();
+  VisionStates curState = VisionStates.TRUSTWHEELS;
+  boolean visionUpdates = true;
+  double timeLastVision = 0;
+  int updatesToWheels = 0;
+
+  // Deadeye<TargetListTargetData> cam = new Deadeye<TargetListTargetData>("A0",
+  // TargetListTargetData.class, NetworkTableInstance.getDefault(), null);
+
+  public Matrix<N3, N1> adaptiveVisionMatrix;
+
+  // Constructor
+  public VisionSubsystem() {
+    cams = new WallEyeCam[VisionConstants.kNumCams];
+    adaptiveVisionMatrix = VisionConstants.kVisionMeasurementStdDevs.copy();
+
+    // Initialize all walleyecams
+    for (int i = 0; i < VisionConstants.kNumCams; ++i) {
+      cams[i] = new WallEyeCam(names[i], camIndex[i], -1);
+    }
+  }
+
+  // Getter/Setter Methods
+  public void setVisionUpdates(boolean visionUpdates) {
+    this.visionUpdates = visionUpdates;
+  }
+
+  public boolean isVisionUpdatingDrive() {
+    return visionUpdates;
+  }
+
+  public boolean isCameraConnected(int index) {
+    return cams[index].isCameraConnected();
+  }
+
+  public VisionStates getState() {
+    return curState;
+  }
+
+  public void setState(VisionStates state) {
+    curState = state;
+  }
+
+  // Helper Methods
+
+  private double getSeconds() {
+    return RobotController.getFPGATime() / 1000000.0;
+  }
+
+  // FIXME NEED DRIVE ODOMETRY
+  private boolean isPoseValidWithWheels(WallEyeResult test, Translation3d pose) {
+    if (isPoseValidWithoutWheels(test)) {
+      // Check with drive odometry and cross reference the two BUT thats for later
+      return true;
+    }
+    return false;
+  }
+
+  private boolean isPoseValidWithoutWheels(WallEyeResult test) {
+    return test.getNumTags() >= 2 || test.getAmbiguity() <= VisionConstants.kMaxAmbig;
+  }
+
+  // Periodic
+  @Override
+  public void periodic() {
+
+    // cam.getEnabled();
+
+    // If enough time elapses trust vision or if enough time elapses reset the counter
+    if (getSeconds() - timeLastVision > VisionConstants.kMaxTimeNoVision) {
+      curState = VisionStates.TRUSTVISION;
+      updatesToWheels = 0;
+    }
+
+    // If the counter gets high enough trust wheels
+    if (updatesToWheels >= VisionConstants.kResultsForWheels) {
+      curState = VisionStates.TRUSTWHEELS;
+      adaptiveVisionMatrix = VisionConstants.kVisionMeasurementStdDevs.copy();
+    }
+
+    // Clear out old results
+    validResults.clear();
+
+    // Go through each camera and store result and array idx in pair
+    for (int i = 0; i < VisionConstants.kNumCams; ++i) {
+      if (cams[i].hasNewUpdate()) {
+        timeLastVision = getSeconds();
+        validResults.add(new Pair<WallEyeResult, Integer>(cams[i].getResults(), i));
+      }
+    }
+
+    // Tightens std devs if time elapses
+    if (getSeconds() - timeLastVision >= VisionConstants.kTimeToDecayDev) {
+
+      // Take x and y weights and linearly decrease them
+      for (int i = 0; i < 2; ++i) {
+        double estimatedWeight =
+            VisionConstants.kVisionMeasurementStdDevs.get(i, 0)
+                - VisionConstants.kStdDevDecayCoeff
+                    * (getSeconds() - timeLastVision - VisionConstants.kTimeToDecayDev);
+        adaptiveVisionMatrix.set(
+            i,
+            0,
+            estimatedWeight > VisionConstants.kMinStdDev
+                ? estimatedWeight
+                : VisionConstants.kMinStdDev);
+      }
+    }
+
+    // Go through results
+    for (Pair<WallEyeResult, Integer> res : validResults) {
+
+      // Take out data from pair
+      WallEyeResult result = res.getFirst();
+      int idx = res.getSecond();
+      System.out.print(result.getCameraPose());
+
+      // Get center of Robot pose
+      Pose3d cameraPose = result.getCameraPose();
+      Translation3d centerPose =
+          cameraPose
+              .getTranslation()
+              .plus(offsets[idx].rotateBy(cameraPose.getRotation().minus(rotsOff[idx])));
+
+      // If updating with vision go into state machine to update
+      if (visionUpdates) {
+        switch (curState) {
+
+            // Uses wheels to act as a filter for the cameras
+          case TRUSTWHEELS:
+            if (isPoseValidWithWheels(result, centerPose)) {
+              // Feed into odometry
+
+            }
+            break;
+
+            // Purely trust vision
+          case TRUSTVISION:
+            if (isPoseValidWithoutWheels(result)) {
+              updatesToWheels++;
+              // Feed into odometry
+            }
+
+            break;
+        }
+      }
+    }
+  }
+
+  // Grapher
+
+  // State Enum
+  public enum VisionStates {
+    TRUSTWHEELS,
+    TRUSTVISION
+  }
+}

vendordeps/deadeye.json

@@ -0,0 +1,20 @@
+{
+    "fileName": "deadeye.json",
+    "name": "StrykeForce-Deadeye",
+    "version": "22.2.0",
+    "uuid": "a8620993-3ffc-425e-95d9-a5e73eabef40",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://d1cfub2bqs98lp.cloudfront.net/repo"
+    ],
+    "jsonUrl": "http://maven.strykeforce.org/deadeye.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.strykeforce",
+            "artifactId": "deadeye",
+            "version": "22.2.0"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": []
+}