fixed vision

Author: Swordman51

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

@@ -482,7 +482,7 @@ public Command prepShooter() {
 
   public Command shootSpeaker() {
     return (
-      rotateArmSpeaker().deadlineWith(prepShooter())
+      rotateArmSpeaker().deadlineWith(shooter.runVoltage(0))
         .andThen(shootNote().deadlineWith(rotateArmSpeaker().repeatedly())));
   }
 
@@ -529,10 +529,10 @@ public Command shootNote() {
     .alongWith(shooter.runVoltage(ShooterConstants.SHOOTER_UNJAM_VOLTAGE))).withTimeout(0.1).andThen(shooter.runVoltageBoth(rightShooterVolts::get, leftShooterVolts::get) // run shooter full speed
         .alongWith(
             new SequentialCommandGroup(
-              indexer.manualCommand(IndexerConstants.INDEXER_OUT_VOLTAGE / 10).withTimeout(0.6), // run intake back for 0.1 seconds
-              indexer.manualCommand(IndexerConstants.INDEXER_IN_VOLTAGE) // run intake in to shoot
+              indexer.manualCommand(IndexerConstants.INDEXER_OUT_VOLTAGE / 100).withTimeout(1), // run intake back for 0.1 seconds
+              /*new WaitCommand(0.8).andThen */(indexer.manualCommand(IndexerConstants.INDEXER_IN_VOLTAGE)) // run intake in to shoot
             ))
-        .withTimeout(1.5));//.alongWith(new InstantCommand(() -> {NoteVisualizer.shoot().schedule();})); // run the visualizer
+        .withTimeout(1.9));//.alongWith(new InstantCommand(() -> {NoteVisualizer.shoot().schedule();})); // run the visualizer
   }
 
   /* // Brings the note forward and back for 0.5 seconds each to center it

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -37,6 +37,7 @@
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -46,6 +47,8 @@
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -54,6 +57,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
+import frc.robot.subsystems.vision.VisionConstants;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOInputsAutoLogged;
 import frc.robot.util.autonomous.DeadzoneChooser;
@@ -186,8 +190,15 @@ public void periodic() {
       visionIO.updateInputs(visionInputs, getPose());
       Logger.processInputs("Vision", visionInputs);
       if (visionInputs.hasEstimate) {
+        List<Matrix<N3, N1>> stdDeviations = visionIO.getStdArray(getPose());
+
         for (int i = 0; i < visionInputs.estimate.length; i++) {
-          poseEstimator.addVisionMeasurement(visionInputs.estimate[i], Timer.getFPGATimestamp());
+          if (stdDeviations.size() <= i) {
+            poseEstimator.addVisionMeasurement(visionInputs.estimate[i], Timer.getFPGATimestamp(), VisionConstants.kSingleTagStdDevs);
+          } else {
+            poseEstimator.addVisionMeasurement(visionInputs.estimate[i], Timer.getFPGATimestamp(), stdDeviations.get(i));
+          }
+          
         }
       }
     }

src/main/java/frc/robot/subsystems/groundIntake/GroundIntakeConstants.java

@@ -22,7 +22,7 @@ public static class GroundIntakePhysicalConstants {
       public static final double kGroundIntakeD = 0.0;
     }
 
-    public static final double GROUND_INTAKE_IN_VOLTAGE = 9;
+    public static final double GROUND_INTAKE_IN_VOLTAGE = 9.2;
     public static final double GROUND_INTAKE_WEAK_IN_VOLTAGE = 4.5;
     public static final double GROUND_INTAKE_OUT_VOLTAGE = -9;
   }

src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java

@@ -38,7 +38,7 @@ public static class IndexerPhysicalConstants {
   }
 
   public static final double INDEXER_IN_VOLTAGE = 8;
-  public static final double INDEXER_IN_VOLTAGE_WEAK = 3.95;
+  public static final double INDEXER_IN_VOLTAGE_WEAK = 4.2;
   public static final double INDEXER_OUT_VOLTAGE = -2;
 
   public static final double SHOOTER_UNJAM_TIME = 0.2;

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

@@ -133,6 +133,35 @@ public default Pose2d[] getEstimatesArray(PhotonPipelineResult[] results, Photon
     return finalEstimates;
   }
 
+  public default List<Matrix<N3, N1>> getStdArray(Pose2d currentPose) {
+    PhotonPipelineResult[] results = getResults();
+    PhotonPoseEstimator[] photonEstimator = getEstimators();
+    PhotonCamera[] cams = getCameras();
+
+    List<Matrix<N3, N1>> stdsArray = new ArrayList<Matrix<N3, N1>>();
+    Optional<Pose2d>[] estimates = getEstimates(results, photonEstimator);
+
+    for (int i = 0; i < estimates.length; i++) {
+      if (estimates[i].isPresent() && estimates[i].get() != null) {
+        stdsArray.add(getEstimationStdDevs(currentPose, cams[i], photonEstimator[i]));
+      }
+    }
+
+    return stdsArray;
+  }
+
+  public default PhotonPipelineResult[] getResults() {
+    return null;
+  }
+
+  public default PhotonPoseEstimator[] getEstimators() {
+    return null;
+  }
+
+  public default PhotonCamera[] getCameras() {
+    return null;
+  }
+
   public default double estimateLatestTimestamp(PhotonPipelineResult[] results) {
     double latestTimestamp = 0;
     int count = 0;
@@ -218,6 +247,37 @@ public default Matrix<N3, N1> getEstimationStdDevs(Pose2d estimatedPose, PhotonP
     return estStdDevs;
   }
 
+  /**
+   * The standard deviations of the estimated pose from {@link #getEstimatedGlobalPose()}, for use
+   * with {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}.
+   * This should only be used when there are targets visible.
+   *
+   * @param estimatedPose The estimated pose to guess standard deviations for.
+   */
+  public default Matrix<N3, N1> getEstimationStdDevs(Pose2d estimatedPose, PhotonCamera camera, PhotonPoseEstimator orangeEstimator) {
+    var estStdDevs = kSingleTagStdDevs;
+    var targets = getLatestResult(camera).getTargets();
+    int numTags = 0;
+    double avgDist = 0;
+    for (var tgt : targets) {
+        var tagPose = orangeEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+        if (tagPose.isEmpty()) continue;
+        numTags++;
+        avgDist +=
+                tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation());
+    }
+    if (numTags == 0) return estStdDevs;
+    avgDist /= numTags;
+    // Decrease std devs if multiple targets are visible
+    if (numTags > 1) estStdDevs = kMultiTagStdDevs;
+    // Increase std devs based on (average) distance
+    if (numTags == 1 && avgDist > 4)
+        estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+
+    return estStdDevs;
+  }
+
   // Override this
   public default Matrix<N3, N1> getEstimationStdDevs(Pose2d estimatedPose) {
     return kSingleTagStdDevs;

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

@@ -131,4 +131,15 @@ public boolean goodResult(PhotonPipelineResult result) {
                                                                                                       */;
     }
 
+    public PhotonPipelineResult[] getResults() {
+        return getAprilTagResults();
+      }
+    
+    public PhotonPoseEstimator[] getEstimators() {
+        return new PhotonPoseEstimator[] { camera1Estimator, camera2Estimator, camera3Estimator };
+    }
+
+    public PhotonCamera[] getCameras() {
+        return new PhotonCamera[] { camera1, camera2, camera3 };
+    }
 }

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

