diff --git a/src/main/deploy/swerve/comp/swervedrive.json b/src/main/deploy/swerve/comp/swervedrive.json index 969b7a8..fab8811 100644 --- a/src/main/deploy/swerve/comp/swervedrive.json +++ b/src/main/deploy/swerve/comp/swervedrive.json @@ -4,7 +4,7 @@ "id": 0, "canbus": null }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc3512/robot/Constants.java b/src/main/java/frc3512/robot/Constants.java index 322ad17..f5260f3 100644 --- a/src/main/java/frc3512/robot/Constants.java +++ b/src/main/java/frc3512/robot/Constants.java @@ -1,5 +1,6 @@ package frc3512.robot; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; public final class Constants { @@ -27,4 +28,9 @@ public static final class GeneralConstants { public static final RobotType robotType = RobotType.COMP; } + + public static final class VisionConstants { + public static final String cameraName = "USB_GS_Camera"; + public static final Transform3d robotToCam = new Transform3d(); + } } diff --git a/src/main/java/frc3512/robot/subsystems/Superstructure.java b/src/main/java/frc3512/robot/subsystems/Superstructure.java index 7b4cbe6..8360c3b 100644 --- a/src/main/java/frc3512/robot/subsystems/Superstructure.java +++ b/src/main/java/frc3512/robot/subsystems/Superstructure.java @@ -16,6 +16,7 @@ public class Superstructure extends SubsystemBase { // Subsystems public final Swerve swerve = new Swerve(); + public final Vision vision = new Vision(); // Joysticks private final CommandXboxController driverXbox = @@ -46,8 +47,9 @@ public void configureAxisActions() { driverXbox.getRawAxis(strafeAxis), Constants.SwerveConstants.swerveDeadband), () -> MathUtil.applyDeadband( - driverXbox.getRawAxis(rotationAxis), - Constants.SwerveConstants.swerveDeadband))); + driverXbox.getRawAxis(rotationAxis), Constants.SwerveConstants.swerveDeadband), + () -> driverXbox.leftBumper().getAsBoolean(), + vision.returnCamera(vision))); } public void setMotorBrake(boolean brake) { diff --git a/src/main/java/frc3512/robot/subsystems/Swerve.java b/src/main/java/frc3512/robot/subsystems/Swerve.java index ffa9f76..4e6be68 100644 --- a/src/main/java/frc3512/robot/subsystems/Swerve.java +++ b/src/main/java/frc3512/robot/subsystems/Swerve.java @@ -1,6 +1,7 @@ package frc3512.robot.subsystems; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -13,7 +14,10 @@ import frc3512.lib.logging.SpartanEntryManager; import frc3512.robot.Constants; import java.io.File; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.parser.SwerveDriveConfiguration; @@ -64,22 +68,42 @@ public Swerve() { * @return Drive command. */ public Command driveCommand( - DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier angularRotationX, + BooleanSupplier doAim, + PhotonCamera camera) { return run( () -> { - swerve.drive( - new Translation2d( - MathUtil.applyDeadband( - translationX.getAsDouble() * swerve.getMaximumVelocity(), - Constants.SwerveConstants.swerveDeadband), - MathUtil.applyDeadband( - translationY.getAsDouble() * swerve.getMaximumVelocity(), - Constants.SwerveConstants.swerveDeadband)), - MathUtil.applyDeadband( - angularRotationX.getAsDouble() * swerve.getMaximumAngularVelocity(), - Constants.SwerveConstants.swerveDeadband), - true, - false); + PhotonPipelineResult result = camera.getLatestResult(); + final double ANGULAR_P = 0.1; + final double ANGULAR_D = 0.0; + PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D); + if (result.hasTargets() && doAim.getAsBoolean()) { + drive( + swerve.swerveController.getRawTargetSpeeds( + MathUtil.applyDeadband( + translationX.getAsDouble() * swerve.getMaximumVelocity(), + Constants.SwerveConstants.swerveDeadband), + MathUtil.applyDeadband( + translationY.getAsDouble() * swerve.getMaximumVelocity(), + Constants.SwerveConstants.swerveDeadband), + -turnController.calculate(result.getBestTarget().getYaw(), 0))); + } else { + swerve.drive( + new Translation2d( + MathUtil.applyDeadband( + translationX.getAsDouble() * swerve.getMaximumVelocity(), + Constants.SwerveConstants.swerveDeadband), + MathUtil.applyDeadband( + translationY.getAsDouble() * swerve.getMaximumVelocity(), + Constants.SwerveConstants.swerveDeadband)), + MathUtil.applyDeadband( + angularRotationX.getAsDouble() * swerve.getMaximumAngularVelocity(), + Constants.SwerveConstants.swerveDeadband), + true, + false); + } }); } @@ -170,7 +194,7 @@ public void setMotorBrake(boolean brake) { * @return The yaw angle */ public Rotation2d getHeading() { - return swerve.getOdometryHeading(); + return swerve.getYaw(); } /** diff --git a/src/main/java/frc3512/robot/subsystems/Vision.java b/src/main/java/frc3512/robot/subsystems/Vision.java new file mode 100644 index 0000000..2a20bb3 --- /dev/null +++ b/src/main/java/frc3512/robot/subsystems/Vision.java @@ -0,0 +1,47 @@ +package frc3512.robot.subsystems; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc3512.robot.Constants; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +public class Vision extends SubsystemBase { + public PhotonCamera photonCamera = new PhotonCamera(Constants.VisionConstants.cameraName); + public PhotonPoseEstimator photonPoseEstimator; + public AprilTagFieldLayout atfl; + + public Vision() { + PhotonCamera.setVersionCheckEnabled(false); + // photonCamera.setDriverMode(true); + + atfl = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + + // Create pose estimator + photonPoseEstimator = + new PhotonPoseEstimator( + atfl, + PoseStrategy.LOWEST_AMBIGUITY, + photonCamera, + Constants.VisionConstants.robotToCam); + } + + /** + * @param estimatedRobotPose The current best guess at robot pose + * @return A pair of the fused camera observations to a single Pose2d on the field, and the time + * of the observation. Assumes a planar field and the robot is always firmly on the ground + */ + public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { + photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + return photonPoseEstimator.update(); + } + + public PhotonCamera returnCamera(Vision vision) { + return vision.photonCamera; + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..46211fc --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.4", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "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": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.2.4", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.4", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.2.4" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.4" + } + ] +} \ No newline at end of file