Feature/vision testing bugfixes

src/main/java/competition/electrical_contract/CompetitionContract.java

@@ -120,6 +120,10 @@ public DeviceInfo getShooterMotorFollower() {
     public boolean isCollectorReady() {
         return true;
     }
+
+    public boolean isScoocherReady() {
+        return false;
+    }
     public DeviceInfo getScoocherMotor(){
         return new DeviceInfo("ScoocherMotor", 14);
     }

src/main/java/competition/electrical_contract/ElectricalContract.java

@@ -22,6 +22,8 @@ public abstract class ElectricalContract implements XSwerveDriveElectricalContra
     public abstract XYPair getSwerveModuleOffsets(SwerveInstance swerveInstance);
 
     public abstract boolean isCollectorReady();
+
+    public abstract boolean isScoocherReady();
     public abstract DeviceInfo getScoocherMotor();
 
     public abstract DeviceInfo getLightsDio0();

src/main/java/competition/subsystems/oracle/DynamicOracle.java

@@ -62,7 +62,7 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri
         Pose2d scoringPositionMiddle = new Pose2d(1.5, 5.5, Rotation2d.fromDegrees(0));
         Pose2d scoringPositionBottom = new Pose2d(0.9, 4.3, Rotation2d.fromDegrees(0));
 
-        activeScoringPosition = scoringPositionTop;
+        activeScoringPosition = scoringPositionMiddle;
         //createRobotObstacle(scoringPositionMiddle.getTranslation(), 1.75, "PartnerA");
         //createRobotObstacle(scoringPositionBottom.getTranslation(), 1.75, "PartnerB");
     }

src/main/java/competition/subsystems/pose/PoseSubsystem.java

@@ -55,7 +55,7 @@ public class PoseSubsystem extends BasePoseSubsystem {
 
     public static final  Translation2d SPEAKER_POSITION = new Translation2d(-0.0381,5.547868);
     public static Pose2d SpikeTop = new Pose2d(2.8956, 7.0012, new Rotation2d());
-    public static Pose2d SpikeMiddle = new Pose2d(2.8956, 5.5534, new Rotation2d());
+    public static Pose2d SpikeMiddle = new Pose2d(2.8956, 5.5478, new Rotation2d());
     public static Pose2d SpikeBottom = new Pose2d(2.8956, 4.1056, new Rotation2d());
     public static Pose2d CenterLine1 = new Pose2d(8.2956, 7.4584, new Rotation2d());
     public static Pose2d CenterLine2 = new Pose2d(8.2956, 5.7820, new Rotation2d());

src/main/java/competition/subsystems/schoocher/ScoocherSubsystem.java

@@ -15,32 +15,42 @@ public class ScoocherSubsystem extends BaseSubsystem implements DataFrameRefresh
 
     public final ElectricalContract contract;
     public DoubleProperty sendingPower;
-    public final XCANSparkMax scoocherMotor;
+    public XCANSparkMax scoocherMotor;
 
     @Inject
     public ScoocherSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory,
                              ElectricalContract electricalContract, XDigitalInput.XDigitalInputFactory xDigitalInputFactory) {
         this.contract = electricalContract;
-        this.scoocherMotor = sparkMaxFactory.createWithoutProperties(contract.getScoocherMotor(), getPrefix(), "Scoocher");
 
+        if (contract.isScoocherReady()) {
+            this.scoocherMotor = sparkMaxFactory.createWithoutProperties(contract.getScoocherMotor(), getPrefix(), "Scoocher");
+        }
         pf.setPrefix(this);
         sendingPower = pf.createPersistentProperty("sendingPower", 0.1);
     }
 
+    private void setPower(double power) {
+        if (contract.isScoocherReady()) {
+            scoocherMotor.set(power);
+        }
+    }
+
     public void intakeNote() {
-        scoocherMotor.set(sendingPower.get());
+        setPower(sendingPower.get());
     }
     public void ejectNote(){
-        scoocherMotor.set(-sendingPower.get());
+        setPower(-sendingPower.get());
     }
 
     public void stop() {
-        scoocherMotor.set(0);
+        setPower(0);
     }
 
     @Override
     public void refreshDataFrame() {
-        scoocherMotor.refreshDataFrame();
+        if (contract.isScoocherReady()) {
+            scoocherMotor.refreshDataFrame();
+        }
     }
 }
 

