diff --git a/simgui-ds.json b/simgui-ds.json index 49cf3aa..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000" + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3bfe1d4..61cbf07 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -27,6 +27,15 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; @@ -267,6 +276,16 @@ public final class Constants { } public static final class VisionConstants { + public static final String CAMERA_NAME = "photonvision"; + + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(), new Rotation3d()); + + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + + // The standard deviations of our vision estimated poses, which affect correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } public static final class DriveConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 63f9eec..3be1700 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,7 @@ import frc4388.robot.commands.Swerve.neoJoystickRecorder; // Subsystems // import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.SwerveDrive; // Utilites @@ -53,8 +54,10 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); + public final Vision m_vision = new Vision(m_robotMap.camera); + + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4990efa..3541a58 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,6 +8,9 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; + +import org.photonvision.PhotonCamera; + import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain; @@ -18,6 +21,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.VisionConstants; // import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; @@ -28,6 +32,8 @@ import frc4388.utility.RobotGyro; 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 RobotMap() { configureDriveMotorControllers(); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java new file mode 100644 index 0000000..070fc21 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -0,0 +1,43 @@ +package frc4388.robot.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.VirtualController; + +public class GotoPositionCommand extends Command { + private boolean isFinished = false; + + SwerveDrive swerveDrive; + + /** + * Command to drive robot to position. + * @param SwerveDrive m_robotSwerveDrive + */ + public GotoPositionCommand(SwerveDrive swerveDrive) { + this.swerveDrive = swerveDrive; + } + + @Override + public void initialize() { + isFinished = false; + } + + @Override + public void execute() { + + + } + + @Override + public void end(boolean interrupted) { + + } + + @Override + public boolean isFinished() { + return isFinished; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index c685f23..09b3bb5 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -33,6 +33,8 @@ import frc4388.utility.Status.ReportLevel; public class SwerveDrive extends Subsystem { private SwerveDrivetrain swerveDriveTrain; + + private Vision vision; private int gear_index = SwerveDriveConstants.STARTING_GEAR; private boolean stopped = false; @@ -45,16 +47,14 @@ public class SwerveDrive extends Subsystem { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - private Field2d field = new Field2d(); - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { + public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { super(); - SmartDashboard.putData(field); - this.swerveDriveTrain = swerveDriveTrain; + this.vision = vision; } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); // // rightStick.getAngle() @@ -172,10 +172,9 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("RotTartget", rotTarget); - Optional e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()); + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(Vision.getTime())); - if(e.isPresent()) - field.setRobotPose(e.get()); + // if(e.isPresent()) } private void reset_index() { diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..899dba0 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,242 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; + +public class Vision extends Subsystem { + + private PhotonCamera camera; + + private boolean isTag = false; + private Pose2d lastVisionPose = new Pose2d(); + private Pose2d lastPhysOdomPose = new Pose2d(); + + private Matrix curStdDevs; + private final PhotonPoseEstimator photonEstimator; + + private Field2d field = new Field2d(); + + public Vision(PhotonCamera camera){ + this.camera = camera; + SmartDashboard.putData(field); + + photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + @Override + public void periodic() { + var result = camera.getLatestResult(); + isTag = result.hasTargets(); + + if(!isTag){ + field.setRobotPose(lastPhysOdomPose); + return; + } + + var EstimatedRobotPose = getEstimatedGlobalPose(); + + // In case the pose estimator fails to estimate the pose, fallback to physical odometry. + if(EstimatedRobotPose.isEmpty()){ + field.setRobotPose(lastPhysOdomPose); + isTag = false; + return; + } + + field.setRobotPose(lastVisionPose); + } + + + + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + for (var change : camera.getAllUnreadResults()) { + visionEst = photonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + + // if (Robot.isSimulation()) { + // visionEst.ifPresentOrElse( + // est -> + // getSimDebugField() + // .getObject("VisionEstimation") + // .setPose(est.estimatedPose.toPose2d()), + // () -> { + // getSimDebugField().getObject("VisionEstimation").setPoses(); + // }); + // } + } + return visionEst; + } + + + + + + + + /** + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + * deviations based on number of tags, estimation strategy, and distance from the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = VisionConstants.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)); + curStdDevs = estStdDevs; + } + } + } + + /** + * Returns the latest 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. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; + } + + + + + + + + + + public void setLastOdomPose(Optional pose){ + if(pose.isPresent()) + lastPhysOdomPose = pose.get(); + } + + public Pose2d getPose2d() { + if(isTag) + return lastVisionPose; + else + return lastPhysOdomPose; + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + + + + + + + + + + @Override + public String getSubsystemName() { + return "Vision"; + } + + + ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + GenericEntry sbTag = subsystemLayout + .add("Tag Detected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + +// GenericEntry sbShiftState = subsystemLayout +// .add("Shift State", 0) +// .withWidget(BuiltInWidgets.kNumberBar) +// .getEntry(); + + + @Override + public void queryStatus() { + sbTag.setBoolean(isTag); + field.setRobotPose(getPose2d()); + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + return status; + } + +} diff --git a/src/main/java/frc4388/utility/CanDevice.java b/src/main/java/frc4388/utility/CanDevice.java index ec05fdb..1805403 100644 --- a/src/main/java/frc4388/utility/CanDevice.java +++ b/src/main/java/frc4388/utility/CanDevice.java @@ -35,13 +35,13 @@ public class CanDevice { System.out.println(getName() + " - " + str); } - public Status queryStatus() { - Status s = new Status(); + // public Status queryStatus() { + // Status s = new Status(); - s.addReport(ReportLevel.INFO, "TODO"); + // s.addReport(ReportLevel.INFO, "TODO"); - return s; - } + // return s; + // } public Status diagnosticStatus() { Status s = new Status(); diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.1.1.json new file mode 100644 index 0000000..6322388 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.1.1.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.1.1.json", + "name": "PathplannerLib", + "version": "2025.1.1", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.1.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.1.1", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..db43d6d --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-8", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-8" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-8" + } + ] +} \ No newline at end of file