Merge pull request #121 from strykeforce/Implement-WallEye

mwitcpalek · web-flow · commit d04cb667fc9a · 2023-10-17T18:30:23.000-04:00
Implement wall eye - build fails b/c of wallEye maven dependency
diff --git a/build.gradle b/build.gradle
@@ -52,6 +52,7 @@ dependencies {
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
+    implementation "org.strykeforce:wallEYE:1.0.0"
     implementation("com.opencsv:opencsv:5.7.1")
     implementation("com.squareup.moshi:moshi:1.13.0")
     implementation 'net.jafama:jafama:2.3.2'
diff --git a/networktables.json b/networktables.json
@@ -0,0 +1 @@
+[]
diff --git a/simgui-ds.json b/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
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
@@ -0,0 +1,32 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Match/SetAllianceBlue": "Command",
+      "/Shuffleboard/Match/SetAllianceRed": "Command",
+      "/Shuffleboard/Match/Update With Vision": "Command"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "High": {
+        "open": true
+      },
+      "WallEYE": {
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "glass@1": {
+      "Publishers": {
+        "open": true
+      },
+      "open": true
+    },
+    "visible": true
+  }
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -387,7 +387,7 @@ public static final class VisionConstants {
 
     public static final double kCameraOffset = .335; // was .273 on driveChasis
     public static final double kCameraAngleOffset = 0; // DEGREES was 24 on driveChasis
-    public static final double kHighCameraAngleOffset = 0.0;
+    public static final double kHighCameraAngleOffset = 0;
     public static final double kHighCameraOffset = -0.075;
     public static final double kLastUpdateCloseEnoughThreshold = 2.0; // IN SECONDS
     public static final double kLastUpdateCloseEnoughThresholdYaw = 1.0;
@@ -398,7 +398,7 @@ public static final class VisionConstants {
     public static int kBufferLookupOffset = 2;
 
     public static Matrix<N3, N1> kStateStdDevs =
-        VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5));
+        VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(0));
 
     // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is
     // in the form [theta], with units in radians.