src/main/java/competition/subsystems/vision/VisionSubsystem.java

@@ -52,6 +52,8 @@ public class VisionSubsystem extends BaseSubsystem implements DataFrameRefreshab
     final DoubleProperty yawOffset;
     final DoubleProperty waitForStablePoseTime;
     final DoubleProperty errorThreshold;
+    final DoubleProperty singleTagStableDistance;
+    final DoubleProperty multiTagStableDistance;
     final TimeStableValidator frontReliablePoseIsStable;
     final TimeStableValidator rearReliablePoseIsStable;
     NetworkTable visionTable;
@@ -70,6 +72,9 @@ public VisionSubsystem(PropertyFactory pf, RobotAssertionManager assertionManage
         pf.setPrefix(this);
         isInverted = pf.createPersistentProperty("Yaw inverted", true);
         yawOffset = pf.createPersistentProperty("Yaw offset", 0);
+        singleTagStableDistance = pf.createPersistentProperty("Single tag stable distance", 2.0);
+        multiTagStableDistance = pf.createPersistentProperty("Multi tag stable distance", 4.0);
+
 
         waitForStablePoseTime = pf.createPersistentProperty("Pose stable time", 0.0, Property.PropertyLevel.Debug);
         errorThreshold = pf.createPersistentProperty("Error threshold",200);
@@ -152,6 +157,7 @@ public Optional<EstimatedRobotPose> getPhotonVisionEstimatedPose(
             }
 
             var isReliable = !estimatedPose.isEmpty() && isEstimatedPoseReliable(estimatedPose.get(), previousEstimatedRobotPose);
+            aKitLog.record(name+"Reliable", isReliable);
             var isStable = waitForStablePoseTime.get() == 0.0 || poseTimeValidator.checkStable(isReliable);
             if (isReliable && isStable) {
                 return estimatedPose;
@@ -201,14 +207,19 @@ public boolean isEstimatedPoseReliable(EstimatedRobotPose estimatedPose, Pose2d
             //        estimatedPose.estimatedPose.toPose2d(), getStringFromList(allTagIds), previousEstimatedPose));
         }
 
-        // Two or more targets tends to be very reliable
-        if (estimatedPose.targetsUsed.size() > 1) {
+        // How far away is the camera from the target?
+        double cameraDistance =
+                estimatedPose.targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm();
+
+        // Two or more targets tends to be very reliable, but there's still a limit for distance
+        if (estimatedPose.targetsUsed.size() > 1
+        && cameraDistance < multiTagStableDistance.get()) {
             return true;
         }
 
         // For a single target we need to be above reliability threshold and within 1m
         return estimatedPose.targetsUsed.get(0).getPoseAmbiguity() < 0.20
-                && estimatedPose.targetsUsed.get(0).getBestCameraToTarget().getTranslation().getX() < 1.5;
+                && cameraDistance < singleTagStableDistance.get();
     }
 
     private List<Integer> getTagListFromPose(EstimatedRobotPose estimatedPose) {

vendordeps/photonlib.json

@@ -1,7 +1,7 @@
 {
   "fileName": "photonlib.json",
   "name": "photonlib",
-  "version": "v2024.2.0",
+  "version": "v2024.2.2",
   "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
   "frcYear": "2024",
   "mavenUrls": [
@@ -14,7 +14,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-cpp",
-      "version": "v2024.2.0",
+      "version": "v2024.2.2",
       "libName": "photonlib",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -29,7 +29,7 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-cpp",
-      "version": "v2024.2.0",
+      "version": "v2024.2.2",
       "libName": "photontargeting",
       "headerClassifier": "headers",
       "sharedLibrary": true,
@@ -46,12 +46,12 @@
     {
       "groupId": "org.photonvision",
       "artifactId": "photonlib-java",
-      "version": "v2024.2.0"
+      "version": "v2024.2.2"
     },
     {
       "groupId": "org.photonvision",
       "artifactId": "photontargeting-java",
-      "version": "v2024.2.0"
+      "version": "v2024.2.2"
     }
   ]
 }