From 337595a6c8b899abe5f6240a3f9ca7b745b3b822 Mon Sep 17 00:00:00 2001 From: TreeWonTwoToFor Date: Fri, 9 Feb 2024 18:56:17 -0800 Subject: [PATCH 1/3] basically a main and vision merge --- src/main/java/frc3512/robot/Constants.java | 6 ++ .../robot/subsystems/Superstructure.java | 2 + .../java/frc3512/robot/subsystems/Swerve.java | 21 ++++++- .../java/frc3512/robot/subsystems/Vision.java | 47 +++++++++++++++ vendordeps/photonlib.json | 57 +++++++++++++++++++ 5 files changed, 132 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc3512/robot/subsystems/Vision.java create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc3512/robot/Constants.java b/src/main/java/frc3512/robot/Constants.java index 322ad17..e81f8dd 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 = "photonvision"; + 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 2eb592d..3ff4e8a 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 = @@ -32,6 +33,7 @@ public Superstructure() { public void configureBindings() { driverXbox.x().onTrue(new InstantCommand(() -> swerve.zeroGyro())); + driverXbox.leftBumper().whileTrue(swerve.aimAtTarget(vision.returnCamera(vision))); } public void configureAxisActions() { diff --git a/src/main/java/frc3512/robot/subsystems/Swerve.java b/src/main/java/frc3512/robot/subsystems/Swerve.java index ffa9f76..d4f85fa 100644 --- a/src/main/java/frc3512/robot/subsystems/Swerve.java +++ b/src/main/java/frc3512/robot/subsystems/Swerve.java @@ -14,6 +14,8 @@ import frc3512.robot.Constants; import java.io.File; import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.parser.SwerveDriveConfiguration; @@ -55,6 +57,23 @@ public Swerve() { swerve.setHeadingCorrection(false); } + public Command aimAtTarget(PhotonCamera camera) { + return run( + () -> { + PhotonPipelineResult result = camera.getLatestResult(); + if (result.hasTargets()) { + drive( + getTargetSpeeds( + 0, + 0, + Rotation2d.fromDegrees( + result + .getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } + }); + } + /** * Command to drive the robot using translative values and heading as angular velocity. * @@ -170,7 +189,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 From 4742507f454253b04fefe1a08af7c7bd64caee38 Mon Sep 17 00:00:00 2001 From: TreeWonTwoToFor Date: Fri, 9 Feb 2024 20:09:14 -0800 Subject: [PATCH 2/3] added vision control over rotation based on target --- src/main/java/frc3512/robot/Constants.java | 2 +- .../robot/subsystems/Superstructure.java | 6 +- .../java/frc3512/robot/subsystems/Swerve.java | 68 ++++++++++--------- 3 files changed, 41 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc3512/robot/Constants.java b/src/main/java/frc3512/robot/Constants.java index e81f8dd..d305e7c 100644 --- a/src/main/java/frc3512/robot/Constants.java +++ b/src/main/java/frc3512/robot/Constants.java @@ -30,7 +30,7 @@ public static final class GeneralConstants { } public static final class VisionConstants { - public static final String cameraName = "photonvision"; + public static final String cameraName = "jonny"; 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 3ff4e8a..04f496b 100644 --- a/src/main/java/frc3512/robot/subsystems/Superstructure.java +++ b/src/main/java/frc3512/robot/subsystems/Superstructure.java @@ -33,7 +33,6 @@ public Superstructure() { public void configureBindings() { driverXbox.x().onTrue(new InstantCommand(() -> swerve.zeroGyro())); - driverXbox.leftBumper().whileTrue(swerve.aimAtTarget(vision.returnCamera(vision))); } public void configureAxisActions() { @@ -48,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 d4f85fa..93f0fea 100644 --- a/src/main/java/frc3512/robot/subsystems/Swerve.java +++ b/src/main/java/frc3512/robot/subsystems/Swerve.java @@ -8,11 +8,13 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; 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; @@ -57,23 +59,6 @@ public Swerve() { swerve.setHeadingCorrection(false); } - public Command aimAtTarget(PhotonCamera camera) { - return run( - () -> { - PhotonPipelineResult result = camera.getLatestResult(); - if (result.hasTargets()) { - drive( - getTargetSpeeds( - 0, - 0, - Rotation2d.fromDegrees( - result - .getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. - } - }); - } - /** * Command to drive the robot using translative values and heading as angular velocity. * @@ -83,22 +68,43 @@ public Command aimAtTarget(PhotonCamera camera) { * @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); + SmartDashboard.putBoolean("Diagnostics/Vision/DoVisionAim", doAim.getAsBoolean()); + PhotonPipelineResult result = camera.getLatestResult(); + if (result.hasTargets() && doAim.getAsBoolean()) { + drive( + getTargetSpeeds( + MathUtil.applyDeadband( + translationX.getAsDouble() * swerve.getMaximumVelocity(), + Constants.SwerveConstants.swerveDeadband), + MathUtil.applyDeadband( + translationY.getAsDouble() * swerve.getMaximumVelocity(), + Constants.SwerveConstants.swerveDeadband), + Rotation2d.fromDegrees( + result + .getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } 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); + } }); } From d8ac87b107fcaec93931fef8884fe3a03fcafb39 Mon Sep 17 00:00:00 2001 From: TreeWonTwoToFor Date: Sat, 10 Feb 2024 14:43:55 -0800 Subject: [PATCH 3/3] Fully automatic aiming system at target! --- src/main/deploy/swerve/comp/swervedrive.json | 2 +- src/main/java/frc3512/robot/Constants.java | 2 +- src/main/java/frc3512/robot/subsystems/Swerve.java | 13 ++++++------- 3 files changed, 8 insertions(+), 9 deletions(-) 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 d305e7c..f5260f3 100644 --- a/src/main/java/frc3512/robot/Constants.java +++ b/src/main/java/frc3512/robot/Constants.java @@ -30,7 +30,7 @@ public static final class GeneralConstants { } public static final class VisionConstants { - public static final String cameraName = "jonny"; + public static final String cameraName = "USB_GS_Camera"; public static final Transform3d robotToCam = new Transform3d(); } } diff --git a/src/main/java/frc3512/robot/subsystems/Swerve.java b/src/main/java/frc3512/robot/subsystems/Swerve.java index 93f0fea..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; @@ -8,7 +9,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc3512.lib.logging.SpartanEntryManager; @@ -75,21 +75,20 @@ public Command driveCommand( PhotonCamera camera) { return run( () -> { - SmartDashboard.putBoolean("Diagnostics/Vision/DoVisionAim", doAim.getAsBoolean()); 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( - getTargetSpeeds( + swerve.swerveController.getRawTargetSpeeds( MathUtil.applyDeadband( translationX.getAsDouble() * swerve.getMaximumVelocity(), Constants.SwerveConstants.swerveDeadband), MathUtil.applyDeadband( translationY.getAsDouble() * swerve.getMaximumVelocity(), Constants.SwerveConstants.swerveDeadband), - Rotation2d.fromDegrees( - result - .getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. + -turnController.calculate(result.getBestTarget().getYaw(), 0))); } else { swerve.drive( new Translation2d(