Merge pull request #10 from strykeforce/Initial-Vision

added filter and logging

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

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

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

@@ -14,6 +14,12 @@ public class VisionConstants {
   public static final double kStdDevDecayCoeff = -0.005;
   public static final double kMinStdDev = 0.01;
   public static final double kMaxAmbig = 0.15;
+  public static final int kMaxTimesOffWheels = 5;
+
+  // Velocity Filter
+  public static final double kLinearCoeffOnVelFilter = 0.1;
+  public static final double kOffsetOnVelFilter = 0.2;
+  public static final double kSquaredCoeffOnVelFilter = 0.2;
 
   public static Matrix<N3, N1> kStateStdDevs = VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(0));
 

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

@@ -4,15 +4,21 @@
 import WallEye.WallEyeResult;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 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 frc.robot.subsystems.drive.DriveSubsystem;
 import java.util.ArrayList;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class VisionSubsystem extends SubsystemBase {
 
@@ -27,14 +33,21 @@ public class VisionSubsystem extends SubsystemBase {
   boolean visionUpdates = true;
   double timeLastVision = 0;
   int updatesToWheels = 0;
+  DriveSubsystem driveSubsystem;
+  int offWheels = 0;
+  Logger logger;
 
   // Deadeye<TargetListTargetData> cam = new Deadeye<TargetListTargetData>("A0",
   // TargetListTargetData.class, NetworkTableInstance.getDefault(), null);
 
   public Matrix<N3, N1> adaptiveVisionMatrix;
 
   // Constructor
-  public VisionSubsystem() {
+  public VisionSubsystem(DriveSubsystem driveSubsystem) {
+    this.driveSubsystem = driveSubsystem;
+
+    logger = LoggerFactory.getLogger(this.getClass());
+
     cams = new WallEyeCam[VisionConstants.kNumCams];
     adaptiveVisionMatrix = VisionConstants.kVisionMeasurementStdDevs.copy();
 
@@ -62,6 +75,7 @@ public VisionStates getState() {
   }
 
   public void setState(VisionStates state) {
+    logger.info("{} -> {}", curState, state);
     curState = state;
   }
 
@@ -74,8 +88,22 @@ private double getSeconds() {
   // 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;
+
+      // Get Speed and current position
+      ChassisSpeeds speed = driveSubsystem.getFieldRelSpeed();
+      Pose2d curPose = driveSubsystem.getPoseMeters();
+
+      // Find the x and y difference between cam and wheels
+      Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d()));
+      double magnitudeVel =
+          Math.sqrt(Math.pow(speed.vxMetersPerSecond, 2) + Math.pow(speed.vyMetersPerSecond, 2));
+      double magnitudeDisp = Math.sqrt(Math.pow(disp.getX(), 2) + Math.pow(disp.getY(), 2));
+
+      // Test to see if the displacement falls beneath a line based on velocity
+      return (magnitudeDisp
+          < ((magnitudeVel * VisionConstants.kLinearCoeffOnVelFilter)
+              + VisionConstants.kOffsetOnVelFilter
+              + Math.pow((magnitudeVel * VisionConstants.kSquaredCoeffOnVelFilter), 2)));
     }
     return false;
   }
@@ -91,13 +119,18 @@ public void periodic() {
     // cam.getEnabled();
 
     // If enough time elapses trust vision or if enough time elapses reset the counter
-    if (getSeconds() - timeLastVision > VisionConstants.kMaxTimeNoVision) {
+    if ((getSeconds() - timeLastVision > VisionConstants.kMaxTimeNoVision)
+        && (curState != VisionStates.TRUSTVISION || updatesToWheels >= 1)) {
+      logger.info("{} -> TRUSTVISION");
       curState = VisionStates.TRUSTVISION;
       updatesToWheels = 0;
     }
 
     // If the counter gets high enough trust wheels
-    if (updatesToWheels >= VisionConstants.kResultsForWheels) {
+    if ((updatesToWheels >= VisionConstants.kResultsForWheels)
+        && curState != VisionStates.TRUSTWHEELS) {
+      logger.info("{} -> TRUSTWHEELS", curState);
+      updatesToWheels = 0;
       curState = VisionStates.TRUSTWHEELS;
       adaptiveVisionMatrix = VisionConstants.kVisionMeasurementStdDevs.copy();
     }
@@ -146,21 +179,37 @@ public void periodic() {
               .getTranslation()
               .plus(offsets[idx].rotateBy(cameraPose.getRotation().minus(rotsOff[idx])));
 
+      String output = "VisionSubsystem/Cam" + res.getSecond().toString() + "Pose";
+      org.littletonrobotics.junction.Logger.recordOutput(output, centerPose);
+
       // 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)) {
+              String outputAccept =
+                  "VisionSubsystem/AcceptedCam" + res.getSecond().toString() + "Pose";
+              org.littletonrobotics.junction.Logger.recordOutput(outputAccept, centerPose);
               // Feed into odometry
-
+              offWheels--;
+
+            } else {
+              offWheels = offWheels < 0 ? 1 : offWheels++;
+              if (offWheels >= VisionConstants.kMaxTimesOffWheels) {
+                logger.info("{} -> TRUSTVISION", curState);
+                curState = VisionStates.TRUSTVISION;
+              }
             }
             break;
 
             // Purely trust vision
           case TRUSTVISION:
             if (isPoseValidWithoutWheels(result)) {
+              String outputAccept =
+                  "VisionSubsystem/AcceptedCam" + res.getSecond().toString() + "Pose";
+              org.littletonrobotics.junction.Logger.recordOutput(outputAccept, centerPose);
               updatesToWheels++;
               // Feed into odometry
             }