diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ec01c5d..236b237 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -340,9 +340,11 @@ public final class Constants { } public static final class VisionConstants { - public static final String CAMERA_NAME = "Camera_Module_v1"; + public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; + public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b84bf62..9db5009 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -75,7 +75,7 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); - public final Vision m_vision = new Vision(m_robotMap.camera); + public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a5a630a..d996288 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -35,7 +35,8 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME); + public PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + public PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); public RobotMap() { configureDriveMotorControllers(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index aeed160..a66a00a 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -39,7 +39,11 @@ import frc4388.utility.Subsystem; public class Vision extends Subsystem { - private PhotonCamera camera; + // private PhotonCamera leftCamera; + // private PhotonCamera rightCamera; + + private PhotonCamera[] cameras; + private PhotonPoseEstimator[] estimators; private boolean isTagDetected = false; private boolean isTagProcessed = false; @@ -47,7 +51,6 @@ public class Vision extends Subsystem { private Pose2d lastPhysOdomPose = new Pose2d(); private Matrix curStdDevs; - private final PhotonPoseEstimator photonEstimator; private Field2d field = new Field2d(); @@ -66,59 +69,87 @@ public class Vision extends Subsystem { .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); - GenericEntry sbCamConnected = subsystemLayout - .add("Camera Connnected", false) + GenericEntry sbLeftCamConnected = subsystemLayout + .add("Left Camera Connnected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + + GenericEntry sbRightCamConnected = subsystemLayout + .add("Right Camera Connnected", false) .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); - public Vision(PhotonCamera camera){ - this.camera = camera; + public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ SmartDashboard.putData(field); - photonEstimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); - photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; + + PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS); + PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS); + + photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; } @Override public void periodic() { - // var result = camera.getLatestResult(); - var results = camera.getAllUnreadResults(); - if (results.size() == 0) return; - var result = results.get(results.size()-1); - - isTagDetected = result.hasTargets(); - isTagProcessed = false; - - - if(!isTagDetected){ - // sbTagDetected.setBoolean(isTagDetected); - field.setRobotPose(getPose2d()); - return; - } - - var EstimatedRobotPose = getEstimatedGlobalPose(result); - - // In case the pose estimator fails to estimate the pose, fallback to physical odometry. - if(EstimatedRobotPose.isEmpty()){ - field.setRobotPose(getPose2d()); - return; - } - - isTagProcessed = true; - - lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); - // lastVisionPose.rotateBy(Rotation2d.k180deg); - // lastVisionPose = new Pose2d( - // lastVisionPose.getTranslation(), - // lastPhysOdomPose.getRotation() - // ); - - - + update(); field.setRobotPose(getPose2d()); } + private void update() { + isTagProcessed = false; + isTagDetected = false; + + + int cams = 0; + + double X = 0; + double Y = 0; + double Yaw = 0; + + for(int i = 0; i < cameras.length; i++){ + PhotonCamera camera = cameras[i]; + PhotonPoseEstimator estimator = estimators[i]; + + var results = camera.getAllUnreadResults(); + + // If there are no more updates from the camera + if (results.size() == 0) + continue; + + + var result = results.get(results.size()-1); + + isTagDetected = isTagDetected | result.hasTargets(); + + // If there are no tags + if(!result.hasTargets()) + continue; + + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); + + // If the tag was failed to be processed + if(estimatedRobotPose.isEmpty()) + continue; + + Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d(); + X += pose.getX(); + Y += pose.getY(); + Yaw += pose.getRotation().getDegrees(); + cams++; + isTagProcessed = true; + + + } + + if(isTagProcessed){ + lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams)); + } + } /** @@ -131,36 +162,24 @@ public class Vision extends Subsystem { * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets * used for estimation. */ - public Optional getEstimatedGlobalPose(PhotonPipelineResult change) { + public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { Optional visionEst = Optional.empty(); - // for (var change : camera.getAllUnreadResults()) { - var targets = change.getTargets(); - for(int i = targets.size()-1; i >= 0; i--){ - Transform3d pos = targets.get(i).getBestCameraToTarget(); - double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); - if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { - change.targets.remove(i); - } + var targets = change.getTargets(); + for(int i = targets.size()-1; i >= 0; i--){ + Transform3d pos = targets.get(i).getBestCameraToTarget(); + double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); + if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { + change.targets.remove(i); } + } - if(targets.size() <= 0) - return visionEst; // Will be empty + if(targets.size() <= 0) + return visionEst; // Will be empty - visionEst = photonEstimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets()); + visionEst = estimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets(), estimator); - // if (Robot.isSimulation()) { - // visionEst.ifPresentOrElse( - // est -> - // getSimDebugField() - // .getObject("VisionEstimation") - // .setPose(est.estimatedPose.toPose2d()), - // () -> { - // getSimDebugField().getObject("VisionEstimation").setPoses(); - // }); - // } - // } return visionEst; } @@ -173,7 +192,9 @@ public class Vision extends Subsystem { * @param targets All targets in this camera frame */ private void updateEstimationStdDevs( - Optional estimatedPose, List targets) { + Optional estimatedPose, + List targets, + PhotonPoseEstimator estimator) { if (estimatedPose.isEmpty()) { // No pose input. Default to single-tag std devs curStdDevs = VisionConstants.kSingleTagStdDevs; @@ -186,7 +207,7 @@ public class Vision extends Subsystem { // Precalculation - see how many tags we found, and calculate an average-distance metric for (var tgt : targets) { - var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); if (tagPose.isEmpty()) continue; double distance = tagPose @@ -281,7 +302,8 @@ public class Vision extends Subsystem { public void queryStatus() { sbTagDetected.setBoolean(isTagDetected); sbTagProcessed.setBoolean(isTagProcessed); - sbCamConnected.setBoolean(camera.isConnected()); + sbLeftCamConnected.setBoolean(cameras[0].isConnected()); + sbRightCamConnected.setBoolean(cameras[1].isConnected()); // field.setRobotPose(getPose2d()); }