@@ -409,7 +409,32 @@ public static final class VisionConstants {
     // 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(5));
+        VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5000));
+
+    // When trusting wheels the amount of times before the decay goes away
+    public static int kNumResultsToResetStdDev = 3;
+
+    // Times it takes to trust wheels after trusting vision
+    public static final int kNumResultsToTrustWheels = 5;
+
+    // Time (seconds) before std dev starts to decay
+    public static final double kTimeToTightenStdDev = 1.0;
+
+    // Time (seconds) before getting kicked into only trusting camera
+    public static final double kTimeToTrustCamera = 10.0;
+
+    // The linear rate of change on the std dev
+    public static final double kStdDevDecayCoeff = 0.01 / 3.0;
+
+    // Minimum std dev for the declining std dev
+    public static final double kMinimumStdDev = 0.01;
+
+    // Constants for the vision filter equations
+    public static final int kMaxVisionOff = 5;
+    public static final double kLinearCoeffOnVelFilter = 0.1;
+    public static final double kOffsetOnVelFilter = 0.2;
+    public static final double kSquaredCoeffOnVelFilter = 0.2;
+    public static final double kCloseDistance = 4.0;
   }
 
   public static final class FieldConstants {
@@ -877,7 +902,7 @@ public static class CompConstants {
     public static final double kShoulderFollowerZeroTicks = 2097; // 2057
 
     // Intake
-    public static final int kIntakeZeroTicks = 3_300; // 2440 ->2540->2_900
+    public static final int kIntakeZeroTicks = 3_700; // 3300
     public static final double kExtendPosTicks = -2_200;
 
     // Hand
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -168,12 +168,10 @@ public RobotContainer() {
             rgbLightsSubsystem);
 
     driveSubsystem.setRobotStateSubsystem(robotStateSubsystem);
+    visionSubsystem.setRobotStateSubsystem(robotStateSubsystem);
 
     driveSubsystem.setVisionSubsystem(visionSubsystem);
-    visionSubsystem.setFillBuffers(true); // FIXME TRUE
-
-    // FIX ME
-    robotStateSubsystem.setAllianceColor(Alliance.Blue);
+    // visionSubsystem.setFillBuffers(true); // FIXME TRUE
 
     configurePaths();
     // configureDriverButtonBindings();
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -30,9 +30,9 @@
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants;
 import frc.robot.Constants.DriveConstants;
-import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.RobotStateSubsystem.GamePiece;
 import frc.robot.subsystems.RobotStateSubsystem.TargetCol;
+import frc.robot.subsystems.VisionSubsystem.VisionStates;
 import java.nio.file.Paths;
 import java.util.ArrayList;
 import java.util.Set;
@@ -349,10 +349,9 @@ public void driveToPose(TargetCol targetCol) {
     if (Math.abs(getFieldRelSpeed().vxMetersPerSecond) // FIXME TEMP CHECK IF STATEMENT
                 <= DriveConstants.kMaxSpeedToAutoDrive
             && Math.abs(getFieldRelSpeed().vyMetersPerSecond) <= DriveConstants.kMaxSpeedToAutoDrive
-            && (visionSubsystem.lastUpdateWithinThresholdTime(
-                    VisionConstants.kLastUpdateCloseEnoughThreshold)
+            && (visionSubsystem.getState() == VisionStates.trustWheels
                 || robotStateSubsystem.getIsAuto())
-            && ((visionSubsystem.getBufferedVelocity() <= DriveConstants.kMaxSpeedForCamUpdate))
+            && ((true))
         || robotStateSubsystem.getIsAuto()) {
       setAutoDriving(true);
       if (robotStateSubsystem.getGamePiece() != GamePiece.NONE) {
@@ -385,8 +384,7 @@ public void driveToPose(TargetCol targetCol) {
                   >= DriveConstants.kMaxSpeedToAutoDrive
               && Math.abs(getFieldRelSpeed().vyMetersPerSecond)
                   >= DriveConstants.kMaxSpeedToAutoDrive,
-          visionSubsystem.lastUpdateWithinThresholdTime(
-              VisionConstants.kLastUpdateCloseEnoughThreshold));
+          visionSubsystem.getState() == VisionStates.trustWheels);
       logger.info("{} -> AUTO_DRIVE_FAILED", currDriveState);
       currDriveState = DriveStates.AUTO_DRIVE_FAILED;
     }
@@ -1037,8 +1035,8 @@ public void updateOdometryWithVision(Pose2d calculatedPose) {
     odometryStrategy.addVisionMeasurement(calculatedPose, Timer.getFPGATimestamp());
   }
 
-  public void updateOdometryWithVision(Pose2d calculatedPose, long timestamp) {
-    odometryStrategy.addVisionMeasurement(calculatedPose, timestamp);
+  public void updateOdometryWithVision(Pose2d calculatedPose, double timestamp) {
+    if (visionUpdates) odometryStrategy.addVisionMeasurement(calculatedPose, timestamp);
   }
 
   // Holonomic Controller
diff --git a/src/main/java/frc/robot/subsystems/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/RobotStateSubsystem.java
@@ -15,7 +15,6 @@
 import frc.robot.Constants.HandConstants;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.RobotStateConstants;
-import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.ArmSubsystem.ArmState;
 import frc.robot.subsystems.DriveSubsystem.DriveStates;
 import frc.robot.subsystems.HandSubsystem.HandStates;
@@ -320,6 +319,7 @@ public void toAutoDrive() {
         "Starting AutoDrive. Gamepiece: {}, TargetCol: {}", gamePiece.toString(), tempTargetCol);
     driveSubsystem.driveToPose(tempTargetCol); // FIXME
   }
+
   /**
    * @param isOnAllianceSide Is the robot on the alliance side of the charge station(Towards Tori)
    */
@@ -430,13 +430,14 @@ public boolean shouldFastStowArm() {
   public void periodic() {
     switch (currRobotState) {
       case STOW:
-        // if (!hasZeroedHand && handSubsystem.getVel() <= HandConstants.kHandVelocityThreshold) {
-        //   stableHandVel++;
-        //   if (stableHandVel >= HandConstants.kHandVelStable) {
-        //     stableHandVel = 0;
-        //     handSubsystem.zeroHand();
-        //     hasZeroedHand = true;
-        //   }
+        // if (!hasZeroedHand && handSubsystem.getVel() <=
+        // HandConstants.kHandVelocityThreshold) {
+        // stableHandVel++;
+        // if (stableHandVel >= HandConstants.kHandVelStable) {
+        // stableHandVel = 0;
+        // handSubsystem.zeroHand();
+        // hasZeroedHand = true;
+        // }
         // }
         if (currRobotState != nextRobotState) {
           switch (nextRobotState) {
@@ -937,7 +938,7 @@ public void periodic() {
         // INDICATOR STATE
 
         // if (driveSubsystem.currDriveState == DriveStates.AUTO_BALANCE_FINISHED) {
-        //   //Auto_Balance_Finished
+        // //Auto_Balance_Finished
         // }
         break;
       case CHECK_AMBIGUITY:
@@ -950,11 +951,11 @@ public void periodic() {
           currRobotState = RobotState.SHELF_WAIT_TRANSITION;
         }
         // if (visionSubsystem.lastUpdateWithinThresholdTime(
-        //     VisionConstants.kLastUpdateCloseEnoughThreshold)) {
-        //   rgbLightsSubsystem.setColor(0.0, 1.0, 1.0);
-        //   // toAutoDrive();
+        // VisionConstants.kLastUpdateCloseEnoughThreshold)) {
+        // rgbLightsSubsystem.setColor(0.0, 1.0, 1.0);
+        // // toAutoDrive();
         // } else {
-        //   rgbLightsSubsystem.setColor(1.0, 0.0, 0.0);
+        // rgbLightsSubsystem.setColor(1.0, 0.0, 0.0);
         // }
         break;
       case RECOVER_GAMEPIECE:
@@ -987,11 +988,7 @@ public void periodic() {
                 && driveSubsystem.getPoseMeters().getX()
                     <= (RobotStateConstants.kFieldMaxX - DriveConstants.kPastBumpIndicateX)))
         && !driveSubsystem.isAutoDriving()) {
-      if (visionSubsystem.lastUpdateWithinThresholdTime(
-              VisionConstants.kLastUpdateCloseEnoughThreshold
-                  - VisionConstants.kDifferenceCloseEnoughThreshold)
-          && (visionSubsystem.getBufferedVelocity() <= DriveConstants.kMaxSpeedForCamUpdate))
-        rgbLightsSubsystem.setCubeColor();
+      if (true) rgbLightsSubsystem.setCubeColor();
       else rgbLightsSubsystem.setColor(1.0, 0.0, 0.0);
     }
   }
@@ -1048,35 +1045,37 @@ public Pose2d getAutoPlaceDriveTarget(double yCoord, TargetCol tempTargetCol) {
   }
 
   // public double autoDriveYawRight(double yCoord) {
-  //   int gridIndex =
-  //       ((yCoord > Constants.RobotStateConstants.kBound1Y) ? 1 : 0)
-  //           + ((yCoord > Constants.RobotStateConstants.kBound2Y) ? 1 : 0);
-  //   // CHECK VISION
-  //   double tempYaw = driveSubsystem.getGyroRotation2d().getDegrees();
-  //   if (getAllianceColor() == Alliance.Red)
-  //     tempYaw = driveSubsystem.getGyroRotation2d().getDegrees() - 180;
-  //   logger.info("grid Index: {}", gridIndex);
-  //   if (!visionSubsystem.lastUpdateWithinThresholdTime(0.05)) {
-  //     // && visionSubsystem.getHasTargets() == 0) {
-  //     logger.info("Threshold and no targets");
-  //     if (driveSubsystem.getPoseMeters().getY() <
+  // int gridIndex =
+  // ((yCoord > Constants.RobotStateConstants.kBound1Y) ? 1 : 0)
+  // + ((yCoord > Constants.RobotStateConstants.kBound2Y) ? 1 : 0);
+  // // CHECK VISION
+  // double tempYaw = driveSubsystem.getGyroRotation2d().getDegrees();
+  // if (getAllianceColor() == Alliance.Red)
+  // tempYaw = driveSubsystem.getGyroRotation2d().getDegrees() - 180;
+  // logger.info("grid Index: {}", gridIndex);
+  // if (!visionSubsystem.lastUpdateWithinThresholdTime(0.05)) {
+  // // && visionSubsystem.getHasTargets() == 0) {
+  // logger.info("Threshold and no targets");
+  // if (driveSubsystem.getPoseMeters().getY() <
   // Constants.RobotStateConstants.kGridY[gridIndex]) {
-  //       // left of tag on grid {gridIndex} GYRO POSITIVE IS COUNTERCLOCKWISE (Yaw/Look To The
-  //       // Right)
-  //       logger.info("Left Of Tag, Look Right(Yaw CounterClockwise), tempYaw: {}", tempYaw);
-  //       return 1;
-  //     } else {
-  //       // right of tag GYRO NEGATIVE IS CLOCKWISE (Yaw/Look To The LEFT)
-  //       logger.info("Right Of Tag, Look Left(Yaw Clockwise), tempYaw: {}", tempYaw);
-  //       return 2;
-  //     }
-  //   }
-  //   logger.info(
-  //       "Returned 0, tempYaw: {}, withThresholdVisUpdate: {}",
-  //       tempYaw,
-  //       visionSubsystem.lastUpdateWithinThresholdTime(
-  //           Constants.VisionConstants.kLastUpdateCloseEnoughThresholdYaw));
-  //   return 0;
+  // // left of tag on grid {gridIndex} GYRO POSITIVE IS COUNTERCLOCKWISE
+  // (Yaw/Look To The
+  // // Right)
+  // logger.info("Left Of Tag, Look Right(Yaw CounterClockwise), tempYaw: {}",
+  // tempYaw);
+  // return 1;
+  // } else {
+  // // right of tag GYRO NEGATIVE IS CLOCKWISE (Yaw/Look To The LEFT)
+  // logger.info("Right Of Tag, Look Left(Yaw Clockwise), tempYaw: {}", tempYaw);
+  // return 2;
+  // }
+  // }
+  // logger.info(
+  // "Returned 0, tempYaw: {}, withThresholdVisUpdate: {}",
+  // tempYaw,
+  // visionSubsystem.lastUpdateWithinThresholdTime(
+  // Constants.VisionConstants.kLastUpdateCloseEnoughThresholdYaw));
+  // return 0;
   // }
 
   public boolean isBlueAlliance() {
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
diff --git a/src/main/java/frc/robot/subsystems/WallEyeVisionSubsystem.java b/src/main/java/frc/robot/subsystems/WallEyeVisionSubsystem.java