@@ -132,5 +132,22 @@ public boolean goodResult(PhotonPipelineResult result) {
                 && kTagLayout.getTagPose(result.getBestTarget().getFiducialId()).get().toPose2d().getTranslation()
                         .getDistance(lastEstimate.getTranslation()) < MAX_DISTANCE;
     }
+
+    public PhotonPipelineResult[] getResults() {
+        PhotonPipelineResult front_result = getLatestResult(cam1);
+        PhotonPipelineResult back_result = getLatestResult(cam2);
+        PhotonPipelineResult front_result2 = getLatestResult(cam3);
+        
+        PhotonPipelineResult[] results = { front_result, back_result, front_result2 };
+        return results;
+      }
+    
+    public PhotonPoseEstimator[] getEstimators() {
+        return new PhotonPoseEstimator[] { cam1Estimator, cam2Estimator, cam3Estimator };
+    }
+
+    public PhotonCamera[] getCameras() {
+        return new PhotonCamera[] { cam1, cam2, cam3 };
+    }
 
 }

src/main/java/frc/robot/util/drive/DriveControls.java

@@ -118,17 +118,17 @@ public static void configureControls() {
                 PIVOT_ROTATE = () -> (operator.getRightTriggerAxis() - operator.getLeftTriggerAxis());
 
                 // Pivot things
-                PIVOT_AMP = operator.getDPad(DPad.RIGHT);
-                PIVOT_ZERO = operator.getDPad(DPad.DOWN);
-                PIVOT_TO_SPEAKER = operator.getDPad(DPad.LEFT);
+                PIVOT_AMP = operator.getB();
+                PIVOT_ZERO = operator.getA();
+                PIVOT_TO_SPEAKER = operator.getX();
                 PIVOT_PODIUM = EMPTY_TRIGGER;
-                PIVOT_ANYWHERE = operator.getDPad(DPad.UP); // uses vision
+                PIVOT_ANYWHERE = operator.getY(); // uses vision
 
                 // intaking things
                 INTAKE_ROTATE = () -> operator.getLeftYD();
                 INTAKE_IN = operator.rightBumper();
                 INTAKE_OUT = operator.leftBumper();
-                INTAKE_UNTIL_INTAKED = operator.getY();
+                INTAKE_UNTIL_INTAKED = operator.getDPad(DPad.UP);
 
                 // ground intake things
                 GROUND_INTAKE_ROTATE = () -> -2*operator.getLeftXD();
@@ -137,9 +137,9 @@ public static void configureControls() {
 
                 // Shooter things
                 SHOOTER_SPEED = () -> operator.getRightXD();
-                SHOOTER_FULL_SEND_INTAKE = operator.getX();
-                SHOOTER_FULL_SEND = operator.getA();
-                SHOOTER_UNJAM = operator.getB();
+                SHOOTER_FULL_SEND_INTAKE = operator.getDPad(DPad.LEFT);
+                SHOOTER_FULL_SEND = operator.getDPad(DPad.DOWN);
+                SHOOTER_UNJAM = operator.getDPad(DPad.RIGHT);
                 break;
             case PROGRAMMERS:
             default: