From ed336a2c28202bde75df2819680926323fd67417 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 10 Sep 2024 10:51:49 +0300 Subject: [PATCH 001/261] I have no clue what I'm doing and I know there are errors --- .../AprilTagPhotonCameraIO.java | 104 +++++++++++------- .../robotposesources/RobotPoseSource.java | 2 +- .../RobotPoseSourceConstants.java | 2 +- .../robotposesources/RobotPoseSourceIO.java | 5 +- 4 files changed, 70 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index e144ea76..aee6b471 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -1,81 +1,76 @@ package frc.trigon.robot.poseestimation.robotposesources; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.trigon.robot.poseestimation.photonposeestimator.EstimatedRobotPose; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N3; import org.littletonrobotics.junction.Logger; +import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; import java.util.List; import java.util.Optional; public class AprilTagPhotonCameraIO extends RobotPoseSourceIO { private final PhotonCamera photonCamera; - private final PhotonPoseEstimator photonPoseEstimator; - protected AprilTagPhotonCameraIO(String cameraName, Transform3d robotCenterToCamera) { + protected AprilTagPhotonCameraIO(String cameraName) { photonCamera = new PhotonCamera(cameraName); - - photonPoseEstimator = new PhotonPoseEstimator( - RobotPoseSourceConstants.APRIL_TAG_FIELD_LAYOUT, - RobotPoseSourceConstants.PRIMARY_POSE_STRATEGY, - photonCamera, - robotCenterToCamera - ); - - photonPoseEstimator.setMultiTagFallbackStrategy(RobotPoseSourceConstants.SECONDARY_POSE_STRATEGY); } @Override protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - Optional optionalEstimatedRobotPose = photonPoseEstimator.update(latestResult); + final Optional estimatedPose; - inputs.hasResult = hasResult(optionalEstimatedRobotPose); + if (photonCamera.getLatestResult().getMultiTagResult().estimatedPose.isPresent) + estimatedPose = Optional.ofNullable(latestResult.getMultiTagResult().estimatedPose.best); + else + estimatedPose = Optional.ofNullable(latestResult.getBestTarget().getBestCameraToTarget()); + + inputs.hasResult = estimatedPose.isPresent(); if (inputs.hasResult) { - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(estimatedRobotPose.estimatedPose); - inputs.lastResultTimestamp = estimatedRobotPose.timestampSeconds; - inputs.visibleTags = estimatedRobotPose.targetsUsed.size(); - inputs.averageDistanceFromTags = getAverageDistanceFromTags(latestResult); + final Transform3d estimatedRobotPose = estimatedPose.get(); + final Rotation3d bestTargetRelativeRotation3d = test(latestResult); + inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(new Pose3d(estimatedRobotPose.getTranslation(), estimatedRobotPose.getRotation())); + inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); + inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getX(); + inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); + inputs.visibleTags = new int[latestResult.getTargets().size()]; + for (int i = 0; i < inputs.visibleTags.length; i++) { + if (i == 0) { + inputs.visibleTags[i] = latestResult.getBestTarget().getFiducialId(); + continue; + } + final int tagId = latestResult.getTargets().get(i).getFiducialId(); + if (tagId != latestResult.getBestTarget().getFiducialId()) + inputs.visibleTags[i] = tagId; + } } else { - inputs.visibleTags = 0; + inputs.visibleTags = new int[0]; inputs.cameraPose = new double[0]; } - logVisibleTags(inputs.hasResult, optionalEstimatedRobotPose); + logVisibleTags(inputs.hasResult, latestResult); } - private void logVisibleTags(boolean hasResult, Optional optionalEstimatedRobotPose) { + private void logVisibleTags(boolean hasResult, PhotonPipelineResult result) { if (!hasResult) { Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); return; } - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - final Pose2d[] visibleTagPoses = new Pose2d[estimatedRobotPose.targetsUsed.size()]; + final Pose2d[] visibleTagPoses = new Pose2d[result.getTargets().size()]; for (int i = 0; i < visibleTagPoses.length; i++) { - final int currentId = estimatedRobotPose.targetsUsed.get(i).getFiducialId(); + final int currentId = result.getTargets().get(i).getFiducialId(); final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); visibleTagPoses[i] = currentPose; } Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); } - private boolean hasResult(Optional optionalEstimatedRobotPose) { - final boolean isEmpty = optionalEstimatedRobotPose.isEmpty(); - if (isEmpty) - return false; - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - if (estimatedRobotPose.strategy == PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) - return true; - return estimatedRobotPose.targetsUsed.get(0).getPoseAmbiguity() < RobotPoseSourceConstants.MAXIMUM_AMBIGUITY; - } - private double getAverageDistanceFromTags(PhotonPipelineResult result) { final List targets = result.targets; double distanceSum = 0; @@ -87,4 +82,35 @@ private double getAverageDistanceFromTags(PhotonPipelineResult result) { return distanceSum / targets.size(); } + + private Rotation3d test(PhotonPipelineResult result) { + List targetCorners = result.getBestTarget().getDetectedCorners(); + double sumX = 0.0; + double sumY = 0.0; + for (TargetCorner t : targetCorners) { + sumX += t.x; + sumY += t.y; + } + + Point tagCenter = new Point(sumX / 4, sumY / 4); + + return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); + } + + public static Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { + double fx = camIntrinsics.get(0, 0); + double cx = camIntrinsics.get(0, 2); + double xOffset = cx - pixel.x; + + double fy = camIntrinsics.get(1, 1); + double cy = camIntrinsics.get(1, 2); + double yOffset = cy - pixel.y; + + // calculate yaw normally + var yaw = new Rotation2d(fx, xOffset); + // correct pitch based on yaw + var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); + + return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index ae4ff6b8..1983c0ea 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -53,7 +53,7 @@ public void update() { } public int getVisibleTags() { - return inputs.visibleTags; + return inputs.visibleTags.length; } public double getAverageDistanceFromTags() { diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java index 8303aae5..cb9718a1 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java @@ -27,7 +27,7 @@ public class RobotPoseSourceConstants { static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; public enum RobotPoseSourceType { - PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + PHOTON_CAMERA((name, transform3d) -> new AprilTagPhotonCameraIO(name)), LIMELIGHT((name, transform3d) -> new AprilTagLimelightIO(name)), T265((name, transform3d) -> new T265IO(name)); diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java index 494a71a2..229eed01 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java @@ -11,7 +11,8 @@ public static class RobotPoseSourceInputs { public boolean hasResult = false; public double lastResultTimestamp = 0; public double[] cameraPose = new double[6]; - public double averageDistanceFromTags = 0; - public int visibleTags = 0; + public int[] visibleTags = new int[0]; + public double bestTargetRelativeYaw = 0; + public double bestTargetRelativePitch = 0; } } From 76984ad3f58e2cc830ea5a91036ab713d6adeae6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 10 Sep 2024 12:23:20 +0300 Subject: [PATCH 002/261] Cleaned / fixed code and commented out errors --- .../robotposesources/AprilTagLimelightIO.java | 2 +- .../AprilTagPhotonCameraIO.java | 112 +++++++++--------- .../robotposesources/RobotPoseSource.java | 39 ++---- .../robotposesources/RobotPoseSourceIO.java | 5 +- .../robotposesources/T265IO.java | 2 +- 5 files changed, 72 insertions(+), 88 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java index e5c715fb..49d97050 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java @@ -14,7 +14,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.hasResult = LimelightHelpers.getTV(hostname); if (inputs.hasResult) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; - inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(results.getBotPose3d_wpiBlue()); + inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index aee6b471..d0c2eee5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -1,17 +1,17 @@ package frc.trigon.robot.poseestimation.robotposesources; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; -import org.littletonrobotics.junction.Logger; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; import java.util.List; -import java.util.Optional; public class AprilTagPhotonCameraIO extends RobotPoseSourceIO { private final PhotonCamera photonCamera; @@ -23,67 +23,71 @@ protected AprilTagPhotonCameraIO(String cameraName) { @Override protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - final Optional estimatedPose; - - if (photonCamera.getLatestResult().getMultiTagResult().estimatedPose.isPresent) - estimatedPose = Optional.ofNullable(latestResult.getMultiTagResult().estimatedPose.best); + inputs.hasResult = latestResult.hasTargets(); + if (inputs.hasResult) + updateHasResultInputs(inputs, latestResult); else - estimatedPose = Optional.ofNullable(latestResult.getBestTarget().getBestCameraToTarget()); - - inputs.hasResult = estimatedPose.isPresent(); - if (inputs.hasResult) { - final Transform3d estimatedRobotPose = estimatedPose.get(); - final Rotation3d bestTargetRelativeRotation3d = test(latestResult); - inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(new Pose3d(estimatedRobotPose.getTranslation(), estimatedRobotPose.getRotation())); - inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getX(); - inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); - inputs.visibleTags = new int[latestResult.getTargets().size()]; - for (int i = 0; i < inputs.visibleTags.length; i++) { - if (i == 0) { - inputs.visibleTags[i] = latestResult.getBestTarget().getFiducialId(); - continue; - } - final int tagId = latestResult.getTargets().get(i).getFiducialId(); - if (tagId != latestResult.getBestTarget().getFiducialId()) - inputs.visibleTags[i] = tagId; - } - } else { - inputs.visibleTags = new int[0]; - inputs.cameraPose = new double[0]; - } - - logVisibleTags(inputs.hasResult, latestResult); + updateNoResultInputs(inputs); } - private void logVisibleTags(boolean hasResult, PhotonPipelineResult result) { - if (!hasResult) { - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); - return; +// private void logVisibleTags(boolean hasResult, PhotonPipelineResult result) { +// if (!hasResult) { +// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); +// return; +// } +// +// final Pose2d[] visibleTagPoses = new Pose2d[result.getTargets().size()]; +// for (int i = 0; i < visibleTagPoses.length; i++) { +// final int currentId = result.getTargets().get(i).getFiducialId(); +// final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); +// visibleTagPoses[i] = currentPose; +// } +// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); +// } +// +// private double getAverageDistanceFromTags(PhotonPipelineResult result) { +// final List targets = result.targets; +// double distanceSum = 0; +// +// for (PhotonTrackedTarget currentTarget : targets) { +// final Translation2d distanceTranslation = currentTarget.getBestCameraToTarget().getTranslation().toTranslation2d(); +// distanceSum += distanceTranslation.getNorm(); +// } +// +// return distanceSum / targets.size(); +// } + + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + Transform3d estimatedPose = result.getBestTarget().getBestCameraToTarget(); + if (result.getMultiTagResult().estimatedPose.isPresent) { + estimatedPose = result.getMultiTagResult().estimatedPose.best; } + return new Pose3d(estimatedPose.getTranslation(), estimatedPose.getRotation()); + } - final Pose2d[] visibleTagPoses = new Pose2d[result.getTargets().size()]; - for (int i = 0; i < visibleTagPoses.length; i++) { - final int currentId = result.getTargets().get(i).getFiducialId(); - final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); - visibleTagPoses[i] = currentPose; + private int[] getVisibleTagIDs(PhotonPipelineResult result) { + int[] visibleTagIDs = new int[result.getTargets().size()]; + for (int i = 0; i < visibleTagIDs.length; i++) { + visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); } - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); + return visibleTagIDs; } - private double getAverageDistanceFromTags(PhotonPipelineResult result) { - final List targets = result.targets; - double distanceSum = 0; - - for (PhotonTrackedTarget currentTarget : targets) { - final Translation2d distanceTranslation = currentTarget.getBestCameraToTarget().getTranslation().toTranslation2d(); - distanceSum += distanceTranslation.getNorm(); - } + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + inputs.solvePNPPose = getSolvePNPPose(latestResult); + inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); + inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getX(); + inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + } - return distanceSum / targets.size(); + private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[0]; + inputs.solvePNPPose = new Pose3d(); } - private Rotation3d test(PhotonPipelineResult result) { + private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { List targetCorners = result.getBestTarget().getDetectedCorners(); double sumX = 0.0; double sumY = 0.0; diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 1983c0ea..66910223 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -1,6 +1,8 @@ package frc.trigon.robot.poseestimation.robotposesources; -import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -28,40 +30,27 @@ public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSou robotPoseSourceIO = new RobotPoseSourceIO(); } - public static double[] pose3dToDoubleArray(Pose3d pose) { - if (pose == null) - return new double[0]; - - return new double[]{ - pose.getTranslation().getX(), - pose.getTranslation().getY(), - pose.getTranslation().getZ(), - pose.getRotation().getX(), - pose.getRotation().getY(), - pose.getRotation().getZ() - }; - } - public void update() { robotPoseSourceIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); cachedPose = getUnCachedRobotPose(); - if (!inputs.hasResult || inputs.averageDistanceFromTags == 0 || cachedPose == null) + if (!inputs.hasResult || cachedPose == null) Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); else Logger.recordOutput("Poses/Robot/" + name + "Pose", cachedPose); } public int getVisibleTags() { - return inputs.visibleTags.length; + return inputs.visibleTagIDs.length; } public double getAverageDistanceFromTags() { - return inputs.averageDistanceFromTags; +// return inputs.averageDistanceFromTags; + return 0; } public boolean hasNewResult() { - return (inputs.hasResult && inputs.averageDistanceFromTags != 0) && isNewTimestamp(); + return inputs.hasResult && isNewTimestamp(); } public Pose2d getRobotPose() { @@ -77,7 +66,7 @@ public double getLastResultTimestamp() { } private Pose2d getUnCachedRobotPose() { - final Pose3d cameraPose = doubleArrayToPose3d(inputs.cameraPose); + final Pose3d cameraPose = inputs.solvePNPPose; if (cameraPose == null) return null; @@ -91,14 +80,4 @@ private boolean isNewTimestamp() { lastUpdatedTimestamp = getLastResultTimestamp(); return true; } - - private Pose3d doubleArrayToPose3d(double[] doubleArray) { - if (doubleArray == null || doubleArray.length != 6) - return null; - - return new Pose3d( - new Translation3d(doubleArray[0], doubleArray[1], doubleArray[2]), - new Rotation3d(doubleArray[3], doubleArray[4], doubleArray[5]) - ); - } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java index 229eed01..36fbf130 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java @@ -1,5 +1,6 @@ package frc.trigon.robot.poseestimation.robotposesources; +import edu.wpi.first.math.geometry.Pose3d; import org.littletonrobotics.junction.AutoLog; public class RobotPoseSourceIO { @@ -10,8 +11,8 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { public static class RobotPoseSourceInputs { public boolean hasResult = false; public double lastResultTimestamp = 0; - public double[] cameraPose = new double[6]; - public int[] visibleTags = new int[0]; + public Pose3d solvePNPPose = new Pose3d(); + public int[] visibleTagIDs = new int[0]; public double bestTargetRelativeYaw = 0; public double bestTargetRelativePitch = 0; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java index 89d7c8d6..506fb400 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java @@ -19,7 +19,7 @@ protected T265IO(String name) { protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.hasResult = canUseJsonDump(); if (inputs.hasResult) - inputs.cameraPose = RobotPoseSource.pose3dToDoubleArray(getCameraPose()); + inputs.solvePNPPose = getCameraPose(); inputs.lastResultTimestamp = (double) jsonDump.getLastChange() / 1000000; } From f3cfd1baebe3b35669dcbe5b5f455e53a64ad23c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 11 Sep 2024 11:43:05 +0300 Subject: [PATCH 003/261] Added assumed robot heading pose stuff --- .../PhotonPoseEstimator.java | 6 +- .../poseestimator/PoseEstimator.java | 4 +- .../AprilTagPhotonCameraIO.java | 101 ++++++++++-------- .../robotposesources/RobotPoseSource.java | 101 +++++++++++++----- .../RobotPoseSourceConstants.java | 13 ++- .../robotposesources/RobotPoseSourceIO.java | 2 + .../robotposesources/T265IO.java | 76 ------------- 7 files changed, 141 insertions(+), 162 deletions(-) delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java index 9c932c21..bd2c3269 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java @@ -494,11 +494,11 @@ private Optional multiTagOnCoprocStrategy( Optional> cameraMatrixOpt, Optional> distCoeffsOpt) { if (result.getMultiTagResult().estimatedPose.isPresent) { - var best_tf = result.getMultiTagResult().estimatedPose.best; - var cam = new Pose3d() + Transform3d best_tf = result.getMultiTagResult().estimatedPose.best; + Pose3d cam = new Pose3d() .plus(best_tf) // field-to-camera .relativeTo(fieldTags.getOrigin()); - var best = cam.plus(robotToCamera.inverse()); // field-to-robot + Pose3d best = cam.plus(robotToCamera.inverse()); // field-to-robot // Logger.recordOutput(camera.getName(), Math.toDegrees(cam.getRotation().getY())); return Optional.of( diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 1cc1deec..77011b31 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -104,14 +104,14 @@ private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource robotPoseSource.update(); if (!robotPoseSource.hasNewResult()) return null; - final Pose2d robotPose = robotPoseSource.getRobotPose(); + final Pose2d robotPose = robotPoseSource.getCurrentRobotPose(); if (robotPose == null) return null; return new PoseEstimator6328.VisionObservation( robotPose, robotPoseSource.getLastResultTimestamp(), - averageDistanceToStdDevs(robotPoseSource.getAverageDistanceFromTags(), robotPoseSource.getVisibleTags()) + averageDistanceToStdDevs(robotPoseSource.getAverageDistanceFromTags(), robotPoseSource.getNumberOfVisibleTags()) ); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index d0c2eee5..9bffb993 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.constants.FieldConstants; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; @@ -23,6 +24,7 @@ protected AprilTagPhotonCameraIO(String cameraName) { @Override protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); + inputs.hasResult = latestResult.hasTargets(); if (inputs.hasResult) updateHasResultInputs(inputs, latestResult); @@ -30,49 +32,54 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { updateNoResultInputs(inputs); } -// private void logVisibleTags(boolean hasResult, PhotonPipelineResult result) { -// if (!hasResult) { -// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); -// return; -// } -// -// final Pose2d[] visibleTagPoses = new Pose2d[result.getTargets().size()]; -// for (int i = 0; i < visibleTagPoses.length; i++) { -// final int currentId = result.getTargets().get(i).getFiducialId(); -// final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); -// visibleTagPoses[i] = currentPose; -// } -// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); -// } -// -// private double getAverageDistanceFromTags(PhotonPipelineResult result) { -// final List targets = result.targets; -// double distanceSum = 0; -// -// for (PhotonTrackedTarget currentTarget : targets) { -// final Translation2d distanceTranslation = currentTarget.getBestCameraToTarget().getTranslation().toTranslation2d(); -// distanceSum += distanceTranslation.getNorm(); -// } -// -// return distanceSum / targets.size(); -// } + private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { + List tagCorners = result.getBestTarget().getDetectedCorners(); + Point tagCenter = new Point(getTagXFromTagCorners(tagCorners), getTagYFromTagCorners(tagCorners)); + return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); + } private Pose3d getSolvePNPPose(PhotonPipelineResult result) { - Transform3d estimatedPose = result.getBestTarget().getBestCameraToTarget(); if (result.getMultiTagResult().estimatedPose.isPresent) { - estimatedPose = result.getMultiTagResult().estimatedPose.best; + final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; + return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - return new Pose3d(estimatedPose.getTranslation(), estimatedPose.getRotation()); + + final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); + return RobotPoseSource.getTagPose(result.getBestTarget().getFiducialId()).transformBy(targetToCamera); } private int[] getVisibleTagIDs(PhotonPipelineResult result) { - int[] visibleTagIDs = new int[result.getTargets().size()]; + final int[] visibleTagIDs = new int[result.getTargets().size()]; + visibleTagIDs[0] = result.getBestTarget().getFiducialId(); + int idAddition = 1; + for (int i = 0; i < visibleTagIDs.length; i++) { - visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); + final int currentID = result.getTargets().get(i).getFiducialId(); + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + visibleTagIDs[i + idAddition] = currentID; } + return visibleTagIDs; } + private double getAverageDistanceFromAllTags(PhotonPipelineResult result) { + final int tagsSeen = result.getTargets().size(); + double totalTagDistance = 0; + + for (int i = 0; i < tagsSeen; i++) + totalTagDistance += result.getTargets().get(i).getBestCameraToTarget().getTranslation().getNorm(); + + return totalTagDistance / tagsSeen; + } + + private double getAverageDistanceFromBestTag(PhotonPipelineResult result) { + return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); + } + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); inputs.solvePNPPose = getSolvePNPPose(latestResult); @@ -80,6 +87,8 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Photo inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getX(); inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(latestResult); + inputs.averageDistanceFromBestTag = getAverageDistanceFromBestTag(latestResult); } private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { @@ -87,21 +96,7 @@ private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.solvePNPPose = new Pose3d(); } - private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - List targetCorners = result.getBestTarget().getDetectedCorners(); - double sumX = 0.0; - double sumY = 0.0; - for (TargetCorner t : targetCorners) { - sumX += t.x; - sumY += t.y; - } - - Point tagCenter = new Point(sumX / 4, sumY / 4); - - return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); - } - - public static Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { + private static Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { double fx = camIntrinsics.get(0, 0); double cx = camIntrinsics.get(0, 2); double xOffset = cx - pixel.x; @@ -117,4 +112,18 @@ public static Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsi return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); } + + private static double getTagXFromTagCorners(List tagCorners) { + double tagCornerSumX = 0; + for (TargetCorner tagCorner : tagCorners) + tagCornerSumX += tagCorner.x; + return tagCornerSumX / tagCorners.size(); + } + + private static double getTagYFromTagCorners(List tagCorners) { + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) + tagCornerSumY += tagCorner.y; + return tagCornerSumY / tagCorners.size(); + } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 66910223..5e69f215 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -1,10 +1,11 @@ package frc.trigon.robot.poseestimation.robotposesources; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.*; import frc.trigon.robot.Robot; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonUtils; /** * A pose source is a class that provides the robot's pose, from a camera. @@ -15,17 +16,14 @@ public class RobotPoseSource { private final Transform3d robotCenterToCamera; private final RobotPoseSourceIO robotPoseSourceIO; private double lastUpdatedTimestamp; - private Pose2d cachedPose = null; + private Pose2d robotPose = null; public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera) { this.name = name; - if (robotPoseSourceType != RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA) - this.robotCenterToCamera = robotCenterToCamera; - else - this.robotCenterToCamera = new Transform3d(); + this.robotCenterToCamera = robotCenterToCamera; if (Robot.IS_REAL) - robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name, robotCenterToCamera); + robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name); else robotPoseSourceIO = new RobotPoseSourceIO(); } @@ -33,44 +31,76 @@ public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSou public void update() { robotPoseSourceIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); - cachedPose = getUnCachedRobotPose(); - if (!inputs.hasResult || cachedPose == null) + + robotPose = getBestRobotPose(inputs.averageDistanceFromAllTags); + + if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); else - Logger.recordOutput("Poses/Robot/" + name + "Pose", cachedPose); + Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); } - public int getVisibleTags() { - return inputs.visibleTagIDs.length; + public boolean hasNewResult() { + return (inputs.hasResult && inputs.averageDistanceFromAllTags != 0) && isNewTimestamp(); } - public double getAverageDistanceFromTags() { -// return inputs.averageDistanceFromTags; - return 0; + public Pose2d getCurrentRobotPose() { + return robotPose; } - public boolean hasNewResult() { - return inputs.hasResult && isNewTimestamp(); + public String getName() { + return name; } - public Pose2d getRobotPose() { - return cachedPose; + public double getAverageDistanceFromTags() { + return inputs.averageDistanceFromAllTags; } - public String getName() { - return name; + public int getNumberOfVisibleTags() { + return inputs.visibleTagIDs.length; } public double getLastResultTimestamp() { return inputs.lastResultTimestamp; } - private Pose2d getUnCachedRobotPose() { - final Pose3d cameraPose = inputs.solvePNPPose; - if (cameraPose == null) - return null; + protected static Pose3d getTagPose(int targetTag) { + return FieldConstants.APRIL_TAG_FIELD_LAYOUT.getTagPose(targetTag).get(); + } + + private Pose2d getBestRobotPose(double averageDistanceFromAllTags) { + if (averageDistanceFromAllTags < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) + return getRobotPoseFromCameraPose(inputs.solvePNPPose); + return getAssumedRobotHeadingPose().toPose2d(); + } + + private Pose3d getAssumedRobotHeadingPose() { + final Rotation2d robotHeading = RobotContainer.SWERVE.getHeading(); + final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); + return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeading)); + } - return cameraPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); + private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { + final Pose3d tagPose = getTagPose(inputs.visibleTagIDs[0]); + final Translation2d tagToCamera = getTagToCamera(tagPose); + final Translation2d robotToTag = tagToCamera.plus(robotCenterToCamera.getTranslation().toTranslation2d()); + final Translation2d fieldRelativeTagPose = robotToTag.rotateBy(robotHeading); + return tagPose.getTranslation().toTranslation2d().plus(fieldRelativeTagPose); + } + + private Translation2d getTagToCamera(Pose3d tagPose) { + final double cameraToTagDistanceMeters = -PhotonUtils.calculateDistanceToTargetMeters( + robotCenterToCamera.getZ(), tagPose.getZ(), robotCenterToCamera.getRotation().getY(), inputs.bestTargetRelativePitch + ); + final double cameraToTagXDistance = Math.sin(inputs.bestTargetRelativeYaw + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; + final double cameraToTagYDistance = -Math.cos(inputs.bestTargetRelativeYaw + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; + return new Translation2d(cameraToTagXDistance, cameraToTagYDistance); + } + + private Pose2d getRobotPoseFromCameraPose(Pose3d solvePNPPose) { + if (solvePNPPose == null) + return null; + return solvePNPPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); } private boolean isNewTimestamp() { @@ -80,4 +110,19 @@ private boolean isNewTimestamp() { lastUpdatedTimestamp = getLastResultTimestamp(); return true; } + +// private void logVisibleTags(boolean hasResult, PhotonPipelineResult result) { +// if (!hasResult) { +// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); +// return; +// } +// +// final Pose2d[] visibleTagPoses = new Pose2d[result.getTargets().size()]; +// for (int i = 0; i < visibleTagPoses.length; i++) { +// final int currentId = result.getTargets().get(i).getFiducialId(); +// final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); +// visibleTagPoses[i] = currentPose; +// } +// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); +// } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java index cb9718a1..a57e2b0b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java @@ -5,11 +5,10 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; import java.util.HashMap; -import java.util.function.BiFunction; +import java.util.function.Function; public class RobotPoseSourceConstants { public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); @@ -24,16 +23,16 @@ public class RobotPoseSourceConstants { } static final double MAXIMUM_AMBIGUITY = 0.2; + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; public enum RobotPoseSourceType { - PHOTON_CAMERA((name, transform3d) -> new AprilTagPhotonCameraIO(name)), - LIMELIGHT((name, transform3d) -> new AprilTagLimelightIO(name)), - T265((name, transform3d) -> new T265IO(name)); + PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + LIMELIGHT(AprilTagLimelightIO::new); - final BiFunction createIOFunction; + final Function createIOFunction; - RobotPoseSourceType(BiFunction createIOFunction) { + RobotPoseSourceType(Function createIOFunction) { this.createIOFunction = createIOFunction; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java index 36fbf130..0e735038 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java @@ -15,5 +15,7 @@ public static class RobotPoseSourceInputs { public int[] visibleTagIDs = new int[0]; public double bestTargetRelativeYaw = 0; public double bestTargetRelativePitch = 0; + public double averageDistanceFromAllTags = 0; + public double averageDistanceFromBestTag = 0; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java deleted file mode 100644 index 506fb400..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java +++ /dev/null @@ -1,76 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import org.trigon.utilities.JsonHandler; - -public class T265IO extends RobotPoseSourceIO { - private static final NetworkTable NETWORK_TABLE = NetworkTableInstance.getDefault().getTable("T265"); - private static final short CONFIDENCE_THRESHOLD = 2; - private final NetworkTableEntry jsonDump; - - protected T265IO(String name) { - jsonDump = NETWORK_TABLE.getEntry(name + "/jsonDump"); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = canUseJsonDump(); - if (inputs.hasResult) - inputs.solvePNPPose = getCameraPose(); - inputs.lastResultTimestamp = (double) jsonDump.getLastChange() / 1000000; - } - - private Pose3d getCameraPose() { - if (!canUseJsonDump()) - return null; - - return getRobotPoseFromJsonDump(); - } - - private Pose3d getRobotPoseFromJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - final Translation3d translation = getTranslationFromDoubleArray(jsonDump.translation); - final Rotation3d rotation = getRotationFromDoubleArray(jsonDump.rotation); - - return t265PoseToWPIPose(new Pose3d(translation, rotation)); - } - - private Pose3d t265PoseToWPIPose(Pose3d t265Pose) { - final CoordinateSystem eusCoordinateSystem = new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.U(), CoordinateAxis.S()); - final Pose3d convertedPose = CoordinateSystem.convert(t265Pose, eusCoordinateSystem, CoordinateSystem.NWU()); - final Rotation3d convertedRotation = convertedPose.getRotation().plus(new Rotation3d(0, 0, Math.toRadians(90))); - - return new Pose3d(convertedPose.getTranslation(), convertedRotation); - } - - private Translation3d getTranslationFromDoubleArray(double[] xyz) { - return new Translation3d(xyz[0], xyz[1], xyz[2]); - } - - private Rotation3d getRotationFromDoubleArray(double[] wxyz) { - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); - } - - private boolean canUseJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - - try { - return jsonDump.confidence >= CONFIDENCE_THRESHOLD && jsonDump.translation.length == 3 && jsonDump.rotation.length == 4; - } catch (NullPointerException e) { - return false; - } - } - - private T265JsonDump getJsonDump() { - return JsonHandler.parseJsonStringToObject(jsonDump.getString(""), T265JsonDump.class); - } - - private static class T265JsonDump { - private double[] translation; - private double[] rotation; - private int confidence; - } -} \ No newline at end of file From 8c813aba7f33f0136a996ec1971dd3eae2d8eeb7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 11 Sep 2024 18:20:20 +0300 Subject: [PATCH 004/261] Recleaned code and added some javadoc --- .../AprilTagPhotonCameraIO.java | 41 +++++++++++-------- .../robotposesources/RobotPoseSource.java | 38 ++++++++++------- 2 files changed, 47 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index 9bffb993..679700ca 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -32,12 +32,22 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { updateNoResultInputs(inputs); } + /** + * @param result the camera's final resulted image + * @return the apriltag's rotation relative to the camera + */ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { List tagCorners = result.getBestTarget().getDetectedCorners(); - Point tagCenter = new Point(getTagXFromTagCorners(tagCorners), getTagYFromTagCorners(tagCorners)); - return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); + Point tagCenter = getTagCenter(tagCorners); + if (photonCamera.getCameraMatrix().isPresent()) + return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); + return null; } + /** + * @param result the camera's final resulted image + * @return the estimated camera's position using solve PNP and multiple tags if possible + */ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { if (result.getMultiTagResult().estimatedPose.isPresent) { final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; @@ -82,9 +92,10 @@ private double getAverageDistanceFromBestTag(PhotonPipelineResult result) { private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + inputs.solvePNPPose = getSolvePNPPose(latestResult); inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getX(); + inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getY(); inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(latestResult); @@ -96,6 +107,16 @@ private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.solvePNPPose = new Pose3d(); } + private static Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + private static Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { double fx = camIntrinsics.get(0, 0); double cx = camIntrinsics.get(0, 2); @@ -112,18 +133,4 @@ private static Rotation3d correctPixelRot(Point pixel, Matrix camIntrins return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); } - - private static double getTagXFromTagCorners(List tagCorners) { - double tagCornerSumX = 0; - for (TargetCorner tagCorner : tagCorners) - tagCornerSumX += tagCorner.x; - return tagCornerSumX / tagCorners.size(); - } - - private static double getTagYFromTagCorners(List tagCorners) { - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) - tagCornerSumY += tagCorner.y; - return tagCornerSumY / tagCorners.size(); - } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 5e69f215..9db49ee0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -7,6 +7,8 @@ import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonUtils; +import java.util.Optional; + /** * A pose source is a class that provides the robot's pose, from a camera. */ @@ -44,6 +46,9 @@ public boolean hasNewResult() { return (inputs.hasResult && inputs.averageDistanceFromAllTags != 0) && isNewTimestamp(); } + /** + * @return the current estimated robot position + */ public Pose2d getCurrentRobotPose() { return robotPose; } @@ -65,9 +70,14 @@ public double getLastResultTimestamp() { } protected static Pose3d getTagPose(int targetTag) { - return FieldConstants.APRIL_TAG_FIELD_LAYOUT.getTagPose(targetTag).get(); + Optional tagPose = FieldConstants.APRIL_TAG_FIELD_LAYOUT.getTagPose(targetTag); + return tagPose.orElse(new Pose3d()); } + /** + * @param averageDistanceFromAllTags the average of the distance from all visible tags + * @return the best calculated robot pose + */ private Pose2d getBestRobotPose(double averageDistanceFromAllTags) { if (averageDistanceFromAllTags < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) return getRobotPoseFromCameraPose(inputs.solvePNPPose); @@ -111,18 +121,16 @@ private boolean isNewTimestamp() { return true; } -// private void logVisibleTags(boolean hasResult, PhotonPipelineResult result) { -// if (!hasResult) { -// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); -// return; -// } -// -// final Pose2d[] visibleTagPoses = new Pose2d[result.getTargets().size()]; -// for (int i = 0; i < visibleTagPoses.length; i++) { -// final int currentId = result.getTargets().get(i).getFiducialId(); -// final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); -// visibleTagPoses[i] = currentPose; -// } -// Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); -// } + private void logVisibleTags() { + if (inputs.hasResult) { + Logger.recordOutput("VisibleTags/" + this.getName(), new Pose2d[0]); + return; + } + + final Pose2d[] visibleTagPoses = new Pose2d[inputs.visibleTagIDs.length]; + for (int i = 0; i < visibleTagPoses.length; i++) { + visibleTagPoses[i] = getTagPose(i).toPose2d(); + } + Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); + } } From 63a65121a6f31287c0bd19983378d6425cb33a27 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 12 Sep 2024 09:04:47 +0300 Subject: [PATCH 005/261] Updated javadoc --- .../robotposesources/AprilTagPhotonCameraIO.java | 8 ++++++-- .../poseestimation/robotposesources/RobotPoseSource.java | 4 +++- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index 679700ca..66adf5c2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -33,8 +33,10 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { } /** + * Estimates the camera's rotation relative to the apriltag + * * @param result the camera's final resulted image - * @return the apriltag's rotation relative to the camera + * @return the result */ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { List tagCorners = result.getBestTarget().getDetectedCorners(); @@ -45,8 +47,10 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { } /** + * Estimates the camera's pose using Solve PNP and uses multiple tags if possible + * * @param result the camera's final resulted image - * @return the estimated camera's position using solve PNP and multiple tags if possible + * @return the result */ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { if (result.getMultiTagResult().estimatedPose.isPresent) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 9db49ee0..ea653e27 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -75,8 +75,10 @@ protected static Pose3d getTagPose(int targetTag) { } /** + * Gets the best possible robot Pose from the camera's data + * * @param averageDistanceFromAllTags the average of the distance from all visible tags - * @return the best calculated robot pose + * @return the result */ private Pose2d getBestRobotPose(double averageDistanceFromAllTags) { if (averageDistanceFromAllTags < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) From ae1eda80bda82314490d5baf36bce1747c795b92 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 12 Sep 2024 12:45:12 +0300 Subject: [PATCH 006/261] Made variables final --- .../robotposesources/AprilTagPhotonCameraIO.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index 66adf5c2..6c88d7b6 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -39,8 +39,8 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { * @return the result */ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - List tagCorners = result.getBestTarget().getDetectedCorners(); - Point tagCenter = getTagCenter(tagCorners); + final List tagCorners = result.getBestTarget().getDetectedCorners(); + final Point tagCenter = getTagCenter(tagCorners); if (photonCamera.getCameraMatrix().isPresent()) return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); return null; From 788667bbe9bc4f2e9028707e2caf500a9fa97feb Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 12 Sep 2024 18:33:29 +0300 Subject: [PATCH 007/261] Used thingy in field constants --- .../robotposesources/RobotPoseSource.java | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index ea653e27..139ae2c5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -7,8 +7,6 @@ import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonUtils; -import java.util.Optional; - /** * A pose source is a class that provides the robot's pose, from a camera. */ @@ -69,11 +67,6 @@ public double getLastResultTimestamp() { return inputs.lastResultTimestamp; } - protected static Pose3d getTagPose(int targetTag) { - Optional tagPose = FieldConstants.APRIL_TAG_FIELD_LAYOUT.getTagPose(targetTag); - return tagPose.orElse(new Pose3d()); - } - /** * Gets the best possible robot Pose from the camera's data * @@ -93,7 +86,7 @@ private Pose3d getAssumedRobotHeadingPose() { } private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { - final Pose3d tagPose = getTagPose(inputs.visibleTagIDs[0]); + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]); final Translation2d tagToCamera = getTagToCamera(tagPose); final Translation2d robotToTag = tagToCamera.plus(robotCenterToCamera.getTranslation().toTranslation2d()); final Translation2d fieldRelativeTagPose = robotToTag.rotateBy(robotHeading); @@ -131,7 +124,7 @@ private void logVisibleTags() { final Pose2d[] visibleTagPoses = new Pose2d[inputs.visibleTagIDs.length]; for (int i = 0; i < visibleTagPoses.length; i++) { - visibleTagPoses[i] = getTagPose(i).toPose2d(); + visibleTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(i).toPose2d(); } Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); } From 9994c28064292db03122b22082ab686565ddde7f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 12 Sep 2024 18:35:06 +0300 Subject: [PATCH 008/261] Fixed error --- .../poseestimation/robotposesources/AprilTagPhotonCameraIO.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index 6c88d7b6..b41abef4 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -59,7 +59,7 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { } final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); - return RobotPoseSource.getTagPose(result.getBestTarget().getFiducialId()).transformBy(targetToCamera); + return FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()).transformBy(targetToCamera); } private int[] getVisibleTagIDs(PhotonPipelineResult result) { From cd7e5de7517cfd975811ea295c6fba5fa5b82b59 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 09:28:08 +0300 Subject: [PATCH 009/261] Fixed javadoc and added some random thing I don't even know --- .../AprilTagPhotonCameraIO.java | 12 ++--- .../robotposesources/RobotPoseSource.java | 51 ++++++++++++++----- 2 files changed, 43 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index b41abef4..2c4341f5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -33,10 +33,10 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { } /** - * Estimates the camera's rotation relative to the apriltag + * Estimates the camera's rotation relative to the apriltag. * - * @param result the camera's final resulted image - * @return the result + * @param result the camera's pipeline result + * @return the estimated rotation */ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { final List tagCorners = result.getBestTarget().getDetectedCorners(); @@ -47,10 +47,10 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { } /** - * Estimates the camera's pose using Solve PNP and uses multiple tags if possible + * Estimates the camera's pose using Solve PNP using as many tags as possible. * - * @param result the camera's final resulted image - * @return the result + * @param result the camera's pipeline result + * @return the estimated pose */ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { if (result.getMultiTagResult().estimatedPose.isPresent) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 139ae2c5..01219b8f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -1,6 +1,10 @@ package frc.trigon.robot.poseestimation.robotposesources; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.Robot; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; @@ -32,7 +36,7 @@ public void update() { robotPoseSourceIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); - robotPose = getBestRobotPose(inputs.averageDistanceFromAllTags); + robotPose = calculateBestRobotPose(inputs.averageDistanceFromBestTag); if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); @@ -68,18 +72,24 @@ public double getLastResultTimestamp() { } /** - * Gets the best possible robot Pose from the camera's data + * If the robot is close enough to the tag, it gets the estimated solve PNP pose. + * If it's too far, it assumes the robot's heading and calculates its position from there. * - * @param averageDistanceFromAllTags the average of the distance from all visible tags - * @return the result + * @param averageDistanceFromBestTag the average of the distance from the best visible tag + * @return the robot's pose */ - private Pose2d getBestRobotPose(double averageDistanceFromAllTags) { - if (averageDistanceFromAllTags < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) - return getRobotPoseFromCameraPose(inputs.solvePNPPose); - return getAssumedRobotHeadingPose().toPose2d(); + private Pose2d calculateBestRobotPose(double averageDistanceFromBestTag) { + if (averageDistanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) + return cameraPoseToRobotPose(inputs.solvePNPPose); + return calculateAssumedRobotHeadingPose().toPose2d(); } - private Pose3d getAssumedRobotHeadingPose() { + /** + * Calculates the robot's pose by assuming its heading. + * + * @return the robot's pose + */ + private Pose3d calculateAssumedRobotHeadingPose() { final Rotation2d robotHeading = RobotContainer.SWERVE.getHeading(); final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeading)); @@ -102,10 +112,10 @@ private Translation2d getTagToCamera(Pose3d tagPose) { return new Translation2d(cameraToTagXDistance, cameraToTagYDistance); } - private Pose2d getRobotPoseFromCameraPose(Pose3d solvePNPPose) { - if (solvePNPPose == null) + private Pose2d cameraPoseToRobotPose(Pose3d cameraPose) { + if (cameraPose == null) return null; - return solvePNPPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); + return cameraPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); } private boolean isNewTimestamp() { @@ -123,9 +133,22 @@ private void logVisibleTags() { } final Pose2d[] visibleTagPoses = new Pose2d[inputs.visibleTagIDs.length]; - for (int i = 0; i < visibleTagPoses.length; i++) { + for (int i = 0; i < visibleTagPoses.length; i++) visibleTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(i).toPose2d(); - } Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); } + + private Matrix solvePNPAverageDistanceToStdDevs(double translationsStdExponent, double thetaStdExponent, int visibleTags) { + final double translationStd = translationsStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / (visibleTags * visibleTags); + final double thetaStd = thetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / visibleTags; + + return VecBuilder.fill(translationStd, translationStd, thetaStd); + } + + private Matrix assumedHeadingAverageDistanceToStdDevs(double assumedRobotHeadingPose, int visibleTags) { + final double translationStd = assumedRobotHeadingPose * Math.pow(inputs.averageDistanceFromBestTag, 2) / (visibleTags * visibleTags); + final double thetaStd = Double.POSITIVE_INFINITY; + + return VecBuilder.fill(translationStd, translationStd, thetaStd); + } } From 11503b26ab82735ff307359f3ddc5046fd1d65f8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 10:08:14 -0400 Subject: [PATCH 010/261] Did random stuff --- .../poseestimator/PoseEstimator.java | 2 +- .../AprilTagPhotonCameraIO.java | 56 +++++++++---------- .../robotposesources/RobotPoseSource.java | 35 ++++++++---- .../robotposesources/RobotPoseSourceIO.java | 2 +- 4 files changed, 53 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 77011b31..7989e895 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -111,7 +111,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource return new PoseEstimator6328.VisionObservation( robotPose, robotPoseSource.getLastResultTimestamp(), - averageDistanceToStdDevs(robotPoseSource.getAverageDistanceFromTags(), robotPoseSource.getNumberOfVisibleTags()) + robotPoseSource.calculateStdDevs() ); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index 2c4341f5..61d1bbfe 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -32,6 +32,33 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { updateNoResultInputs(inputs); } + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + + inputs.solvePNPPose = getSolvePNPPose(latestResult); + inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); + inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(latestResult); + inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + } + + private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[0]; + inputs.solvePNPPose = new Pose3d(); + } + + private static Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + /** * Estimates the camera's rotation relative to the apriltag. * @@ -90,37 +117,10 @@ private double getAverageDistanceFromAllTags(PhotonPipelineResult result) { return totalTagDistance / tagsSeen; } - private double getAverageDistanceFromBestTag(PhotonPipelineResult result) { + private double getDistanceFromBestTag(PhotonPipelineResult result) { return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); } - private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { - final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); - - inputs.solvePNPPose = getSolvePNPPose(latestResult); - inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getY(); - inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); - inputs.visibleTagIDs = getVisibleTagIDs(latestResult); - inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(latestResult); - inputs.averageDistanceFromBestTag = getAverageDistanceFromBestTag(latestResult); - } - - private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.visibleTagIDs = new int[0]; - inputs.solvePNPPose = new Pose3d(); - } - - private static Point getTagCenter(List tagCorners) { - double tagCornerSumX = 0; - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) { - tagCornerSumX += tagCorner.x; - tagCornerSumY += tagCorner.y; - } - return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); - } - private static Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { double fx = camIntrinsics.get(0, 0); double cx = camIntrinsics.get(0, 2); diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 01219b8f..21ff27d9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -8,6 +8,7 @@ import frc.trigon.robot.Robot; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonUtils; @@ -19,12 +20,14 @@ public class RobotPoseSource { private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); private final Transform3d robotCenterToCamera; private final RobotPoseSourceIO robotPoseSourceIO; - private double lastUpdatedTimestamp; + private double lastUpdatedTimestamp, translationsStdExponent, thetaStdExponent; private Pose2d robotPose = null; - public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera) { + public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double translationsStdExponent, double thetaStdExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; + this.translationsStdExponent = translationsStdExponent; + this.thetaStdExponent = thetaStdExponent; if (Robot.IS_REAL) robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name); @@ -36,7 +39,7 @@ public void update() { robotPoseSourceIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); - robotPose = calculateBestRobotPose(inputs.averageDistanceFromBestTag); + robotPose = calculateBestRobotPose(inputs.distanceFromBestTag); if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); @@ -71,15 +74,21 @@ public double getLastResultTimestamp() { return inputs.lastResultTimestamp; } + public Matrix calculateStdDevs() { + if (inputs.distanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) + return solvePNPAverageDistanceToStdDevs(); + return assumedHeadingAverageDistanceToStdDevs(); + } + /** * If the robot is close enough to the tag, it gets the estimated solve PNP pose. * If it's too far, it assumes the robot's heading and calculates its position from there. * - * @param averageDistanceFromBestTag the average of the distance from the best visible tag + * @param distanceFromBestTag the average of the distance from the best visible tag * @return the robot's pose */ - private Pose2d calculateBestRobotPose(double averageDistanceFromBestTag) { - if (averageDistanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) + private Pose2d calculateBestRobotPose(double distanceFromBestTag) { + if (distanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) return cameraPoseToRobotPose(inputs.solvePNPPose); return calculateAssumedRobotHeadingPose().toPose2d(); } @@ -90,7 +99,8 @@ private Pose2d calculateBestRobotPose(double averageDistanceFromBestTag) { * @return the robot's pose */ private Pose3d calculateAssumedRobotHeadingPose() { - final Rotation2d robotHeading = RobotContainer.SWERVE.getHeading(); + final Rotation2d robotHeading = PoseEstimator6328.getInstance().getEstimatedPose().getRotation(); + RobotContainer.SWERVE.getHeading(); final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeading)); } @@ -138,15 +148,16 @@ private void logVisibleTags() { Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); } - private Matrix solvePNPAverageDistanceToStdDevs(double translationsStdExponent, double thetaStdExponent, int visibleTags) { - final double translationStd = translationsStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / (visibleTags * visibleTags); - final double thetaStd = thetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / visibleTags; + private Matrix solvePNPAverageDistanceToStdDevs() { + final int numberOfVisibleTags = inputs.visibleTagIDs.length; + final double translationStd = translationsStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / (numberOfVisibleTags * numberOfVisibleTags); + final double thetaStd = thetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / numberOfVisibleTags; return VecBuilder.fill(translationStd, translationStd, thetaStd); } - private Matrix assumedHeadingAverageDistanceToStdDevs(double assumedRobotHeadingPose, int visibleTags) { - final double translationStd = assumedRobotHeadingPose * Math.pow(inputs.averageDistanceFromBestTag, 2) / (visibleTags * visibleTags); + private Matrix assumedHeadingAverageDistanceToStdDevs() { + final double translationStd = translationsStdExponent * inputs.distanceFromBestTag * inputs.distanceFromBestTag; final double thetaStd = Double.POSITIVE_INFINITY; return VecBuilder.fill(translationStd, translationStd, thetaStd); diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java index 0e735038..1b5e39a3 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java @@ -16,6 +16,6 @@ public static class RobotPoseSourceInputs { public double bestTargetRelativeYaw = 0; public double bestTargetRelativePitch = 0; public double averageDistanceFromAllTags = 0; - public double averageDistanceFromBestTag = 0; + public double distanceFromBestTag = 0; } } From ba639849f26f97897b5056ed523e5ae6c2a3dd73 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 10:09:14 -0400 Subject: [PATCH 011/261] Clean --- .../robotposesources/RobotPoseSource.java | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 21ff27d9..11f21370 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -20,7 +20,8 @@ public class RobotPoseSource { private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); private final Transform3d robotCenterToCamera; private final RobotPoseSourceIO robotPoseSourceIO; - private double lastUpdatedTimestamp, translationsStdExponent, thetaStdExponent; + private double lastUpdatedTimestamp; + private final double translationsStdExponent, thetaStdExponent; private Pose2d robotPose = null; public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double translationsStdExponent, double thetaStdExponent) { @@ -62,14 +63,6 @@ public String getName() { return name; } - public double getAverageDistanceFromTags() { - return inputs.averageDistanceFromAllTags; - } - - public int getNumberOfVisibleTags() { - return inputs.visibleTagIDs.length; - } - public double getLastResultTimestamp() { return inputs.lastResultTimestamp; } From 43905df4c2e8bf0491ef7dc130800ba14d442f0e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 11:10:38 -0400 Subject: [PATCH 012/261] Starting to understand sort of --- .../poseestimator/PoseEstimator6328.java | 4 ++ .../AprilTagPhotonCameraIO.java | 4 +- .../robotposesources/RobotPoseSource.java | 40 ++++++++++++++----- .../robotposesources/RobotPoseSourceIO.java | 4 +- 4 files changed, 37 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 6819d8ea..9eaff4b5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -145,6 +145,10 @@ public void addVisionObservation(VisionObservation observation) { estimatedPose = estimateAtTime.plus(scaledTransform).plus(sampleToOdometryTransform); } + public Pose2d getSampleHeading(double timeStamp) { + return poseBuffer.getSample(timeStamp).orElse(null); + } + /** * Reset estimated pose and odometry pose to pose
* Clear pose buffer diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index 61d1bbfe..c1592beb 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -37,8 +37,8 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Photo inputs.solvePNPPose = getSolvePNPPose(latestResult); inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitch = bestTargetRelativeRotation3d.getY(); - inputs.bestTargetRelativeYaw = bestTargetRelativeRotation3d.getZ(); + inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(latestResult); inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 11f21370..7654868a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -67,32 +67,40 @@ public double getLastResultTimestamp() { return inputs.lastResultTimestamp; } + + /** + * Calculates the standard deviations of the estimated robot pose. + * + * @return the standard deviations for the pose estimation strategy used + */ public Matrix calculateStdDevs() { - if (inputs.distanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) - return solvePNPAverageDistanceToStdDevs(); - return assumedHeadingAverageDistanceToStdDevs(); + if (isWithinTagRange()) + return calculateSolvePNPStdDevs(); + return calculateAssumedHeadingStdDevs(); } /** * If the robot is close enough to the tag, it gets the estimated solve PNP pose. * If it's too far, it assumes the robot's heading and calculates its position from there. + * Assuming the robot's heading is more robust, but using solve PNP resets the robot's heading. * * @param distanceFromBestTag the average of the distance from the best visible tag * @return the robot's pose */ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { - if (distanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS) + if (isWithinTagRange()) return cameraPoseToRobotPose(inputs.solvePNPPose); return calculateAssumedRobotHeadingPose().toPose2d(); } /** * Calculates the robot's pose by assuming its heading. + * This method of finding the robot's pose is more robust but relies on knowing the robot's heading beforehand. * * @return the robot's pose */ private Pose3d calculateAssumedRobotHeadingPose() { - final Rotation2d robotHeading = PoseEstimator6328.getInstance().getEstimatedPose().getRotation(); + final Rotation2d robotHeading = PoseEstimator6328.getInstance().getSampleHeading(lastUpdatedTimestamp).getRotation(); RobotContainer.SWERVE.getHeading(); final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeading)); @@ -108,10 +116,10 @@ private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { private Translation2d getTagToCamera(Pose3d tagPose) { final double cameraToTagDistanceMeters = -PhotonUtils.calculateDistanceToTargetMeters( - robotCenterToCamera.getZ(), tagPose.getZ(), robotCenterToCamera.getRotation().getY(), inputs.bestTargetRelativePitch + robotCenterToCamera.getZ(), tagPose.getZ(), robotCenterToCamera.getRotation().getY(), inputs.bestTargetRelativePitchRadians ); - final double cameraToTagXDistance = Math.sin(inputs.bestTargetRelativeYaw + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; - final double cameraToTagYDistance = -Math.cos(inputs.bestTargetRelativeYaw + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; + final double cameraToTagXDistance = Math.sin(inputs.bestTargetRelativeYawRadians + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; + final double cameraToTagYDistance = -Math.cos(inputs.bestTargetRelativeYawRadians + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; return new Translation2d(cameraToTagXDistance, cameraToTagYDistance); } @@ -141,18 +149,28 @@ private void logVisibleTags() { Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); } - private Matrix solvePNPAverageDistanceToStdDevs() { + private Matrix calculateSolvePNPStdDevs() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; - final double translationStd = translationsStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / (numberOfVisibleTags * numberOfVisibleTags); + final double translationStd = translationsStdExponent * inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags / numberOfVisibleTags; final double thetaStd = thetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / numberOfVisibleTags; return VecBuilder.fill(translationStd, translationStd, thetaStd); } - private Matrix assumedHeadingAverageDistanceToStdDevs() { + /** + * Calculates the standard deviations of the pose. + * The theta deviation is infinite because we assume the heading is correct. + * + * @return the standard deviations + */ + private Matrix calculateAssumedHeadingStdDevs() { final double translationStd = translationsStdExponent * inputs.distanceFromBestTag * inputs.distanceFromBestTag; final double thetaStd = Double.POSITIVE_INFINITY; return VecBuilder.fill(translationStd, translationStd, thetaStd); } + + private boolean isWithinTagRange() { + return inputs.distanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; + } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java index 1b5e39a3..75a8f409 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java @@ -13,8 +13,8 @@ public static class RobotPoseSourceInputs { public double lastResultTimestamp = 0; public Pose3d solvePNPPose = new Pose3d(); public int[] visibleTagIDs = new int[0]; - public double bestTargetRelativeYaw = 0; - public double bestTargetRelativePitch = 0; + public double bestTargetRelativeYawRadians = 0; + public double bestTargetRelativePitchRadians = 0; public double averageDistanceFromAllTags = 0; public double distanceFromBestTag = 0; } From 35774ab8d15bd7f1fd0bb6f968eefcaff428cdef Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 11:42:00 -0400 Subject: [PATCH 013/261] Removed useless line --- .../robot/poseestimation/robotposesources/RobotPoseSource.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 7654868a..40b42973 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.Robot; -import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; @@ -101,7 +100,6 @@ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { */ private Pose3d calculateAssumedRobotHeadingPose() { final Rotation2d robotHeading = PoseEstimator6328.getInstance().getSampleHeading(lastUpdatedTimestamp).getRotation(); - RobotContainer.SWERVE.getHeading(); final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeading)); } From 395776583787cb48801e2eb989cc5db650749039 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 11:45:00 -0400 Subject: [PATCH 014/261] Fixed javadoc --- .../robot/poseestimation/robotposesources/RobotPoseSource.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 40b42973..8f133c21 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -93,7 +93,7 @@ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { } /** - * Calculates the robot's pose by assuming its heading. + * Calculates the robot's pose by assuming its heading and the apriltag's position. * This method of finding the robot's pose is more robust but relies on knowing the robot's heading beforehand. * * @return the robot's pose From c36c94f5d2be0d4516392f382dab8fc16b8b1a9e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 12:54:46 +0300 Subject: [PATCH 015/261] Mostly fixed javadoc --- .../poseestimator/PoseEstimator.java | 2 +- .../poseestimator/PoseEstimator6328.java | 4 +- .../robotposesources/RobotPoseSource.java | 39 +++++++++---------- 3 files changed, 22 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 7989e895..0a9a7450 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -104,7 +104,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource robotPoseSource.update(); if (!robotPoseSource.hasNewResult()) return null; - final Pose2d robotPose = robotPoseSource.getCurrentRobotPose(); + final Pose2d robotPose = robotPoseSource.getEstimatedRobotPose(); if (robotPose == null) return null; diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 9eaff4b5..af9609c0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -145,8 +145,8 @@ public void addVisionObservation(VisionObservation observation) { estimatedPose = estimateAtTime.plus(scaledTransform).plus(sampleToOdometryTransform); } - public Pose2d getSampleHeading(double timeStamp) { - return poseBuffer.getSample(timeStamp).orElse(null); + public Pose2d getSamplePose(double timestamp) { + return poseBuffer.getSample(timestamp).orElse(null); } /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java index 8f133c21..ceb0fbd5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java @@ -20,13 +20,14 @@ public class RobotPoseSource { private final Transform3d robotCenterToCamera; private final RobotPoseSourceIO robotPoseSourceIO; private double lastUpdatedTimestamp; - private final double translationsStdExponent, thetaStdExponent; + private final double solvePNPTranslationsStdExponent, assumedRobotPoseTranslationsStdExponent, thetaStdExponent; private Pose2d robotPose = null; - public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double translationsStdExponent, double thetaStdExponent) { + public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double assumedRobotPoseTranslationsStdExponent, double thetaStdExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; - this.translationsStdExponent = translationsStdExponent; + this.solvePNPTranslationsStdExponent = solvePNPTranslationsStdExponent; + this.assumedRobotPoseTranslationsStdExponent = assumedRobotPoseTranslationsStdExponent; this.thetaStdExponent = thetaStdExponent; if (Robot.IS_REAL) @@ -51,10 +52,7 @@ public boolean hasNewResult() { return (inputs.hasResult && inputs.averageDistanceFromAllTags != 0) && isNewTimestamp(); } - /** - * @return the current estimated robot position - */ - public Pose2d getCurrentRobotPose() { + public Pose2d getEstimatedRobotPose() { return robotPose; } @@ -68,12 +66,12 @@ public double getLastResultTimestamp() { /** - * Calculates the standard deviations of the estimated robot pose. + * Calculates the range of how inaccurate the estimated pose could be * * @return the standard deviations for the pose estimation strategy used */ public Matrix calculateStdDevs() { - if (isWithinTagRange()) + if (isWithinBestTagRangeForSolvePNP()) return calculateSolvePNPStdDevs(); return calculateAssumedHeadingStdDevs(); } @@ -81,27 +79,28 @@ public Matrix calculateStdDevs() { /** * If the robot is close enough to the tag, it gets the estimated solve PNP pose. * If it's too far, it assumes the robot's heading and calculates its position from there. - * Assuming the robot's heading is more robust, but using solve PNP resets the robot's heading. + * Assuming the robot's heading is more robust, but won't fix current wrong heading. + * To fix this, we use solve PNP to reset the robot's heading, when we're close enough for an accurate result. * * @param distanceFromBestTag the average of the distance from the best visible tag * @return the robot's pose */ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { - if (isWithinTagRange()) + if (isWithinBestTagRangeForSolvePNP()) return cameraPoseToRobotPose(inputs.solvePNPPose); return calculateAssumedRobotHeadingPose().toPose2d(); } /** - * Calculates the robot's pose by assuming its heading and the apriltag's position. + * Calculates the robot's pose by assuming its heading, the apriltag's position, and the camera's position on the robot. * This method of finding the robot's pose is more robust but relies on knowing the robot's heading beforehand. * * @return the robot's pose */ private Pose3d calculateAssumedRobotHeadingPose() { - final Rotation2d robotHeading = PoseEstimator6328.getInstance().getSampleHeading(lastUpdatedTimestamp).getRotation(); - final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); - return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeading)); + final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().getSamplePose(inputs.lastResultTimestamp).getRotation(); + final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeadingAtResultTimestamp); + return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeadingAtResultTimestamp)); } private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { @@ -149,26 +148,26 @@ private void logVisibleTags() { private Matrix calculateSolvePNPStdDevs() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; - final double translationStd = translationsStdExponent * inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags / numberOfVisibleTags; + final double translationStd = solvePNPTranslationsStdExponent * inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags / numberOfVisibleTags; final double thetaStd = thetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / numberOfVisibleTags; return VecBuilder.fill(translationStd, translationStd, thetaStd); } /** - * Calculates the standard deviations of the pose. - * The theta deviation is infinite because we assume the heading is correct. + * Calculates the standard deviations of the pose, for when the pose is calculated by assuming the robot's heading. + * The theta deviation is infinite so that it won't change anything in the heading because we assume it is correct. * * @return the standard deviations */ private Matrix calculateAssumedHeadingStdDevs() { - final double translationStd = translationsStdExponent * inputs.distanceFromBestTag * inputs.distanceFromBestTag; + final double translationStd = assumedRobotPoseTranslationsStdExponent * inputs.distanceFromBestTag * inputs.distanceFromBestTag; final double thetaStd = Double.POSITIVE_INFINITY; return VecBuilder.fill(translationStd, translationStd, thetaStd); } - private boolean isWithinTagRange() { + private boolean isWithinBestTagRangeForSolvePNP() { return inputs.distanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; } } From 6dc784f6febb9f88e22cb749c5367d7b17a15233 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 13:33:40 +0300 Subject: [PATCH 016/261] Renamed stuff --- .../poseestimator/PoseEstimator.java | 30 +++++++++---------- .../poseestimator/PoseEstimator6328.java | 2 +- ...botPoseSource.java => AprilTagCamera.java} | 26 ++++++++-------- ...ants.java => AprilTagCameraConstants.java} | 6 ++-- ...oseSourceIO.java => AprilTagCameraIO.java} | 2 +- .../robotposesources/AprilTagLimelightIO.java | 2 +- .../AprilTagPhotonCameraIO.java | 2 +- 7 files changed, 35 insertions(+), 35 deletions(-) rename src/main/java/frc/trigon/robot/poseestimation/robotposesources/{RobotPoseSource.java => AprilTagCamera.java} (87%) rename src/main/java/frc/trigon/robot/poseestimation/robotposesources/{RobotPoseSourceConstants.java => AprilTagCameraConstants.java} (87%) rename src/main/java/frc/trigon/robot/poseestimation/robotposesources/{RobotPoseSourceIO.java => AprilTagCameraIO.java} (95%) diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 0a9a7450..4c89f069 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.poseestimation.robotposesources.AprilTagCamera; +import frc.trigon.robot.poseestimation.robotposesources.AprilTagCameraConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -26,16 +26,16 @@ */ public class PoseEstimator implements AutoCloseable { private final Field2d field = new Field2d(); - private final RobotPoseSource[] robotPoseSources; + private final AprilTagCamera[] robotPoseSources; private final PoseEstimator6328 poseEstimator6328 = PoseEstimator6328.getInstance(); /** * Constructs a new PoseEstimator. * - * @param robotPoseSources the sources that should update the pose estimator apart from the odometry. This should be cameras etc. + * @param aprilTagCameras the sources that should update the pose estimator apart from the odometry. This should be cameras etc. */ - public PoseEstimator(RobotPoseSource... robotPoseSources) { - this.robotPoseSources = robotPoseSources; + public PoseEstimator(AprilTagCamera... aprilTagCameras) { + this.robotPoseSources = aprilTagCameras; putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); PathPlannerLogging.setLogActivePathCallback((pose) -> { @@ -92,26 +92,26 @@ private void updateFromVision() { private List getViableVisionObservations() { List viableVisionObservations = new ArrayList<>(); - for (RobotPoseSource robotPoseSource : robotPoseSources) { - final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(robotPoseSource); + for (AprilTagCamera aprilTagCamera : robotPoseSources) { + final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(aprilTagCamera); if (visionObservation != null) viableVisionObservations.add(visionObservation); } return viableVisionObservations; } - private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource robotPoseSource) { - robotPoseSource.update(); - if (!robotPoseSource.hasNewResult()) + private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera aprilTagCamera) { + aprilTagCamera.update(); + if (!aprilTagCamera.hasNewResult()) return null; - final Pose2d robotPose = robotPoseSource.getEstimatedRobotPose(); + final Pose2d robotPose = aprilTagCamera.getEstimatedRobotPose(); if (robotPose == null) return null; return new PoseEstimator6328.VisionObservation( robotPose, - robotPoseSource.getLastResultTimestamp(), - robotPoseSource.calculateStdDevs() + aprilTagCamera.getLastResultTimestamp(), + aprilTagCamera.calculateStdDevs() ); } @@ -123,7 +123,7 @@ private Matrix averageDistanceToStdDevs(double averageDistance, int visi } private void putAprilTagsOnFieldWidget() { - for (Map.Entry entry : RobotPoseSourceConstants.TAG_ID_TO_POSE.entrySet()) { + for (Map.Entry entry : AprilTagCameraConstants.TAG_ID_TO_POSE.entrySet()) { final Pose2d tagPose = entry.getValue().toPose2d(); field.getObject("Tag " + entry.getKey()).setPose(tagPose); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index af9609c0..1ec10814 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -145,7 +145,7 @@ public void addVisionObservation(VisionObservation observation) { estimatedPose = estimateAtTime.plus(scaledTransform).plus(sampleToOdometryTransform); } - public Pose2d getSamplePose(double timestamp) { + public Pose2d samplePose(double timestamp) { return poseBuffer.getSample(timestamp).orElse(null); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCamera.java similarity index 87% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCamera.java index ceb0fbd5..ccf42ad3 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCamera.java @@ -14,36 +14,36 @@ /** * A pose source is a class that provides the robot's pose, from a camera. */ -public class RobotPoseSource { +public class AprilTagCamera { protected final String name; private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); private final Transform3d robotCenterToCamera; - private final RobotPoseSourceIO robotPoseSourceIO; + private final AprilTagCameraIO aprilTagCameraIO; private double lastUpdatedTimestamp; - private final double solvePNPTranslationsStdExponent, assumedRobotPoseTranslationsStdExponent, thetaStdExponent; + private final double solvePNPTranslationsStdExponent, assumedRobotPoseTranslationsStdExponent, solvePNPThetaStdExponent; private Pose2d robotPose = null; - public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double assumedRobotPoseTranslationsStdExponent, double thetaStdExponent) { + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double assumedRobotPoseTranslationsStdExponent, double solvePNPThetaStdExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; this.solvePNPTranslationsStdExponent = solvePNPTranslationsStdExponent; this.assumedRobotPoseTranslationsStdExponent = assumedRobotPoseTranslationsStdExponent; - this.thetaStdExponent = thetaStdExponent; + this.solvePNPThetaStdExponent = solvePNPThetaStdExponent; if (Robot.IS_REAL) - robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name); + aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name); else - robotPoseSourceIO = new RobotPoseSourceIO(); + aprilTagCameraIO = new AprilTagCameraIO(); } public void update() { - robotPoseSourceIO.updateInputs(inputs); + aprilTagCameraIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); robotPose = calculateBestRobotPose(inputs.distanceFromBestTag); if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) - Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); + Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); else Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); } @@ -66,7 +66,7 @@ public double getLastResultTimestamp() { /** - * Calculates the range of how inaccurate the estimated pose could be + * Calculates the range of how inaccurate the estimated pose could be. * * @return the standard deviations for the pose estimation strategy used */ @@ -98,7 +98,7 @@ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { * @return the robot's pose */ private Pose3d calculateAssumedRobotHeadingPose() { - final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().getSamplePose(inputs.lastResultTimestamp).getRotation(); + final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.lastResultTimestamp).getRotation(); final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeadingAtResultTimestamp); return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeadingAtResultTimestamp)); } @@ -149,7 +149,7 @@ private void logVisibleTags() { private Matrix calculateSolvePNPStdDevs() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; final double translationStd = solvePNPTranslationsStdExponent * inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags / numberOfVisibleTags; - final double thetaStd = thetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / numberOfVisibleTags; + final double thetaStd = solvePNPThetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / numberOfVisibleTags; return VecBuilder.fill(translationStd, translationStd, thetaStd); } @@ -168,6 +168,6 @@ private Matrix calculateAssumedHeadingStdDevs() { } private boolean isWithinBestTagRangeForSolvePNP() { - return inputs.distanceFromBestTag < RobotPoseSourceConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; + return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraConstants.java similarity index 87% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraConstants.java index a57e2b0b..bb64c655 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraConstants.java @@ -10,7 +10,7 @@ import java.util.HashMap; import java.util.function.Function; -public class RobotPoseSourceConstants { +public class AprilTagCameraConstants { public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); static final PhotonPoseEstimator.PoseStrategy PRIMARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, @@ -30,9 +30,9 @@ public enum RobotPoseSourceType { PHOTON_CAMERA(AprilTagPhotonCameraIO::new), LIMELIGHT(AprilTagLimelightIO::new); - final Function createIOFunction; + final Function createIOFunction; - RobotPoseSourceType(Function createIOFunction) { + RobotPoseSourceType(Function createIOFunction) { this.createIOFunction = createIOFunction; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraIO.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraIO.java index 75a8f409..0a0167e0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraIO.java @@ -3,7 +3,7 @@ import edu.wpi.first.math.geometry.Pose3d; import org.littletonrobotics.junction.AutoLog; -public class RobotPoseSourceIO { +public class AprilTagCameraIO { protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java index 49d97050..18e8173e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java @@ -2,7 +2,7 @@ import org.trigon.utilities.LimelightHelpers; -public class AprilTagLimelightIO extends RobotPoseSourceIO { +public class AprilTagLimelightIO extends AprilTagCameraIO { private final String hostname; protected AprilTagLimelightIO(String hostname) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java index c1592beb..03a0d018 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java @@ -14,7 +14,7 @@ import java.util.List; -public class AprilTagPhotonCameraIO extends RobotPoseSourceIO { +public class AprilTagPhotonCameraIO extends AprilTagCameraIO { private final PhotonCamera photonCamera; protected AprilTagPhotonCameraIO(String cameraName) { From 13235d1ed5e90d4ec82d62947e5c1c5af689c7a5 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 13:36:11 +0300 Subject: [PATCH 017/261] Renamed package --- .../AprilTagCamera.java | 6 +++--- .../AprilTagCameraConstants.java | 2 +- .../AprilTagCameraIO.java | 2 +- .../AprilTagLimelightIO.java | 2 +- .../AprilTagPhotonCameraIO.java | 2 +- .../robot/poseestimation/poseestimator/PoseEstimator.java | 4 ++-- 6 files changed, 9 insertions(+), 9 deletions(-) rename src/main/java/frc/trigon/robot/poseestimation/{robotposesources => apriltagcamera}/AprilTagCamera.java (96%) rename src/main/java/frc/trigon/robot/poseestimation/{robotposesources => apriltagcamera}/AprilTagCameraConstants.java (96%) rename src/main/java/frc/trigon/robot/poseestimation/{robotposesources => apriltagcamera}/AprilTagCameraIO.java (92%) rename src/main/java/frc/trigon/robot/poseestimation/{robotposesources => apriltagcamera}/AprilTagLimelightIO.java (92%) rename src/main/java/frc/trigon/robot/poseestimation/{robotposesources => apriltagcamera}/AprilTagPhotonCameraIO.java (98%) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java similarity index 96% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCamera.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index ccf42ad3..ff41ef5b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.robotposesources; +package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -148,8 +148,8 @@ private void logVisibleTags() { private Matrix calculateSolvePNPStdDevs() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; - final double translationStd = solvePNPTranslationsStdExponent * inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags / numberOfVisibleTags; - final double thetaStd = solvePNPThetaStdExponent * Math.pow(inputs.averageDistanceFromAllTags, 2) / numberOfVisibleTags; + final double translationStd = solvePNPTranslationsStdExponent * (inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags) / numberOfVisibleTags; + final double thetaStd = solvePNPThetaStdExponent * (inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags) / numberOfVisibleTags; return VecBuilder.fill(translationStd, translationStd, thetaStd); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java similarity index 96% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index bb64c655..f9a9d272 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.robotposesources; +package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java similarity index 92% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index 0a0167e0..0a9daa18 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.robotposesources; +package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose3d; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagLimelightIO.java similarity index 92% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagLimelightIO.java index 18e8173e..94ae1821 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagLimelightIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.robotposesources; +package frc.trigon.robot.poseestimation.apriltagcamera; import org.trigon.utilities.LimelightHelpers; diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagPhotonCameraIO.java similarity index 98% rename from src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagPhotonCameraIO.java index 03a0d018..e9ed63dd 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagPhotonCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.robotposesources; +package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 4c89f069..63fb7704 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.robotposesources.AprilTagCamera; -import frc.trigon.robot.poseestimation.robotposesources.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; From a00b910c28d0ab602525277546f9de9ee73bed64 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 14:28:27 +0300 Subject: [PATCH 018/261] Added io folder and some javadoc --- .../apriltagcamera/AprilTagCamera.java | 25 ++++++++++++++----- .../AprilTagCameraConstants.java | 2 ++ .../{ => io}/AprilTagLimelightIO.java | 4 ++- .../{ => io}/AprilTagPhotonCameraIO.java | 4 ++- 4 files changed, 27 insertions(+), 8 deletions(-) rename src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/{ => io}/AprilTagLimelightIO.java (76%) rename src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/{ => io}/AprilTagPhotonCameraIO.java (96%) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index ff41ef5b..18392c69 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -12,7 +12,7 @@ import org.photonvision.PhotonUtils; /** - * A pose source is a class that provides the robot's pose, from a camera. + * An april tag camera is a class that provides the robot's pose, from a camera. */ public class AprilTagCamera { protected final String name; @@ -23,12 +23,20 @@ public class AprilTagCamera { private final double solvePNPTranslationsStdExponent, assumedRobotPoseTranslationsStdExponent, solvePNPThetaStdExponent; private Pose2d robotPose = null; - public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double assumedRobotPoseTranslationsStdExponent, double solvePNPThetaStdExponent) { + /** + * @param robotPoseSourceType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param solvePNPTranslationsStdExponent + * @param solvePNPThetaStdExponent + * @param assumedRobotPoseTranslationsStdExponent + */ + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double solvePNPThetaStdExponent, double assumedRobotPoseTranslationsStdExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; this.solvePNPTranslationsStdExponent = solvePNPTranslationsStdExponent; - this.assumedRobotPoseTranslationsStdExponent = assumedRobotPoseTranslationsStdExponent; this.solvePNPThetaStdExponent = solvePNPThetaStdExponent; + this.assumedRobotPoseTranslationsStdExponent = assumedRobotPoseTranslationsStdExponent; if (Robot.IS_REAL) aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name); @@ -148,8 +156,8 @@ private void logVisibleTags() { private Matrix calculateSolvePNPStdDevs() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; - final double translationStd = solvePNPTranslationsStdExponent * (inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags) / numberOfVisibleTags; - final double thetaStd = solvePNPThetaStdExponent * (inputs.averageDistanceFromAllTags * inputs.averageDistanceFromAllTags) / numberOfVisibleTags; + final double translationStd = calculateStd(solvePNPTranslationsStdExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); + final double thetaStd = calculateStd(solvePNPThetaStdExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); return VecBuilder.fill(translationStd, translationStd, thetaStd); } @@ -161,12 +169,17 @@ private Matrix calculateSolvePNPStdDevs() { * @return the standard deviations */ private Matrix calculateAssumedHeadingStdDevs() { - final double translationStd = assumedRobotPoseTranslationsStdExponent * inputs.distanceFromBestTag * inputs.distanceFromBestTag; + final double translationStd = calculateStd(assumedRobotPoseTranslationsStdExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); final double thetaStd = Double.POSITIVE_INFINITY; return VecBuilder.fill(translationStd, translationStd, thetaStd); } + private double calculateStd(double exponent, double distance, int visibleTags) { + return exponent * (distance * distance) * visibleTags; + + } + private boolean isWithinBestTagRangeForSolvePNP() { return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index f9a9d272..2ded1c80 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -5,6 +5,8 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; import java.util.HashMap; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java similarity index 76% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagLimelightIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 94ae1821..7501954e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,5 +1,7 @@ -package frc.trigon.robot.poseestimation.apriltagcamera; +package frc.trigon.robot.poseestimation.apriltagcamera.io; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; import org.trigon.utilities.LimelightHelpers; public class AprilTagLimelightIO extends AprilTagCameraIO { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java similarity index 96% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagPhotonCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index e9ed63dd..44d97ee8 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.apriltagcamera; +package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; @@ -7,6 +7,8 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; From f4f52de7c2c85a6316da231a78c0020a1ee71384 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 14:36:06 +0300 Subject: [PATCH 019/261] fixed errors and javadoc --- .../apriltagcamera/AprilTagCamera.java | 16 ++++++++-------- .../apriltagcamera/io/AprilTagLimelightIO.java | 2 +- .../io/AprilTagPhotonCameraIO.java | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 18392c69..85f7e8dc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -24,19 +24,19 @@ public class AprilTagCamera { private Pose2d robotPose = null; /** - * @param robotPoseSourceType the type of camera - * @param name the camera's name - * @param robotCenterToCamera the transform of the robot's origin point to the camera - * @param solvePNPTranslationsStdExponent - * @param solvePNPThetaStdExponent - * @param assumedRobotPoseTranslationsStdExponent + * @param robotPoseSourceType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param solvePNPTranslationsStdExponent the translation exponent for solve PNP + * @param solvePNPThetaStdExponent the theta exponent for solve PNP + * @param assumedRobotHeadingTranslationsStdExponent the translation exponent for the assumed robot heading pose calculation */ - public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double solvePNPThetaStdExponent, double assumedRobotPoseTranslationsStdExponent) { + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double solvePNPThetaStdExponent, double assumedRobotHeadingTranslationsStdExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; this.solvePNPTranslationsStdExponent = solvePNPTranslationsStdExponent; this.solvePNPThetaStdExponent = solvePNPThetaStdExponent; - this.assumedRobotPoseTranslationsStdExponent = assumedRobotPoseTranslationsStdExponent; + this.assumedRobotPoseTranslationsStdExponent = assumedRobotHeadingTranslationsStdExponent; if (Robot.IS_REAL) aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 7501954e..4e3187fb 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -7,7 +7,7 @@ public class AprilTagLimelightIO extends AprilTagCameraIO { private final String hostname; - protected AprilTagLimelightIO(String hostname) { + public AprilTagLimelightIO(String hostname) { this.hostname = hostname; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 44d97ee8..91667292 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -19,7 +19,7 @@ public class AprilTagPhotonCameraIO extends AprilTagCameraIO { private final PhotonCamera photonCamera; - protected AprilTagPhotonCameraIO(String cameraName) { + public AprilTagPhotonCameraIO(String cameraName) { photonCamera = new PhotonCamera(cameraName); } From ac7b82a4418d318b443817fd4fefd1266db208b4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 15:13:58 +0300 Subject: [PATCH 020/261] Renaming + javadoc --- .../apriltagcamera/AprilTagCamera.java | 61 +++++++++++-------- .../io/AprilTagLimelightIO.java | 2 +- .../io/AprilTagPhotonCameraIO.java | 4 +- .../poseestimator/PoseEstimator.java | 2 +- 4 files changed, 39 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 85f7e8dc..cd556bc3 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -13,6 +13,7 @@ /** * An april tag camera is a class that provides the robot's pose, from a camera. + * An april tag is like a 2D barcode */ public class AprilTagCamera { protected final String name; @@ -20,23 +21,23 @@ public class AprilTagCamera { private final Transform3d robotCenterToCamera; private final AprilTagCameraIO aprilTagCameraIO; private double lastUpdatedTimestamp; - private final double solvePNPTranslationsStdExponent, assumedRobotPoseTranslationsStdExponent, solvePNPThetaStdExponent; + private final double solvePNPTranslationsStandardDeviationsExponent, assumedRobotPoseTranslationsStandardDeviationsExponent, solvePNPThetaStandardDeviationsExponent; private Pose2d robotPose = null; /** - * @param robotPoseSourceType the type of camera - * @param name the camera's name - * @param robotCenterToCamera the transform of the robot's origin point to the camera - * @param solvePNPTranslationsStdExponent the translation exponent for solve PNP - * @param solvePNPThetaStdExponent the theta exponent for solve PNP - * @param assumedRobotHeadingTranslationsStdExponent the translation exponent for the assumed robot heading pose calculation + * @param robotPoseSourceType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param solvePNPTranslationsStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP + * @param solvePNPThetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP + * @param assumedRobotHeadingTranslationsStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when getting the pose by assuming the robot's heading */ - public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStdExponent, double solvePNPThetaStdExponent, double assumedRobotHeadingTranslationsStdExponent) { + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStandardDeviationsExponent, double solvePNPThetaStandardDeviationsExponent, double assumedRobotHeadingTranslationsStandardDeviationsExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; - this.solvePNPTranslationsStdExponent = solvePNPTranslationsStdExponent; - this.solvePNPThetaStdExponent = solvePNPThetaStdExponent; - this.assumedRobotPoseTranslationsStdExponent = assumedRobotHeadingTranslationsStdExponent; + this.solvePNPTranslationsStandardDeviationsExponent = solvePNPTranslationsStandardDeviationsExponent; + this.solvePNPThetaStandardDeviationsExponent = solvePNPThetaStandardDeviationsExponent; + this.assumedRobotPoseTranslationsStandardDeviationsExponent = assumedRobotHeadingTranslationsStandardDeviationsExponent; if (Robot.IS_REAL) aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name); @@ -74,14 +75,14 @@ public double getLastResultTimestamp() { /** - * Calculates the range of how inaccurate the estimated pose could be. + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. * * @return the standard deviations for the pose estimation strategy used */ - public Matrix calculateStdDevs() { + public Matrix calculateStandardDeviations() { if (isWithinBestTagRangeForSolvePNP()) - return calculateSolvePNPStdDevs(); - return calculateAssumedHeadingStdDevs(); + return calculateSolvePNPStandardDeviations(); + return calculateAssumedHeadingStandardDeviations(); } /** @@ -154,12 +155,12 @@ private void logVisibleTags() { Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); } - private Matrix calculateSolvePNPStdDevs() { + private Matrix calculateSolvePNPStandardDeviations() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; - final double translationStd = calculateStd(solvePNPTranslationsStdExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); - final double thetaStd = calculateStd(solvePNPThetaStdExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); + final double translationStandardDeviation = calculateStandardDeviations(solvePNPTranslationsStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); + final double thetaStandardDeviation = calculateStandardDeviations(solvePNPThetaStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); - return VecBuilder.fill(translationStd, translationStd, thetaStd); + return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); } /** @@ -168,16 +169,24 @@ private Matrix calculateSolvePNPStdDevs() { * * @return the standard deviations */ - private Matrix calculateAssumedHeadingStdDevs() { - final double translationStd = calculateStd(assumedRobotPoseTranslationsStdExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); - final double thetaStd = Double.POSITIVE_INFINITY; + private Matrix calculateAssumedHeadingStandardDeviations() { + final double translationStandardDeviation = calculateStandardDeviations(assumedRobotPoseTranslationsStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); + final double thetaStandardDeviation = Double.POSITIVE_INFINITY; - return VecBuilder.fill(translationStd, translationStd, thetaStd); + return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); } - private double calculateStd(double exponent, double distance, int visibleTags) { - return exponent * (distance * distance) * visibleTags; - + /** + * Calculates the standard deviations of the estimated pose using a formula + * The farther it is from the tags, the less accurate the result and the more tags, the more accurate the result. + * + * @param exponent a calibrated gain, different for each pose estimating strategy + * @param distance the distance from the tag(s) + * @param numberOfVisibleTags the number of visible tags + * @return the standard deviation + */ + private double calculateStandardDeviations(double exponent, double distance, int numberOfVisibleTags) { + return exponent * (distance * distance) / numberOfVisibleTags; } private boolean isWithinBestTagRangeForSolvePNP() { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 4e3187fb..729de8e2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -12,7 +12,7 @@ public AprilTagLimelightIO(String hostname) { } @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { + public void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.hasResult = LimelightHelpers.getTV(hostname); if (inputs.hasResult) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 91667292..287bb110 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -51,7 +51,7 @@ private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.solvePNPPose = new Pose3d(); } - private static Point getTagCenter(List tagCorners) { + private Point getTagCenter(List tagCorners) { double tagCornerSumX = 0; double tagCornerSumY = 0; for (TargetCorner tagCorner : tagCorners) { @@ -123,7 +123,7 @@ private double getDistanceFromBestTag(PhotonPipelineResult result) { return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); } - private static Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { + private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { double fx = camIntrinsics.get(0, 0); double cx = camIntrinsics.get(0, 2); double xOffset = cx - pixel.x; diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 63fb7704..a2c37bb9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -111,7 +111,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera return new PoseEstimator6328.VisionObservation( robotPose, aprilTagCamera.getLastResultTimestamp(), - aprilTagCamera.calculateStdDevs() + aprilTagCamera.calculateStandardDeviations() ); } From 6bd1ea5794a04b038357103f7485b6d4f317553f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 15:24:46 +0300 Subject: [PATCH 021/261] Fixed javadoc --- .../robot/poseestimation/apriltagcamera/AprilTagCamera.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index cd556bc3..c47e30f1 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -13,7 +13,8 @@ /** * An april tag camera is a class that provides the robot's pose, from a camera. - * An april tag is like a 2D barcode + * An april tag is like a 2D barcode used to find the robot's position on the field. + * By using the angle and size of the tag that the camera sees, it can estimate its position. */ public class AprilTagCamera { protected final String name; @@ -25,6 +26,8 @@ public class AprilTagCamera { private Pose2d robotPose = null; /** + * Constructs a new AprilTagCamera. + * * @param robotPoseSourceType the type of camera * @param name the camera's name * @param robotCenterToCamera the transform of the robot's origin point to the camera @@ -76,6 +79,7 @@ public double getLastResultTimestamp() { /** * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. + * Different pose estimation strategies may use different formulae to calculate the standard deviations. * * @return the standard deviations for the pose estimation strategy used */ From 0710984a6cdb72dff5e764262c84f03291455a21 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 16:17:01 +0300 Subject: [PATCH 022/261] Just tell me what I need to change it to, honestly --- .../io/AprilTagLimelightIO.java | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 729de8e2..91f2b3db 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,5 +1,6 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; +import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; import org.trigon.utilities.LimelightHelpers; @@ -18,6 +19,31 @@ public void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; + inputs.visibleTagIDs = getVisibleTagIDs(results); + inputs.bestTargetRelativeYawRadians = results.getBotPose3d_wpiBlue().getRotation().getZ(); + inputs.bestTargetRelativePitchRadians = results.getBotPose3d_wpiBlue().getRotation().getY(); + inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results); + inputs.distanceFromBestTag = getDistanceFromBestTag(results); } } -} + + private int[] getVisibleTagIDs(LimelightHelpers.Results results) { + LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; + int[] visibleTagIDs = new int[visibleTags.length]; + for (int i = 0; i < visibleTagIDs.length; i++) + visibleTagIDs[i] = (int) visibleTags[i].fiducialID; + return visibleTagIDs; + } + + private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { + int[] visibleTags = getVisibleTagIDs(results); + double totalDistanceFromTags = 0; + for (int i = 0; i < visibleTags.length; i++) + totalDistanceFromTags += FieldConstants.TAG_ID_TO_POSE.get(i).getTranslation().getDistance(results.getBotPose3d_wpiBlue().getTranslation()); + return totalDistanceFromTags /= 4; + } + + private double getDistanceFromBestTag(LimelightHelpers.Results results) { + return FieldConstants.TAG_ID_TO_POSE.get(getVisibleTagIDs(results)[0]).getTranslation().getDistance(results.getBotPose3d_wpiBlue().getTranslation()); + } +} \ No newline at end of file From a362a9adfce641b7ae2b077052a08b5e8f18bb0f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 17:23:26 +0300 Subject: [PATCH 023/261] Fixed javadoc (still working on commented out lines) --- .../apriltagcamera/AprilTagCamera.java | 10 ++-- .../io/AprilTagLimelightIO.java | 51 ++++++++++++------- 2 files changed, 38 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c47e30f1..da1103bf 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -12,9 +12,9 @@ import org.photonvision.PhotonUtils; /** - * An april tag camera is a class that provides the robot's pose, from a camera. + * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. * An april tag is like a 2D barcode used to find the robot's position on the field. - * By using the angle and size of the tag that the camera sees, it can estimate its position. + * Since the tag's position on the field is known, we can calculate our position relative to it, therefore estimating our position on the field. */ public class AprilTagCamera { protected final String name; @@ -181,8 +181,8 @@ private Matrix calculateAssumedHeadingStandardDeviations() { } /** - * Calculates the standard deviations of the estimated pose using a formula - * The farther it is from the tags, the less accurate the result and the more tags, the more accurate the result. + * Calculates the standard deviation of the estimated pose using a formula. + * As we get further from the tag(s), this will return a less trusting (higher deviation) result. * * @param exponent a calibrated gain, different for each pose estimating strategy * @param distance the distance from the tag(s) @@ -196,4 +196,4 @@ private double calculateStandardDeviations(double exponent, double distance, int private boolean isWithinBestTagRangeForSolvePNP() { return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 91f2b3db..fb5b9aeb 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,5 +1,6 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; +import edu.wpi.first.math.geometry.Pose3d; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; @@ -13,37 +14,51 @@ public AprilTagLimelightIO(String hostname) { } @Override - public void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { + protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.hasResult = LimelightHelpers.getTV(hostname); - if (inputs.hasResult) { - final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; - inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); - inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; - inputs.visibleTagIDs = getVisibleTagIDs(results); - inputs.bestTargetRelativeYawRadians = results.getBotPose3d_wpiBlue().getRotation().getZ(); - inputs.bestTargetRelativePitchRadians = results.getBotPose3d_wpiBlue().getRotation().getY(); - inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results); - inputs.distanceFromBestTag = getDistanceFromBestTag(results); - } + + if (inputs.hasResult) + updateHasResultInputs(inputs); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { + final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; + inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); + inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; + inputs.visibleTagIDs = getVisibleTagIDs(results); +// inputs.bestTargetRelativeYawRadians =; +// inputs.bestTargetRelativePitchRadians =; + inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results); + inputs.distanceFromBestTag = getDistanceFromBestTag(results); + } + + private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[0]; + inputs.solvePNPPose = new Pose3d(); } private int[] getVisibleTagIDs(LimelightHelpers.Results results) { - LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; - int[] visibleTagIDs = new int[visibleTags.length]; + final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; + final int[] visibleTagIDs = new int[visibleTags.length]; for (int i = 0; i < visibleTagIDs.length; i++) visibleTagIDs[i] = (int) visibleTags[i].fiducialID; return visibleTagIDs; } private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { - int[] visibleTags = getVisibleTagIDs(results); double totalDistanceFromTags = 0; - for (int i = 0; i < visibleTags.length; i++) - totalDistanceFromTags += FieldConstants.TAG_ID_TO_POSE.get(i).getTranslation().getDistance(results.getBotPose3d_wpiBlue().getTranslation()); - return totalDistanceFromTags /= 4; + for (int i = 0; i < results.targets_Fiducials.length; i++) + totalDistanceFromTags += getDistanceFromTag(i, results.getBotPose3d_wpiBlue()); + return totalDistanceFromTags /= results.targets_Fiducials.length; } private double getDistanceFromBestTag(LimelightHelpers.Results results) { - return FieldConstants.TAG_ID_TO_POSE.get(getVisibleTagIDs(results)[0]).getTranslation().getDistance(results.getBotPose3d_wpiBlue().getTranslation()); + return getDistanceFromTag((int) results.targets_Fiducials[0].fiducialID, results.getBotPose3d_wpiBlue()); + } + + private double getDistanceFromTag(int fiducialID, Pose3d robotPose) { + return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(robotPose.getTranslation()); } } \ No newline at end of file From 7df20a511573b86e9baefe42cf894db751d0a632 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 17:33:17 +0300 Subject: [PATCH 024/261] I doubt this is right --- .../apriltagcamera/io/AprilTagLimelightIO.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index fb5b9aeb..a268735a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,6 +1,7 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; @@ -28,8 +29,8 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; inputs.visibleTagIDs = getVisibleTagIDs(results); -// inputs.bestTargetRelativeYawRadians =; -// inputs.bestTargetRelativePitchRadians =; + inputs.bestTargetRelativeYawRadians = getBestTargetRelativeRotation(results).getZ(); + inputs.bestTargetRelativePitchRadians = getBestTargetRelativeRotation(results).getY(); inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results); inputs.distanceFromBestTag = getDistanceFromBestTag(results); } @@ -47,6 +48,11 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { return visibleTagIDs; } + private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results results) { + int fiducialID = (int) LimelightHelpers.getFiducialID(hostname); + return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getRotation(); + } + private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { double totalDistanceFromTags = 0; for (int i = 0; i < results.targets_Fiducials.length; i++) @@ -55,7 +61,7 @@ private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { } private double getDistanceFromBestTag(LimelightHelpers.Results results) { - return getDistanceFromTag((int) results.targets_Fiducials[0].fiducialID, results.getBotPose3d_wpiBlue()); + return getDistanceFromTag((int) LimelightHelpers.getFiducialID(hostname), results.getBotPose3d_wpiBlue()); } private double getDistanceFromTag(int fiducialID, Pose3d robotPose) { From 58b61c97ef3e99ede44b42284ed9638598f00398 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 17:57:42 +0300 Subject: [PATCH 025/261] A bunch of small things --- .../apriltagcamera/AprilTagCamera.java | 14 +++++++++----- .../apriltagcamera/io/AprilTagLimelightIO.java | 9 +++++---- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index da1103bf..31ff30e9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -51,13 +51,10 @@ public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourc public void update() { aprilTagCameraIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); + logVisibleTags(); robotPose = calculateBestRobotPose(inputs.distanceFromBestTag); - - if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) - Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); - else - Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); + logEstimatedRobotPose(); } public boolean hasNewResult() { @@ -196,4 +193,11 @@ private double calculateStandardDeviations(double exponent, double distance, int private boolean isWithinBestTagRangeForSolvePNP() { return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; } + + private void logEstimatedRobotPose() { + if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) + Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); + else + Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index a268735a..3259a3c6 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -49,10 +49,11 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { } private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results results) { - int fiducialID = (int) LimelightHelpers.getFiducialID(hostname); + final int fiducialID = (int) results.targets_Fiducials[0].fiducialID; return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getRotation(); } + private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { double totalDistanceFromTags = 0; for (int i = 0; i < results.targets_Fiducials.length; i++) @@ -61,10 +62,10 @@ private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { } private double getDistanceFromBestTag(LimelightHelpers.Results results) { - return getDistanceFromTag((int) LimelightHelpers.getFiducialID(hostname), results.getBotPose3d_wpiBlue()); + return getDistanceFromTag((int) results.targets_Fiducials[0].fiducialID, results.getBotPose3d_wpiBlue()); } - private double getDistanceFromTag(int fiducialID, Pose3d robotPose) { - return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(robotPose.getTranslation()); + private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) { + return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation()); } } \ No newline at end of file From a0804613de77242f72a8952c6efb9bb320932f6f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 18:02:15 +0300 Subject: [PATCH 026/261] Fixed for loop problem --- .../apriltagcamera/io/AprilTagLimelightIO.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 3259a3c6..e332ae82 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -55,9 +55,11 @@ private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results result private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { + final LimelightHelpers.LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials; + double totalDistanceFromTags = 0; - for (int i = 0; i < results.targets_Fiducials.length; i++) - totalDistanceFromTags += getDistanceFromTag(i, results.getBotPose3d_wpiBlue()); + for (LimelightHelpers.LimelightTarget_Fiducial targetFiducial : targetFiducials) + totalDistanceFromTags += getDistanceFromTag((int) targetFiducial.fiducialID, results.getBotPose3d_wpiBlue()); return totalDistanceFromTags /= results.targets_Fiducials.length; } From e22b5bb64d5e090bdf447017cdedc98af8eceef0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 18:05:53 +0300 Subject: [PATCH 027/261] Updated timestamp problem for limelight --- .../poseestimation/apriltagcamera/io/AprilTagLimelightIO.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index e332ae82..c547ff88 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; @@ -27,7 +28,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); - inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; + inputs.lastResultTimestamp = Units.millisecondsToSeconds(results.timestamp_LIMELIGHT_publish); inputs.visibleTagIDs = getVisibleTagIDs(results); inputs.bestTargetRelativeYawRadians = getBestTargetRelativeRotation(results).getZ(); inputs.bestTargetRelativePitchRadians = getBestTargetRelativeRotation(results).getY(); From 8eda89f7022a9e89ddc7e1837ac6e58b9d39f602 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 18:07:05 +0300 Subject: [PATCH 028/261] Reordered --- .../robot/poseestimation/apriltagcamera/AprilTagCamera.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 31ff30e9..76059375 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -51,10 +51,10 @@ public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourc public void update() { aprilTagCameraIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); - logVisibleTags(); - robotPose = calculateBestRobotPose(inputs.distanceFromBestTag); + logEstimatedRobotPose(); + logVisibleTags(); } public boolean hasNewResult() { From e0e6be5b462e8b01b5dad4274d259631510e3219 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 18:16:52 +0300 Subject: [PATCH 029/261] Fixed? --- .../poseestimation/apriltagcamera/AprilTagCamera.java | 10 +++++----- .../apriltagcamera/AprilTagCameraIO.java | 2 +- .../apriltagcamera/io/AprilTagLimelightIO.java | 7 +++---- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 2 +- .../poseestimation/poseestimator/PoseEstimator.java | 2 +- 5 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 76059375..993f5b9f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -69,8 +69,8 @@ public String getName() { return name; } - public double getLastResultTimestamp() { - return inputs.lastResultTimestamp; + public double getLastResultTimestampSeconds() { + return inputs.lastResultTimestampSeconds; } @@ -108,7 +108,7 @@ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { * @return the robot's pose */ private Pose3d calculateAssumedRobotHeadingPose() { - final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.lastResultTimestamp).getRotation(); + final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.lastResultTimestampSeconds).getRotation(); final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeadingAtResultTimestamp); return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeadingAtResultTimestamp)); } @@ -137,10 +137,10 @@ private Pose2d cameraPoseToRobotPose(Pose3d cameraPose) { } private boolean isNewTimestamp() { - if (lastUpdatedTimestamp == getLastResultTimestamp()) + if (lastUpdatedTimestamp == getLastResultTimestampSeconds()) return false; - lastUpdatedTimestamp = getLastResultTimestamp(); + lastUpdatedTimestamp = getLastResultTimestampSeconds(); return true; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index 0a9daa18..beed4b31 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -10,7 +10,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { @AutoLog public static class RobotPoseSourceInputs { public boolean hasResult = false; - public double lastResultTimestamp = 0; + public double lastResultTimestampSeconds = 0; public Pose3d solvePNPPose = new Pose3d(); public int[] visibleTagIDs = new int[0]; public double bestTargetRelativeYawRadians = 0; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index c547ff88..741311d9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -28,7 +28,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); - inputs.lastResultTimestamp = Units.millisecondsToSeconds(results.timestamp_LIMELIGHT_publish); + inputs.lastResultTimestampSeconds = Units.millisecondsToSeconds(results.timestamp_LIMELIGHT_publish); inputs.visibleTagIDs = getVisibleTagIDs(results); inputs.bestTargetRelativeYawRadians = getBestTargetRelativeRotation(results).getZ(); inputs.bestTargetRelativePitchRadians = getBestTargetRelativeRotation(results).getY(); @@ -50,11 +50,10 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { } private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results results) { - final int fiducialID = (int) results.targets_Fiducials[0].fiducialID; - return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getRotation(); + final LimelightHelpers.LimelightTarget_Fiducial targetTag = results.targets_Fiducials[0]; + return new Rotation3d(0, targetTag.tx, targetTag.ty); } - private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { final LimelightHelpers.LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 287bb110..1d17a2b3 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -38,7 +38,7 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Photo final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); inputs.solvePNPPose = getSolvePNPPose(latestResult); - inputs.lastResultTimestamp = latestResult.getTimestampSeconds(); + inputs.lastResultTimestampSeconds = latestResult.getTimestampSeconds(); inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index a2c37bb9..69215e11 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -110,7 +110,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera return new PoseEstimator6328.VisionObservation( robotPose, - aprilTagCamera.getLastResultTimestamp(), + aprilTagCamera.getLastResultTimestampSeconds(), aprilTagCamera.calculateStandardDeviations() ); } From c413fde73557e8f62b7c377504fd59cf784353bd Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 13 Sep 2024 18:25:46 +0300 Subject: [PATCH 030/261] Renamed and converted degrees to radians --- .../poseestimation/apriltagcamera/AprilTagCamera.java | 10 +++++----- .../apriltagcamera/AprilTagCameraIO.java | 2 +- .../apriltagcamera/io/AprilTagLimelightIO.java | 6 +++--- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 2 +- .../poseestimation/poseestimator/PoseEstimator.java | 2 +- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 993f5b9f..0c435044 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -69,8 +69,8 @@ public String getName() { return name; } - public double getLastResultTimestampSeconds() { - return inputs.lastResultTimestampSeconds; + public double getLatestResultTimestampSeconds() { + return inputs.latestResultTimestampSeconds; } @@ -108,7 +108,7 @@ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { * @return the robot's pose */ private Pose3d calculateAssumedRobotHeadingPose() { - final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.lastResultTimestampSeconds).getRotation(); + final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeadingAtResultTimestamp); return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeadingAtResultTimestamp)); } @@ -137,10 +137,10 @@ private Pose2d cameraPoseToRobotPose(Pose3d cameraPose) { } private boolean isNewTimestamp() { - if (lastUpdatedTimestamp == getLastResultTimestampSeconds()) + if (lastUpdatedTimestamp == getLatestResultTimestampSeconds()) return false; - lastUpdatedTimestamp = getLastResultTimestampSeconds(); + lastUpdatedTimestamp = getLatestResultTimestampSeconds(); return true; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index beed4b31..8254c8bb 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -10,7 +10,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { @AutoLog public static class RobotPoseSourceInputs { public boolean hasResult = false; - public double lastResultTimestampSeconds = 0; + public double latestResultTimestampSeconds = 0; public Pose3d solvePNPPose = new Pose3d(); public int[] visibleTagIDs = new int[0]; public double bestTargetRelativeYawRadians = 0; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 741311d9..e77b11cc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -28,7 +28,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); - inputs.lastResultTimestampSeconds = Units.millisecondsToSeconds(results.timestamp_LIMELIGHT_publish); + inputs.latestResultTimestampSeconds = Units.millisecondsToSeconds(results.timestamp_LIMELIGHT_publish); inputs.visibleTagIDs = getVisibleTagIDs(results); inputs.bestTargetRelativeYawRadians = getBestTargetRelativeRotation(results).getZ(); inputs.bestTargetRelativePitchRadians = getBestTargetRelativeRotation(results).getY(); @@ -50,8 +50,8 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { } private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results results) { - final LimelightHelpers.LimelightTarget_Fiducial targetTag = results.targets_Fiducials[0]; - return new Rotation3d(0, targetTag.tx, targetTag.ty); + final LimelightHelpers.LimelightTarget_Fiducial bestTag = results.targets_Fiducials[0]; + return new Rotation3d(0, Units.degreesToRadians(bestTag.tx), Units.degreesToRadians(bestTag.ty)); } private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 1d17a2b3..906ef5be 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -38,7 +38,7 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Photo final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); inputs.solvePNPPose = getSolvePNPPose(latestResult); - inputs.lastResultTimestampSeconds = latestResult.getTimestampSeconds(); + inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 69215e11..e05f6145 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -110,7 +110,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera return new PoseEstimator6328.VisionObservation( robotPose, - aprilTagCamera.getLastResultTimestampSeconds(), + aprilTagCamera.getLatestResultTimestampSeconds(), aprilTagCamera.calculateStandardDeviations() ); } From 2ca71c9196a862d90c15c8539a66ababe5d61dc0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 21:16:46 +0300 Subject: [PATCH 031/261] Added javadoc --- .../apriltagcamera/io/AprilTagLimelightIO.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index e77b11cc..a9436674 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -27,11 +27,13 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; + final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); + inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); inputs.latestResultTimestampSeconds = Units.millisecondsToSeconds(results.timestamp_LIMELIGHT_publish); inputs.visibleTagIDs = getVisibleTagIDs(results); - inputs.bestTargetRelativeYawRadians = getBestTargetRelativeRotation(results).getZ(); - inputs.bestTargetRelativePitchRadians = getBestTargetRelativeRotation(results).getY(); + inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); + inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY(); inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results); inputs.distanceFromBestTag = getDistanceFromBestTag(results); } @@ -49,6 +51,12 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { return visibleTagIDs; } + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param results the camera's pipeline result + * @return the estimated rotation + */ private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results results) { final LimelightHelpers.LimelightTarget_Fiducial bestTag = results.targets_Fiducials[0]; return new Rotation3d(0, Units.degreesToRadians(bestTag.tx), Units.degreesToRadians(bestTag.ty)); From 0ed359f518263779fbf9d3c49c4d428e9a6f6b10 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 21:28:15 +0300 Subject: [PATCH 032/261] Made changing the best target if needed easier --- .../io/AprilTagLimelightIO.java | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index a9436674..3a865818 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -46,11 +46,26 @@ private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { private int[] getVisibleTagIDs(LimelightHelpers.Results results) { final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; final int[] visibleTagIDs = new int[visibleTags.length]; - for (int i = 0; i < visibleTagIDs.length; i++) - visibleTagIDs[i] = (int) visibleTags[i].fiducialID; + visibleTagIDs[0] = (int) getBestTarget(results).fiducialID; + int idAddition = 1; + + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = (int) visibleTags[i].fiducialID; + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + + visibleTagIDs[i + idAddition] = currentID; + } return visibleTagIDs; } + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.Results results) { + return results.targets_Fiducials[0]; + } + /** * Estimates the camera's rotation relative to the apriltag. * @@ -58,7 +73,7 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { * @return the estimated rotation */ private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results results) { - final LimelightHelpers.LimelightTarget_Fiducial bestTag = results.targets_Fiducials[0]; + final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results); return new Rotation3d(0, Units.degreesToRadians(bestTag.tx), Units.degreesToRadians(bestTag.ty)); } From 8245c2c31f8c3e6df6abc13a0918f1d65e298617 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 21:28:40 +0300 Subject: [PATCH 033/261] Reordered --- .../apriltagcamera/io/AprilTagLimelightIO.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 3a865818..5f5c5ae6 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -43,6 +43,10 @@ private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.solvePNPPose = new Pose3d(); } + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.Results results) { + return results.targets_Fiducials[0]; + } + private int[] getVisibleTagIDs(LimelightHelpers.Results results) { final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; final int[] visibleTagIDs = new int[visibleTags.length]; @@ -62,10 +66,6 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { return visibleTagIDs; } - private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.Results results) { - return results.targets_Fiducials[0]; - } - /** * Estimates the camera's rotation relative to the apriltag. * From e588f20c9f0bad19ee14854997e6f2e2c2dbaa0f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 21:44:34 +0300 Subject: [PATCH 034/261] Clean --- .../io/AprilTagLimelightIO.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 5f5c5ae6..a95d7c6f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -17,7 +17,12 @@ public AprilTagLimelightIO(String hostname) { @Override protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = LimelightHelpers.getTV(hostname); + final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; + + if (results.targets_Fiducials.length > 0) + inputs.hasResult = LimelightHelpers.getTV(hostname); + else + inputs.hasResult = false; if (inputs.hasResult) updateHasResultInputs(inputs); @@ -30,7 +35,7 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); - inputs.latestResultTimestampSeconds = Units.millisecondsToSeconds(results.timestamp_LIMELIGHT_publish); + inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; inputs.visibleTagIDs = getVisibleTagIDs(results); inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY(); @@ -43,10 +48,6 @@ private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.solvePNPPose = new Pose3d(); } - private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.Results results) { - return results.targets_Fiducials[0]; - } - private int[] getVisibleTagIDs(LimelightHelpers.Results results) { final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; final int[] visibleTagIDs = new int[visibleTags.length]; @@ -87,10 +88,14 @@ private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { } private double getDistanceFromBestTag(LimelightHelpers.Results results) { - return getDistanceFromTag((int) results.targets_Fiducials[0].fiducialID, results.getBotPose3d_wpiBlue()); + return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue()); } private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) { return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation()); } + + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.Results results) { + return results.targets_Fiducials[0]; + } } \ No newline at end of file From 73bb5ba49957a4b325416fb11aa8c1aa54a81b31 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 22:08:35 +0300 Subject: [PATCH 035/261] Fixed problem --- .../poseestimation/apriltagcamera/io/AprilTagLimelightIO.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index a95d7c6f..eb5e96cc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -84,7 +84,7 @@ private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { double totalDistanceFromTags = 0; for (LimelightHelpers.LimelightTarget_Fiducial targetFiducial : targetFiducials) totalDistanceFromTags += getDistanceFromTag((int) targetFiducial.fiducialID, results.getBotPose3d_wpiBlue()); - return totalDistanceFromTags /= results.targets_Fiducials.length; + return totalDistanceFromTags / results.targets_Fiducials.length; } private double getDistanceFromBestTag(LimelightHelpers.Results results) { From eea45cdb5f7950e7479200e5551a34c0eae96c6d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 22:12:38 +0300 Subject: [PATCH 036/261] Reformatted --- .../apriltagcamera/io/AprilTagLimelightIO.java | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index eb5e96cc..20802d34 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -19,19 +19,15 @@ public AprilTagLimelightIO(String hostname) { protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; - if (results.targets_Fiducials.length > 0) - inputs.hasResult = LimelightHelpers.getTV(hostname); - else - inputs.hasResult = false; + inputs.hasResult = results.targets_Fiducials.length > 0; if (inputs.hasResult) - updateHasResultInputs(inputs); + updateHasResultInputs(inputs, results); else updateNoResultInputs(inputs); } - private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs) { - final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, LimelightHelpers.Results results) { final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); @@ -80,10 +76,11 @@ private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results result private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { final LimelightHelpers.LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials; - double totalDistanceFromTags = 0; + for (LimelightHelpers.LimelightTarget_Fiducial targetFiducial : targetFiducials) totalDistanceFromTags += getDistanceFromTag((int) targetFiducial.fiducialID, results.getBotPose3d_wpiBlue()); + return totalDistanceFromTags / results.targets_Fiducials.length; } From 59d92b28cd742a255d8a86377248e8b5025f85da Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 22:43:13 +0300 Subject: [PATCH 037/261] Updated TRIGONLib --- build.gradle | 2 +- .../apriltagcamera/io/AprilTagLimelightIO.java | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index f0ff5222..a54357f9 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.8' + implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.9' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 20802d34..1af060fe 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -17,7 +17,7 @@ public AprilTagLimelightIO(String hostname) { @Override protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; + final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); inputs.hasResult = results.targets_Fiducials.length > 0; @@ -27,7 +27,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { updateNoResultInputs(inputs); } - private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, LimelightHelpers.Results results) { + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); @@ -44,7 +44,7 @@ private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.solvePNPPose = new Pose3d(); } - private int[] getVisibleTagIDs(LimelightHelpers.Results results) { + private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; final int[] visibleTagIDs = new int[visibleTags.length]; visibleTagIDs[0] = (int) getBestTarget(results).fiducialID; @@ -69,12 +69,12 @@ private int[] getVisibleTagIDs(LimelightHelpers.Results results) { * @param results the camera's pipeline result * @return the estimated rotation */ - private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.Results results) { + private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.LimelightResults results) { final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results); return new Rotation3d(0, Units.degreesToRadians(bestTag.tx), Units.degreesToRadians(bestTag.ty)); } - private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { + private double getAverageDistanceFromAllTags(LimelightHelpers.LimelightResults results) { final LimelightHelpers.LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials; double totalDistanceFromTags = 0; @@ -84,7 +84,7 @@ private double getAverageDistanceFromAllTags(LimelightHelpers.Results results) { return totalDistanceFromTags / results.targets_Fiducials.length; } - private double getDistanceFromBestTag(LimelightHelpers.Results results) { + private double getDistanceFromBestTag(LimelightHelpers.LimelightResults results) { return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue()); } @@ -92,7 +92,7 @@ private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) { return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation()); } - private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.Results results) { + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) { return results.targets_Fiducials[0]; } } \ No newline at end of file From 6fa239f85b6cee5638f73bb87abb1b864cc7c70d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 22:47:57 +0300 Subject: [PATCH 038/261] Fixed bug --- src/main/java/frc/trigon/robot/commands/CommandConstants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 1a8f989d..ba2f57cf 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -19,6 +19,7 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.Logger; +import org.trigon.commands.WheelRadiusCharacterizationCommand; import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.Mirrorable; import org.trigon.utilities.mirrorable.MirrorablePose2d; From 10655c557f4b6eef5db720db3070a9303d022be1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 15 Sep 2024 00:13:17 +0300 Subject: [PATCH 039/261] Removed PhotonPoseEstimator --- .../AprilTagCameraConstants.java | 4 - .../EstimatedRobotPose.java | 72 -- .../PhotonPoseEstimator.java | 791 ------------------ 3 files changed, 867 deletions(-) delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 2ded1c80..43c64c2e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -7,16 +7,12 @@ import edu.wpi.first.math.geometry.Pose3d; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; import java.util.HashMap; import java.util.function.Function; public class AprilTagCameraConstants { public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); - static final PhotonPoseEstimator.PoseStrategy - PRIMARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - SECONDARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_HEADING; static AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); static { diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java deleted file mode 100644 index dcc605b7..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java +++ /dev/null @@ -1,72 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.math.geometry.Pose3d; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; - -/** - * An estimated pose based on pipeline result - */ -public class EstimatedRobotPose { - /** - * The estimated pose - */ - public final Pose3d estimatedPose; - - /** - * The estimated time the frame used to derive the robot pose was taken - */ - public final double timestampSeconds; - - /** - * A list of the targets used to compute this pose - */ - public final List targetsUsed; - - /** - * The strategy actually used to produce this pose - */ - public final PhotonPoseEstimator.PoseStrategy strategy; - - /** - * Constructs an EstimatedRobotPose - * - * @param estimatedPose estimated pose - * @param timestampSeconds timestamp of the estimate - */ - public EstimatedRobotPose( - Pose3d estimatedPose, - double timestampSeconds, - List targetsUsed, - PhotonPoseEstimator.PoseStrategy strategy) { - this.estimatedPose = estimatedPose; - this.timestampSeconds = timestampSeconds; - this.targetsUsed = targetsUsed; - this.strategy = strategy; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java deleted file mode 100644 index bd2c3269..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java +++ /dev/null @@ -1,791 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.wpilibj.DriverStation; -import frc.trigon.robot.RobotContainer; -import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; - -/** - * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using the strategy set - * below. Example usage can be found in our apriltagExample example project. - */ -public class PhotonPoseEstimator { - private static int InstanceCount = 0; - - /** - * Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. - */ - public enum PoseStrategy { - /** - * Choose the Pose with the lowest ambiguity. - */ - LOWEST_AMBIGUITY, - - /** - * Choose the Pose which is closest to the camera height. - */ - CLOSEST_TO_CAMERA_HEIGHT, - - /** - * Choose the Pose which is closest to a set Reference position. - */ - CLOSEST_TO_REFERENCE_POSE, - - /** - * Choose the Pose which is closest to the last pose calculated - */ - CLOSEST_TO_LAST_POSE, - - /** - * Return the average of the best target poses using ambiguity as weight. - */ - AVERAGE_BEST_TARGETS, - - /** - * Use all visible tags to compute a single pose estimate on coprocessor. This option needs to - * be enabled on the PhotonVision web UI as well. - */ - MULTI_TAG_PNP_ON_COPROCESSOR, - - /** - * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can - * take a lot of time. - */ - MULTI_TAG_PNP_ON_RIO, - - CLOSEST_TO_HEADING - } - - private AprilTagFieldLayout fieldTags; - private TargetModel tagModel = TargetModel.kAprilTag16h5; - private PoseStrategy primaryStrategy; - private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; - private final PhotonCamera camera; - private Transform3d robotToCamera; - - private Pose3d lastPose; - private Pose3d referencePose; - protected double poseCacheTimestampSeconds = -1; - private final Set reportedErrors = new HashSet<>(); - - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect the - * results from this class. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot -> camera) in the Robot - * Coordinate System. - */ - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, - PoseStrategy strategy, - PhotonCamera camera, - Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = camera; - this.robotToCamera = robotToCamera; - - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); - InstanceCount++; - } - - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this(fieldTags, strategy, null, robotToCamera); - } - - /** - * Invalidates the pose cache. - */ - private void invalidatePoseCache() { - poseCacheTimestampSeconds = -1; - } - - private void checkUpdate(Object oldObj, Object newObj) { - if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) { - invalidatePoseCache(); - } - } - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @return the AprilTagFieldLayout - */ - public AprilTagFieldLayout getFieldTags() { - return fieldTags; - } - - /** - * Set the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @param fieldTags the AprilTagFieldLayout - */ - public void setFieldTags(AprilTagFieldLayout fieldTags) { - checkUpdate(this.fieldTags, fieldTags); - this.fieldTags = fieldTags; - } - - /** - * Get the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - *

By default, this is {@link TargetModel#kAprilTag16h5}. - */ - public TargetModel getTagModel() { - return tagModel; - } - - /** - * Set the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}. - */ - public void setTagModel(TargetModel tagModel) { - this.tagModel = tagModel; - } - - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - */ - public PoseStrategy getPrimaryStrategy() { - return primaryStrategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - */ - public void setPrimaryStrategy(PoseStrategy strategy) { - checkUpdate(this.primaryStrategy, strategy); - this.primaryStrategy = strategy; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must - * NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - */ - public void setMultiTagFallbackStrategy(PoseStrategy strategy) { - checkUpdate(this.multiTagFallbackStrategy, strategy); - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR - || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) { - DriverStation.reportWarning( - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false); - strategy = PoseStrategy.LOWEST_AMBIGUITY; - } - this.multiTagFallbackStrategy = strategy; - } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - */ - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose3d referencePose) { - checkUpdate(this.referencePose, referencePose); - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(referencePose)); - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose2d lastPose) { - setLastPose(new Pose3d(lastPose)); - } - - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - - /** - * Poll data from the configured cameras and update the estimated position of the robot. Returns - * empty if: - * - *

    - *
  • New data has not been received since the last call to {@code update()}. - *
  • No targets were found from the camera - *
  • There is no camera set - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update() { - if (camera == null) { - DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false); - return Optional.empty(); - } - - PhotonPipelineResult cameraResult = camera.getLatestResult(); - - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update(PhotonPipelineResult cameraResult) { - if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty()); - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result -// - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy); - } - - private Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - PoseStrategy strat) { - Optional estimatedPose; - switch (strat) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - estimatedPose = closestToCameraHeightStrategy(cameraResult); - break; - case CLOSEST_TO_REFERENCE_POSE: - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case CLOSEST_TO_LAST_POSE: - setReferencePose(lastPose); - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case AVERAGE_BEST_TARGETS: - estimatedPose = averageBestTargetsStrategy(cameraResult); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case CLOSEST_TO_HEADING: - estimatedPose = closestToHeadingStrategy(cameraResult); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - - private Optional closestToHeadingStrategy(PhotonPipelineResult result) { - double smallestAngleDifferenceRadians; - EstimatedRobotPose closestAngleTarget; - double currentHeadingRadians = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getRadians(); - - PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - return Optional.empty(); - } - - Transform3d bestCameraToTarget = target.getBestCameraToTarget(); - - Pose3d bestPose = - targetPosition - .get() - .transformBy(bestCameraToTarget.inverse()) - .transformBy(robotToCamera.inverse()); - double bestTransformAngle = bestPose.getRotation().getZ(); - smallestAngleDifferenceRadians = Math.abs(currentHeadingRadians - bestTransformAngle); - closestAngleTarget = - new EstimatedRobotPose( - bestPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - - Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); - if (alternateCameraToTarget.getRotation().getZ() != 0) { - Pose3d altPose = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - double alternateTransformAngle = altPose.getRotation().getZ(); - double alternateTransformDelta = Math.abs(currentHeadingRadians - alternateTransformAngle); - - if (alternateTransformDelta < smallestAngleDifferenceRadians) { - closestAngleTarget = - new EstimatedRobotPose( - altPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.of(closestAngleTarget); - } - - private Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - Transform3d best_tf = result.getMultiTagResult().estimatedPose.best; - Pose3d cam = new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(fieldTags.getOrigin()); - Pose3d best = cam.plus(robotToCamera.inverse()); // field-to-robot -// Logger.recordOutput(camera.getName(), Math.toDegrees(cam.getRotation().getY())); - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - } - - private Optional multiTagOnRioStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - - var pnpResult = - VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent) - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - var best = - new Pose3d() - .plus(pnpResult.best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = fieldTags.getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - return Optional.of( - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY)); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta height - * difference between the estimated and actual height of the camera. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToCameraHeightStrategy(PhotonPipelineResult result) { - double smallestHeightDifference = 10e9; - EstimatedRobotPose closestHeightTarget = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - - - double alternateTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .getZ()); - double bestTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .getZ()); - - if (alternateTransformDelta < smallestHeightDifference) { - smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - - if (bestTransformDelta < smallestHeightDifference) { - smallestHeightDifference = bestTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.ofNullable(closestHeightTarget); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta in the vector - * magnitude between it and the reference pose. - * - * @param result pipeline result - * @param referencePose reference pose to check vector magnitude difference against. - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToReferencePoseStrategy( - PhotonPipelineResult result, Pose3d referencePose) { - if (referencePose == null) { -// DriverStation.reportError( -// "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!", -// false); - return Optional.empty(); - } - - double smallestPoseDelta = 10e9; - EstimatedRobotPose lowestDeltaPose = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - continue; - } - - Pose3d altTransformPosition = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - - double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); - double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); - - if (altDifference < smallestPoseDelta) { - smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose( - altTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - if (bestDifference < smallestPoseDelta) { - smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose( - bestTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - } - return Optional.ofNullable(lowestDeltaPose); - } - - /** - * Return the average of the best target poses using ambiguity as weight. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { - if (!result.hasTargets()) - return Optional.empty(); - final PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - final Pose3d cameraPose = targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()); -// Logger.recordOutput(camera.getName(), Math.toDegrees(cameraPose.getRotation().getY())); - final Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse()); - - return Optional.of( - new EstimatedRobotPose( - robotPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.AVERAGE_BEST_TARGETS - ) - ); - } - - /** - * Difference is defined as the vector magnitude between the two poses - * - * @return The absolute "difference" (>=0) between two Pose3ds. - */ - private double calculateDifference(Pose3d x, Pose3d y) { - return x.getTranslation().getDistance(y.getTranslation()); - } - - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); - reportedErrors.add(fiducialId); - } - } -} From 5438a8423011e2d11e8ec3fe7428df219b82ca37 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 15 Sep 2024 09:48:17 +0300 Subject: [PATCH 040/261] Reordered and renamed --- .../apriltagcamera/AprilTagCamera.java | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 0c435044..7af2efa2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -21,26 +21,29 @@ public class AprilTagCamera { private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); private final Transform3d robotCenterToCamera; private final AprilTagCameraIO aprilTagCameraIO; + private final double + solvePNPThetaStandardDeviationsExponent, + solvePNPTranslationStandardDeviationsExponent, + assumedRobotPoseTranslationStandardDeviationsExponent; private double lastUpdatedTimestamp; - private final double solvePNPTranslationsStandardDeviationsExponent, assumedRobotPoseTranslationsStandardDeviationsExponent, solvePNPThetaStandardDeviationsExponent; private Pose2d robotPose = null; /** * Constructs a new AprilTagCamera. * - * @param robotPoseSourceType the type of camera - * @param name the camera's name - * @param robotCenterToCamera the transform of the robot's origin point to the camera - * @param solvePNPTranslationsStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP - * @param solvePNPThetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP - * @param assumedRobotHeadingTranslationsStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when getting the pose by assuming the robot's heading + * @param robotPoseSourceType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param solvePNPTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP + * @param solvePNPThetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP + * @param assumedRobotHeadingTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when getting the pose by assuming the robot's heading */ - public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationsStandardDeviationsExponent, double solvePNPThetaStandardDeviationsExponent, double assumedRobotHeadingTranslationsStandardDeviationsExponent) { + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationStandardDeviationsExponent, double solvePNPThetaStandardDeviationsExponent, double assumedRobotHeadingTranslationStandardDeviationsExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; - this.solvePNPTranslationsStandardDeviationsExponent = solvePNPTranslationsStandardDeviationsExponent; + this.solvePNPTranslationStandardDeviationsExponent = solvePNPTranslationStandardDeviationsExponent; this.solvePNPThetaStandardDeviationsExponent = solvePNPThetaStandardDeviationsExponent; - this.assumedRobotPoseTranslationsStandardDeviationsExponent = assumedRobotHeadingTranslationsStandardDeviationsExponent; + this.assumedRobotPoseTranslationStandardDeviationsExponent = assumedRobotHeadingTranslationStandardDeviationsExponent; if (Robot.IS_REAL) aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name); @@ -158,7 +161,7 @@ private void logVisibleTags() { private Matrix calculateSolvePNPStandardDeviations() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; - final double translationStandardDeviation = calculateStandardDeviations(solvePNPTranslationsStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); + final double translationStandardDeviation = calculateStandardDeviations(solvePNPTranslationStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); final double thetaStandardDeviation = calculateStandardDeviations(solvePNPThetaStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); @@ -171,7 +174,7 @@ private Matrix calculateSolvePNPStandardDeviations() { * @return the standard deviations */ private Matrix calculateAssumedHeadingStandardDeviations() { - final double translationStandardDeviation = calculateStandardDeviations(assumedRobotPoseTranslationsStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); + final double translationStandardDeviation = calculateStandardDeviations(assumedRobotPoseTranslationStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); final double thetaStandardDeviation = Double.POSITIVE_INFINITY; return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); From e10c5f8489ea1fbc7855270c776fe6bf057a2ba9 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 15 Sep 2024 11:05:11 +0300 Subject: [PATCH 041/261] Fixed order and extensive logging --- .../apriltagcamera/AprilTagCamera.java | 34 ++++++++++--------- .../robot/subsystems/MotorSubsystem.java | 2 +- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 7af2efa2..45561a73 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -8,6 +8,7 @@ import frc.trigon.robot.Robot; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; +import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonUtils; @@ -34,15 +35,15 @@ public class AprilTagCamera { * @param robotPoseSourceType the type of camera * @param name the camera's name * @param robotCenterToCamera the transform of the robot's origin point to the camera - * @param solvePNPTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP * @param solvePNPThetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP + * @param solvePNPTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP * @param assumedRobotHeadingTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when getting the pose by assuming the robot's heading */ - public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPTranslationStandardDeviationsExponent, double solvePNPThetaStandardDeviationsExponent, double assumedRobotHeadingTranslationStandardDeviationsExponent) { + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPThetaStandardDeviationsExponent, double solvePNPTranslationStandardDeviationsExponent, double assumedRobotHeadingTranslationStandardDeviationsExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; - this.solvePNPTranslationStandardDeviationsExponent = solvePNPTranslationStandardDeviationsExponent; this.solvePNPThetaStandardDeviationsExponent = solvePNPThetaStandardDeviationsExponent; + this.solvePNPTranslationStandardDeviationsExponent = solvePNPTranslationStandardDeviationsExponent; this.assumedRobotPoseTranslationStandardDeviationsExponent = assumedRobotHeadingTranslationStandardDeviationsExponent; if (Robot.IS_REAL) @@ -57,7 +58,8 @@ public void update() { robotPose = calculateBestRobotPose(inputs.distanceFromBestTag); logEstimatedRobotPose(); - logVisibleTags(); + if (MotorSubsystem.isExtensiveLoggingEnabled()) + logVisibleTags(); } public boolean hasNewResult() { @@ -147,18 +149,6 @@ private boolean isNewTimestamp() { return true; } - private void logVisibleTags() { - if (inputs.hasResult) { - Logger.recordOutput("VisibleTags/" + this.getName(), new Pose2d[0]); - return; - } - - final Pose2d[] visibleTagPoses = new Pose2d[inputs.visibleTagIDs.length]; - for (int i = 0; i < visibleTagPoses.length; i++) - visibleTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(i).toPose2d(); - Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); - } - private Matrix calculateSolvePNPStandardDeviations() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; final double translationStandardDeviation = calculateStandardDeviations(solvePNPTranslationStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); @@ -203,4 +193,16 @@ private void logEstimatedRobotPose() { else Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); } + + private void logVisibleTags() { + if (inputs.hasResult) { + Logger.recordOutput("VisibleTags/" + this.getName(), new Pose2d[0]); + return; + } + + final Pose2d[] visibleTagPoses = new Pose2d[inputs.visibleTagIDs.length]; + for (int i = 0; i < visibleTagPoses.length; i++) + visibleTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(i).toPose2d(); + Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 950793aa..7c022e9d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -63,7 +63,7 @@ public static void setAllSubsystemsBrakeAsync(boolean brake) { } public static boolean isExtensiveLoggingEnabled() { - return ENABLE_EXTENSIVE_LOGGING.get(); + return ENABLE_EXTENSIVE_LOGGING.get() || RobotHardwareStats.isReplay(); } /** From 6293822d268c0a454a0e7ea8909124704a49b554 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 15 Sep 2024 11:18:30 +0300 Subject: [PATCH 042/261] Removed redundant checks --- .../frc/trigon/robot/commands/factories/GeneralCommands.java | 3 +-- src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index bde6eb34..e5a9327e 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -16,7 +16,6 @@ import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import org.littletonrobotics.junction.Logger; -import org.trigon.hardware.RobotHardwareStats; import java.awt.*; import java.util.function.BooleanSupplier; @@ -48,7 +47,7 @@ public static Command getNoteCollectionCommand() { public static Command getVisualizeNoteShootingCommand() { return new InstantCommand( () -> new VisualizeNoteShootingCommand() - .schedule()).onlyIf(() -> RobotHardwareStats.isReplay() || MotorSubsystem.isExtensiveLoggingEnabled()); + .schedule()).onlyIf(MotorSubsystem::isExtensiveLoggingEnabled); } /** diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 7c022e9d..f179b1d7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -74,7 +74,7 @@ public static boolean isExtensiveLoggingEnabled() { @Override public final void periodic() { updatePeriodically(); - if (RobotHardwareStats.isReplay() || isExtensiveLoggingEnabled()) + if (isExtensiveLoggingEnabled()) updateMechanism(); } From 7c4e72192eb0c57b1998a9b256c212e2d826d38b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 24 Sep 2024 10:49:15 +0300 Subject: [PATCH 043/261] Added changes from after testing (still more to do though) --- .../apriltagcamera/AprilTagCamera.java | 70 ++++++++++--------- .../io/AprilTagPhotonCameraIO.java | 12 ++-- .../poseestimator/PoseEstimator6328.java | 5 +- .../poseestimator/PoseEstimatorConstants.java | 2 +- 4 files changed, 49 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 45561a73..c5cb4946 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.Robot; -import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; @@ -39,7 +38,11 @@ public class AprilTagCamera { * @param solvePNPTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP * @param assumedRobotHeadingTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when getting the pose by assuming the robot's heading */ - public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double solvePNPThetaStandardDeviationsExponent, double solvePNPTranslationStandardDeviationsExponent, double assumedRobotHeadingTranslationStandardDeviationsExponent) { + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, + String name, Transform3d robotCenterToCamera, + double solvePNPThetaStandardDeviationsExponent, + double solvePNPTranslationStandardDeviationsExponent, + double assumedRobotHeadingTranslationStandardDeviationsExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; this.solvePNPThetaStandardDeviationsExponent = solvePNPThetaStandardDeviationsExponent; @@ -55,10 +58,10 @@ public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourc public void update() { aprilTagCameraIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); - robotPose = calculateBestRobotPose(inputs.distanceFromBestTag); + robotPose = calculateBestRobotPose(); logEstimatedRobotPose(); - if (MotorSubsystem.isExtensiveLoggingEnabled()) + if (MotorSubsystem.isExtensiveLoggingEnabled() && !AprilTagCameraConstants.TAG_ID_TO_POSE.isEmpty()) logVisibleTags(); } @@ -95,15 +98,15 @@ public Matrix calculateStandardDeviations() { * If the robot is close enough to the tag, it gets the estimated solve PNP pose. * If it's too far, it assumes the robot's heading and calculates its position from there. * Assuming the robot's heading is more robust, but won't fix current wrong heading. - * To fix this, we use solve PNP to reset the robot's heading, when we're close enough for an accurate result. + * To fix this, we use solve PNP to reset the robot's heading when we are close enough for an accurate result. * - * @param distanceFromBestTag the average of the distance from the best visible tag * @return the robot's pose */ - private Pose2d calculateBestRobotPose(double distanceFromBestTag) { + private Pose2d calculateBestRobotPose() { if (isWithinBestTagRangeForSolvePNP()) - return cameraPoseToRobotPose(inputs.solvePNPPose); - return calculateAssumedRobotHeadingPose().toPose2d(); + return calculateAssumedRobotHeadingPose(inputs.solvePNPPose.toPose2d().getRotation()); + final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + return calculateAssumedRobotHeadingPose(robotHeadingAtResultTimestamp); } /** @@ -112,33 +115,36 @@ private Pose2d calculateBestRobotPose(double distanceFromBestTag) { * * @return the robot's pose */ - private Pose3d calculateAssumedRobotHeadingPose() { - final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); - final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeadingAtResultTimestamp); - return new Pose3d(new Pose2d(robotFieldRelativePositionTranslation, robotHeadingAtResultTimestamp)); - } + private Pose2d calculateAssumedRobotHeadingPose(Rotation2d robotHeading) { + if (inputs.visibleTagIDs.length == 0) + return null; - private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { - final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]); - final Translation2d tagToCamera = getTagToCamera(tagPose); - final Translation2d robotToTag = tagToCamera.plus(robotCenterToCamera.getTranslation().toTranslation2d()); - final Translation2d fieldRelativeTagPose = robotToTag.rotateBy(robotHeading); - return tagPose.getTranslation().toTranslation2d().plus(fieldRelativeTagPose); + final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); + return new Pose2d(robotFieldRelativePositionTranslation, robotHeading); } - private Translation2d getTagToCamera(Pose3d tagPose) { - final double cameraToTagDistanceMeters = -PhotonUtils.calculateDistanceToTargetMeters( - robotCenterToCamera.getZ(), tagPose.getZ(), robotCenterToCamera.getRotation().getY(), inputs.bestTargetRelativePitchRadians + private Transform2d toTransform2d(Transform3d transform3d) { + return new Transform2d( + transform3d.getTranslation().toTranslation2d(), + transform3d.getRotation().toRotation2d() ); - final double cameraToTagXDistance = Math.sin(inputs.bestTargetRelativeYawRadians + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; - final double cameraToTagYDistance = -Math.cos(inputs.bestTargetRelativeYawRadians + robotCenterToCamera.getRotation().getZ()) * cameraToTagDistanceMeters; - return new Translation2d(cameraToTagXDistance, cameraToTagYDistance); } - private Pose2d cameraPoseToRobotPose(Pose3d cameraPose) { - if (cameraPose == null) - return null; - return cameraPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); + private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { + if (!inputs.hasResult) + return new Translation2d(); + + Pose2d fieldRelativeRobotPose = PhotonUtils.estimateFieldToRobot( + robotCenterToCamera.getZ(), + AprilTagCameraConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).getZ(), + -robotCenterToCamera.getRotation().getY(), + -inputs.bestTargetRelativePitchRadians, + Rotation2d.fromRadians(inputs.bestTargetRelativeYawRadians), + robotHeading.minus(robotCenterToCamera.getRotation().toRotation2d()), + AprilTagCameraConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).toPose2d(), + toTransform2d(robotCenterToCamera.inverse()) + ); + return fieldRelativeRobotPose.getTranslation(); } private boolean isNewTimestamp() { @@ -195,14 +201,14 @@ private void logEstimatedRobotPose() { } private void logVisibleTags() { - if (inputs.hasResult) { + if (!inputs.hasResult) { Logger.recordOutput("VisibleTags/" + this.getName(), new Pose2d[0]); return; } final Pose2d[] visibleTagPoses = new Pose2d[inputs.visibleTagIDs.length]; for (int i = 0; i < visibleTagPoses.length; i++) - visibleTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(i).toPose2d(); + visibleTagPoses[i] = AprilTagCameraConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]).toPose2d(); Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 906ef5be..3e3de178 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -27,11 +27,11 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - inputs.hasResult = latestResult.hasTargets(); - if (inputs.hasResult) - updateHasResultInputs(inputs, latestResult); - else - updateNoResultInputs(inputs); + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); + if (inputs.hasResult) + updateHasResultInputs(inputs, latestResult); + else + updateNoResultInputs(inputs); } private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { @@ -47,7 +47,7 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Photo } private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.visibleTagIDs = new int[0]; + inputs.visibleTagIDs = new int[]{}; inputs.solvePNPPose = new Pose3d(); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 1ec10814..b68ae479 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -146,7 +146,10 @@ public void addVisionObservation(VisionObservation observation) { } public Pose2d samplePose(double timestamp) { - return poseBuffer.getSample(timestamp).orElse(null); + var sample = poseBuffer.getSample(timestamp).orElse(new Pose2d()); + var odometryToSampleTransform = new Transform2d(odometryPose, sample); + + return estimatedPose.plus(odometryToSampleTransform); } /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index a87aa90e..d6e27e57 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -15,7 +15,7 @@ public class PoseEstimatorConstants { */ static final Vector ODOMETRY_AMBIGUITY = VecBuilder.fill(0.003, 0.003, 0.0002); - static final double + public static final double TRANSLATIONS_STD_EXPONENT = 0.005, THETA_STD_EXPONENT = 0.01; } From 9daaacb9cb383e4c461ddc137d2d7b9840227fcf Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 24 Sep 2024 12:30:27 +0300 Subject: [PATCH 044/261] Fixed logic problems and updated to new pose estimation method --- .../apriltagcamera/AprilTagCamera.java | 14 ++++++-------- .../apriltagcamera/AprilTagCameraIO.java | 4 ++-- .../apriltagcamera/io/AprilTagLimelightIO.java | 5 +++-- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 12 +++++++----- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c5cb4946..0c2ff210 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -103,10 +103,8 @@ public Matrix calculateStandardDeviations() { * @return the robot's pose */ private Pose2d calculateBestRobotPose() { - if (isWithinBestTagRangeForSolvePNP()) - return calculateAssumedRobotHeadingPose(inputs.solvePNPPose.toPose2d().getRotation()); - final Rotation2d robotHeadingAtResultTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); - return calculateAssumedRobotHeadingPose(robotHeadingAtResultTimestamp); + final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + return calculateAssumedRobotHeadingPose(isWithinBestTagRangeForSolvePNP() ? inputs.solvePNPHeading : gyroHeadingAtTimestamp); } /** @@ -119,8 +117,8 @@ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d robotHeading) { if (inputs.visibleTagIDs.length == 0) return null; - final Translation2d robotFieldRelativePositionTranslation = getRobotFieldRelativePosition(robotHeading); - return new Pose2d(robotFieldRelativePositionTranslation, robotHeading); + final Translation2d robotFieldRelativeTranslation = getRobotFieldRelativeTranslation(robotHeading); + return new Pose2d(robotFieldRelativeTranslation, robotHeading); } private Transform2d toTransform2d(Transform3d transform3d) { @@ -130,7 +128,7 @@ private Transform2d toTransform2d(Transform3d transform3d) { ); } - private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { + private Translation2d getRobotFieldRelativeTranslation(Rotation2d robotHeading) { if (!inputs.hasResult) return new Translation2d(); @@ -140,7 +138,7 @@ private Translation2d getRobotFieldRelativePosition(Rotation2d robotHeading) { -robotCenterToCamera.getRotation().getY(), -inputs.bestTargetRelativePitchRadians, Rotation2d.fromRadians(inputs.bestTargetRelativeYawRadians), - robotHeading.minus(robotCenterToCamera.getRotation().toRotation2d()), + robotHeading, AprilTagCameraConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).toPose2d(), toTransform2d(robotCenterToCamera.inverse()) ); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index 8254c8bb..c3f5cb52 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -1,6 +1,6 @@ package frc.trigon.robot.poseestimation.apriltagcamera; -import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public class AprilTagCameraIO { @@ -11,7 +11,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { public static class RobotPoseSourceInputs { public boolean hasResult = false; public double latestResultTimestampSeconds = 0; - public Pose3d solvePNPPose = new Pose3d(); + public Rotation2d solvePNPHeading = new Rotation2d(); public int[] visibleTagIDs = new int[0]; public double bestTargetRelativeYawRadians = 0; public double bestTargetRelativePitchRadians = 0; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 1af060fe..c5837cdc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,6 +1,7 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; @@ -30,7 +31,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); - inputs.solvePNPPose = results.getBotPose3d_wpiBlue(); + inputs.solvePNPHeading = results.getBotPose3d_wpiBlue().toPose2d().getRotation(); inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; inputs.visibleTagIDs = getVisibleTagIDs(results); inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); @@ -41,7 +42,7 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Limel private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.visibleTagIDs = new int[0]; - inputs.solvePNPPose = new Pose3d(); + inputs.solvePNPHeading = new Rotation2d(); } private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 3e3de178..4c75bc53 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -37,7 +37,7 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); - inputs.solvePNPPose = getSolvePNPPose(latestResult); + inputs.solvePNPHeading = getSolvePNPHeading(latestResult); inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); @@ -48,7 +48,7 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Photo private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.visibleTagIDs = new int[]{}; - inputs.solvePNPPose = new Pose3d(); + inputs.solvePNPHeading = new Rotation2d(); } private Point getTagCenter(List tagCorners) { @@ -81,14 +81,16 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { * @param result the camera's pipeline result * @return the estimated pose */ - private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + private Rotation2d getSolvePNPHeading(PhotonPipelineResult result) { if (result.getMultiTagResult().estimatedPose.isPresent) { final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; - return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); + final Pose3d estimatedPose = new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); + return estimatedPose.toPose2d().getRotation(); } final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); - return FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()).transformBy(targetToCamera); + final Pose3d estimatedPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()).transformBy(targetToCamera); + return estimatedPose.toPose2d().getRotation(); } private int[] getVisibleTagIDs(PhotonPipelineResult result) { From 2d5f521181a6c7b5bbdcbd341903b43872efd6ed Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 24 Sep 2024 13:06:01 +0300 Subject: [PATCH 045/261] Removed vars --- .../robot/poseestimation/apriltagcamera/AprilTagCamera.java | 3 +-- .../poseestimation/apriltagcamera/io/AprilTagLimelightIO.java | 1 - .../robot/poseestimation/poseestimator/PoseEstimator6328.java | 4 ++-- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 0c2ff210..74d846fc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -80,8 +80,7 @@ public String getName() { public double getLatestResultTimestampSeconds() { return inputs.latestResultTimestampSeconds; } - - + /** * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. * Different pose estimation strategies may use different formulae to calculate the standard deviations. diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index c5837cdc..43c44519 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -19,7 +19,6 @@ public AprilTagLimelightIO(String hostname) { @Override protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); - inputs.hasResult = results.targets_Fiducials.length > 0; if (inputs.hasResult) diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index b68ae479..e2cb1b5f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -146,8 +146,8 @@ public void addVisionObservation(VisionObservation observation) { } public Pose2d samplePose(double timestamp) { - var sample = poseBuffer.getSample(timestamp).orElse(new Pose2d()); - var odometryToSampleTransform = new Transform2d(odometryPose, sample); + Pose2d sample = poseBuffer.getSample(timestamp).orElse(new Pose2d()); + Transform2d odometryToSampleTransform = new Transform2d(odometryPose, sample); return estimatedPose.plus(odometryToSampleTransform); } From 85e44514853b99888db3721a0c91389863d9cf2c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 30 Sep 2024 10:56:14 +0300 Subject: [PATCH 046/261] =?UTF-8?q?Reverted=20to=20only=20solvePNP=20?= =?UTF-8?q?=F0=9F=98=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../apriltagcamera/AprilTagCamera.java | 133 +++--------------- .../AprilTagCameraConstants.java | 20 +-- .../apriltagcamera/AprilTagCameraIO.java | 6 +- .../io/AprilTagLimelightIO.java | 23 +-- .../io/AprilTagPhotonCameraIO.java | 82 +++-------- .../poseestimator/PoseEstimator.java | 6 +- 6 files changed, 55 insertions(+), 215 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 74d846fc..7ce34cfb 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -2,14 +2,14 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.Robot; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; -import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.constants.FieldConstants; import org.littletonrobotics.junction.Logger; -import org.photonvision.PhotonUtils; /** * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. @@ -22,9 +22,8 @@ public class AprilTagCamera { private final Transform3d robotCenterToCamera; private final AprilTagCameraIO aprilTagCameraIO; private final double - solvePNPThetaStandardDeviationsExponent, - solvePNPTranslationStandardDeviationsExponent, - assumedRobotPoseTranslationStandardDeviationsExponent; + thetaStandardDeviationsExponent, + translationStandardDeviationsExponent; private double lastUpdatedTimestamp; private Pose2d robotPose = null; @@ -34,20 +33,14 @@ public class AprilTagCamera { * @param robotPoseSourceType the type of camera * @param name the camera's name * @param robotCenterToCamera the transform of the robot's origin point to the camera - * @param solvePNPThetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP - * @param solvePNPTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP - * @param assumedRobotHeadingTranslationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when getting the pose by assuming the robot's heading + * @param thetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP + * @param translationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP */ - public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, - String name, Transform3d robotCenterToCamera, - double solvePNPThetaStandardDeviationsExponent, - double solvePNPTranslationStandardDeviationsExponent, - double assumedRobotHeadingTranslationStandardDeviationsExponent) { + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double thetaStandardDeviationsExponent, double translationStandardDeviationsExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; - this.solvePNPThetaStandardDeviationsExponent = solvePNPThetaStandardDeviationsExponent; - this.solvePNPTranslationStandardDeviationsExponent = solvePNPTranslationStandardDeviationsExponent; - this.assumedRobotPoseTranslationStandardDeviationsExponent = assumedRobotHeadingTranslationStandardDeviationsExponent; + this.thetaStandardDeviationsExponent = thetaStandardDeviationsExponent; + this.translationStandardDeviationsExponent = translationStandardDeviationsExponent; if (Robot.IS_REAL) aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name); @@ -58,11 +51,11 @@ public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourc public void update() { aprilTagCameraIO.updateInputs(inputs); Logger.processInputs("Cameras/" + name, inputs); - robotPose = calculateBestRobotPose(); + robotPose = inputs.cameraSolvePNPPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); logEstimatedRobotPose(); - if (MotorSubsystem.isExtensiveLoggingEnabled() && !AprilTagCameraConstants.TAG_ID_TO_POSE.isEmpty()) - logVisibleTags(); + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); } public boolean hasNewResult() { @@ -80,69 +73,6 @@ public String getName() { public double getLatestResultTimestampSeconds() { return inputs.latestResultTimestampSeconds; } - - /** - * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. - * Different pose estimation strategies may use different formulae to calculate the standard deviations. - * - * @return the standard deviations for the pose estimation strategy used - */ - public Matrix calculateStandardDeviations() { - if (isWithinBestTagRangeForSolvePNP()) - return calculateSolvePNPStandardDeviations(); - return calculateAssumedHeadingStandardDeviations(); - } - - /** - * If the robot is close enough to the tag, it gets the estimated solve PNP pose. - * If it's too far, it assumes the robot's heading and calculates its position from there. - * Assuming the robot's heading is more robust, but won't fix current wrong heading. - * To fix this, we use solve PNP to reset the robot's heading when we are close enough for an accurate result. - * - * @return the robot's pose - */ - private Pose2d calculateBestRobotPose() { - final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); - return calculateAssumedRobotHeadingPose(isWithinBestTagRangeForSolvePNP() ? inputs.solvePNPHeading : gyroHeadingAtTimestamp); - } - - /** - * Calculates the robot's pose by assuming its heading, the apriltag's position, and the camera's position on the robot. - * This method of finding the robot's pose is more robust but relies on knowing the robot's heading beforehand. - * - * @return the robot's pose - */ - private Pose2d calculateAssumedRobotHeadingPose(Rotation2d robotHeading) { - if (inputs.visibleTagIDs.length == 0) - return null; - - final Translation2d robotFieldRelativeTranslation = getRobotFieldRelativeTranslation(robotHeading); - return new Pose2d(robotFieldRelativeTranslation, robotHeading); - } - - private Transform2d toTransform2d(Transform3d transform3d) { - return new Transform2d( - transform3d.getTranslation().toTranslation2d(), - transform3d.getRotation().toRotation2d() - ); - } - - private Translation2d getRobotFieldRelativeTranslation(Rotation2d robotHeading) { - if (!inputs.hasResult) - return new Translation2d(); - - Pose2d fieldRelativeRobotPose = PhotonUtils.estimateFieldToRobot( - robotCenterToCamera.getZ(), - AprilTagCameraConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).getZ(), - -robotCenterToCamera.getRotation().getY(), - -inputs.bestTargetRelativePitchRadians, - Rotation2d.fromRadians(inputs.bestTargetRelativeYawRadians), - robotHeading, - AprilTagCameraConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).toPose2d(), - toTransform2d(robotCenterToCamera.inverse()) - ); - return fieldRelativeRobotPose.getTranslation(); - } private boolean isNewTimestamp() { if (lastUpdatedTimestamp == getLatestResultTimestampSeconds()) @@ -152,23 +82,10 @@ private boolean isNewTimestamp() { return true; } - private Matrix calculateSolvePNPStandardDeviations() { + public Matrix getStandardDeviations() { final int numberOfVisibleTags = inputs.visibleTagIDs.length; - final double translationStandardDeviation = calculateStandardDeviations(solvePNPTranslationStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); - final double thetaStandardDeviation = calculateStandardDeviations(solvePNPThetaStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); - - return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); - } - - /** - * Calculates the standard deviations of the pose, for when the pose is calculated by assuming the robot's heading. - * The theta deviation is infinite so that it won't change anything in the heading because we assume it is correct. - * - * @return the standard deviations - */ - private Matrix calculateAssumedHeadingStandardDeviations() { - final double translationStandardDeviation = calculateStandardDeviations(assumedRobotPoseTranslationStandardDeviationsExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); - final double thetaStandardDeviation = Double.POSITIVE_INFINITY; + final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); + final double thetaStandardDeviation = calculateStandardDeviations(thetaStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); } @@ -186,10 +103,6 @@ private double calculateStandardDeviations(double exponent, double distance, int return exponent * (distance * distance) / numberOfVisibleTags; } - private boolean isWithinBestTagRangeForSolvePNP() { - return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; - } - private void logEstimatedRobotPose() { if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); @@ -197,15 +110,15 @@ private void logEstimatedRobotPose() { Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); } - private void logVisibleTags() { + private void logUsedTags() { if (!inputs.hasResult) { - Logger.recordOutput("VisibleTags/" + this.getName(), new Pose2d[0]); + Logger.recordOutput("UsedTags/" + this.getName(), new Pose3d[0]); return; } - final Pose2d[] visibleTagPoses = new Pose2d[inputs.visibleTagIDs.length]; - for (int i = 0; i < visibleTagPoses.length; i++) - visibleTagPoses[i] = AprilTagCameraConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]).toPose2d(); - Logger.recordOutput("VisibleTags/" + this.getName(), visibleTagPoses); + final Pose3d[] usedTagPoses = new Pose3d[inputs.visibleTagIDs.length]; + for (int i = 0; i < usedTagPoses.length; i++) + usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]); + Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 43c64c2e..8e09d6cb 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -1,28 +1,18 @@ package frc.trigon.robot.poseestimation.apriltagcamera; -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; -import java.util.HashMap; import java.util.function.Function; public class AprilTagCameraConstants { - public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); - static AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - static { - for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - TAG_ID_TO_POSE.put(aprilTag.ID, aprilTag.pose); - } - - static final double MAXIMUM_AMBIGUITY = 0.2; - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, -0.06, new Rotation3d(Units.degreesToRadians(-1.8), 0, 0)); static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; + public static final double MAXIMUM_AMBIGUITY = 0.2; public enum RobotPoseSourceType { PHOTON_CAMERA(AprilTagPhotonCameraIO::new), diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index c3f5cb52..d4f1f56f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -1,6 +1,6 @@ package frc.trigon.robot.poseestimation.apriltagcamera; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Pose3d; import org.littletonrobotics.junction.AutoLog; public class AprilTagCameraIO { @@ -11,10 +11,8 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { public static class RobotPoseSourceInputs { public boolean hasResult = false; public double latestResultTimestampSeconds = 0; - public Rotation2d solvePNPHeading = new Rotation2d(); + public Pose3d cameraSolvePNPPose = new Pose3d(); public int[] visibleTagIDs = new int[0]; - public double bestTargetRelativeYawRadians = 0; - public double bestTargetRelativePitchRadians = 0; public double averageDistanceFromAllTags = 0; public double distanceFromBestTag = 0; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 43c44519..5fe25a92 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,9 +1,6 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; @@ -19,6 +16,7 @@ public AprilTagLimelightIO(String hostname) { @Override protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); + inputs.hasResult = results.targets_Fiducials.length > 0; if (inputs.hasResult) @@ -28,20 +26,16 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { } private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { - final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); - - inputs.solvePNPHeading = results.getBotPose3d_wpiBlue().toPose2d().getRotation(); + inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue(); inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; inputs.visibleTagIDs = getVisibleTagIDs(results); - inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); - inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY(); inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results); inputs.distanceFromBestTag = getDistanceFromBestTag(results); } private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.visibleTagIDs = new int[0]; - inputs.solvePNPHeading = new Rotation2d(); + inputs.cameraSolvePNPPose = new Pose3d(); } private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { @@ -63,17 +57,6 @@ private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { return visibleTagIDs; } - /** - * Estimates the camera's rotation relative to the apriltag. - * - * @param results the camera's pipeline result - * @return the estimated rotation - */ - private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.LimelightResults results) { - final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results); - return new Rotation3d(0, Units.degreesToRadians(bestTag.tx), Units.degreesToRadians(bestTag.ty)); - } - private double getAverageDistanceFromAllTags(LimelightHelpers.LimelightResults results) { final LimelightHelpers.LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials; double totalDistanceFromTags = 0; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 4c75bc53..771854d8 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,20 +1,15 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; -import org.opencv.core.Point; import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.TargetCorner; - -import java.util.List; +import org.photonvision.targeting.PhotonTrackedTarget; public class AprilTagPhotonCameraIO extends AprilTagCameraIO { private final PhotonCamera photonCamera; @@ -35,12 +30,8 @@ protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { } private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { - final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); - - inputs.solvePNPHeading = getSolvePNPHeading(latestResult); + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); - inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(latestResult); inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); @@ -48,31 +39,7 @@ private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, Photo private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { inputs.visibleTagIDs = new int[]{}; - inputs.solvePNPHeading = new Rotation2d(); - } - - private Point getTagCenter(List tagCorners) { - double tagCornerSumX = 0; - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) { - tagCornerSumX += tagCorner.x; - tagCornerSumY += tagCorner.y; - } - return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); - } - - /** - * Estimates the camera's rotation relative to the apriltag. - * - * @param result the camera's pipeline result - * @return the estimated rotation - */ - private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - final List tagCorners = result.getBestTarget().getDetectedCorners(); - final Point tagCenter = getTagCenter(tagCorners); - if (photonCamera.getCameraMatrix().isPresent()) - return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); - return null; + inputs.cameraSolvePNPPose = new Pose3d(); } /** @@ -81,16 +48,22 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { * @param result the camera's pipeline result * @return the estimated pose */ - private Rotation2d getSolvePNPHeading(PhotonPipelineResult result) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; - final Pose3d estimatedPose = new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); - return estimatedPose.toPose2d().getRotation(); + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + final PNPResult multitagPose = result.getMultiTagResult().estimatedPose; + if (multitagPose.isPresent && multitagPose.ambiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY) { + final Transform3d cameraPoseTransform = multitagPose.best; + return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); - final Pose3d estimatedPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()).transformBy(targetToCamera); - return estimatedPose.toPose2d().getRotation(); + final PhotonTrackedTarget bestTarget = result.getBestTarget(); + if (bestTarget.getPoseAmbiguity() > AprilTagCameraConstants.MAXIMUM_AMBIGUITY) + return new Pose3d(); + + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(bestTarget.getFiducialId()); + final Transform3d targetToCamera = bestTarget.getBestCameraToTarget().inverse(); + return tagPose + .transformBy(targetToCamera) + .transformBy(AprilTagCameraConstants.TAG_OFFSET); } private int[] getVisibleTagIDs(PhotonPipelineResult result) { @@ -124,21 +97,4 @@ private double getAverageDistanceFromAllTags(PhotonPipelineResult result) { private double getDistanceFromBestTag(PhotonPipelineResult result) { return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); } - - private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { - double fx = camIntrinsics.get(0, 0); - double cx = camIntrinsics.get(0, 2); - double xOffset = cx - pixel.x; - - double fy = camIntrinsics.get(1, 1); - double cy = camIntrinsics.get(1, 2); - double yOffset = cy - pixel.y; - - // calculate yaw normally - var yaw = new Rotation2d(fx, xOffset); - // correct pitch based on yaw - var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); - - return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); - } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index e05f6145..9008083f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -111,7 +111,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera return new PoseEstimator6328.VisionObservation( robotPose, aprilTagCamera.getLatestResultTimestampSeconds(), - aprilTagCamera.calculateStandardDeviations() + aprilTagCamera.getStandardDeviations() ); } @@ -123,7 +123,7 @@ private Matrix averageDistanceToStdDevs(double averageDistance, int visi } private void putAprilTagsOnFieldWidget() { - for (Map.Entry entry : AprilTagCameraConstants.TAG_ID_TO_POSE.entrySet()) { + for (Map.Entry entry : FieldConstants.TAG_ID_TO_POSE.entrySet()) { final Pose2d tagPose = entry.getValue().toPose2d(); field.getObject("Tag " + entry.getKey()).setPose(tagPose); } From a408d1c6b9ff9dc8e0eda51f4283f8477d795be7 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Mon, 30 Sep 2024 20:14:44 +0300 Subject: [PATCH 047/261] pitcher ff --- build.gradle | 2 +- .../java/frc/trigon/robot/RobotContainer.java | 2 ++ .../robot/constants/RobotConstants.java | 2 +- .../robot/subsystems/pitcher/Pitcher.java | 3 +- .../subsystems/pitcher/PitcherConstants.java | 28 +++++++++++-------- 5 files changed, 23 insertions(+), 14 deletions(-) diff --git a/build.gradle b/build.gradle index a54357f9..ed54dec6 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.9' + implementation 'com.github.Programming-TRIGON:TRIGONLib:a7894fd5a48b5154e717cde3514c6ffd6ac1de69' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 7ec9d7ee..620b7aa6 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -89,6 +89,8 @@ private void bindControllerCommands() { OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand()); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); + + configureSysIdBindings(PITCHER); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index d7b6a1d1..9d53837e 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -6,7 +6,7 @@ public class RobotConstants { private static final boolean - IS_SIMULATION = true, + IS_SIMULATION = false, IS_REPLAY = false; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index 5fb63ff3..3628dc03 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -35,7 +35,7 @@ public Pitcher() { public void updateLog(SysIdRoutineLog log) { log.motor("Pitcher") .angularPosition(Units.Rotations.of(masterMotor.getSignal(TalonFXSignal.POSITION))) - .angularVelocity(Units.RotationsPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + .angularVelocity(Units.RotationsPerSecond.of(masterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / PitcherConstants.GEAR_RATIO)) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @@ -53,6 +53,7 @@ public void stop() { @Override public void updatePeriodically() { masterMotor.update(); + PitcherConstants.ENCODER.update(); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index cbb2f726..b29c6f53 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -16,6 +16,7 @@ import frc.trigon.robot.subsystems.ampaligner.AmpAlignerConstants; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; +import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.SingleJointedArmSimulation; @@ -44,22 +45,22 @@ public class PitcherConstants { P = RobotHardwareStats.isSimulation() ? 100 : 0, I = RobotHardwareStats.isSimulation() ? 0 : 0, D = RobotHardwareStats.isSimulation() ? 20 : 0, - KS = RobotHardwareStats.isSimulation() ? 0.2 : 0, - KV = RobotHardwareStats.isSimulation() ? 32 : 0, - KA = RobotHardwareStats.isSimulation() ? 0 : 0, - KG = RobotHardwareStats.isSimulation() ? 0.2 : 0; + KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.18078, + KV = RobotHardwareStats.isSimulation() ? 32 : 26.981, + KA = RobotHardwareStats.isSimulation() ? 0 : 1.4593, + KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.25961; private static final double - EXPO_KV = KV + 5, + EXPO_KV = KV + 40, EXPO_KA = 0.2; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; - private static final double GEAR_RATIO = 227.77777; + static final double GEAR_RATIO = 227.77777; private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(12), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(73); - private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.Clockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = 0; + private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.13898 + 0.5; private static final AbsoluteSensorRangeValue ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final boolean FOC_ENABLED = true; @@ -83,9 +84,9 @@ public class PitcherConstants { ); static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(5).per(Units.Second.of(1)), - Units.Volts.of(9), - Units.Second.of(1000) + Units.Volts.of(1).per(Units.Second.of(1)), + Units.Volts.of(2.5), + Units.Second.of(100) ); static final Pose3d PITCHER_VISUALIZATION_ORIGIN_POINT = new Pose3d(0.2521, 0, 0.15545, new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(-12), 0)); @@ -146,6 +147,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); } private static void configureFollowerMotor() { @@ -171,6 +173,10 @@ public static void configureEncoder() { config.MagnetSensor.AbsoluteSensorRange = ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE; ENCODER.applyConfiguration(config); + + ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); + ENCODER.registerSignal(CANcoderSignal.POSITION, 100); + ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); } } From c7f3f3cfcdb7174e22ea0ce13b10e10131c3c6ef Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 30 Sep 2024 23:21:11 +0300 Subject: [PATCH 048/261] Fixed pitcher (I think) and started shooter stuff --- src/main/java/frc/trigon/robot/RobotContainer.java | 3 +-- .../java/frc/trigon/robot/constants/RobotConstants.java | 4 ++-- .../trigon/robot/subsystems/pitcher/PitcherConstants.java | 7 ++++--- .../trigon/robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 620b7aa6..d8bdbe37 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -20,7 +20,6 @@ import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.pitcher.Pitcher; import frc.trigon.robot.subsystems.shooter.Shooter; -import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -90,7 +89,7 @@ private void bindControllerCommands() { OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand()); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); - configureSysIdBindings(PITCHER); + configureSysIdBindings(SHOOTER); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 9d53837e..dda4e7a0 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -9,8 +9,8 @@ public class RobotConstants { IS_SIMULATION = false, IS_REPLAY = false; private static final double PERIODIC_TIME_SECONDS = 0.02; - public static final String CANIVORE_NAME = "CANivore"; - public static final String LOGGING_PATH = IS_SIMULATION && Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; + public static final String CANIVORE_NAME = "SwerveCANivore"; + public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; public static void init() { RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY); diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index b29c6f53..07156a56 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -20,6 +20,7 @@ import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.SingleJointedArmSimulation; +import org.trigon.utilities.Conversions; import org.trigon.utilities.mechanisms.DoubleJointedArmMechanism2d; public class PitcherConstants { @@ -50,7 +51,7 @@ public class PitcherConstants { KA = RobotHardwareStats.isSimulation() ? 0 : 1.4593, KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.25961; private static final double - EXPO_KV = KV + 40, + EXPO_KV = KV + 80, EXPO_KA = 0.2; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; @@ -59,8 +60,8 @@ public class PitcherConstants { private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(12), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(73); - private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.13898 + 0.5; + private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.Clockwise_Positive; + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.13898 + 0.5 + Conversions.degreesToRotations(90 + 12); private static final AbsoluteSensorRangeValue ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final boolean FOC_ENABLED = true; diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index bb7c00a4..06bc1468 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -48,8 +48,8 @@ public class ShooterConstants { LEFT_SIMULATION = new FlywheelSimulation(LEFT_GEARBOX, GEAR_RATIO, MOMENT_OF_INERTIA); static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(0.25).per(Units.Second), - Units.Volts.of(7), + Units.Volts.of(5).per(Units.Second), + Units.Volts.of(9), Units.Second.of(1000) ); From 9af71d6b3c16df2f2710d838ebe8fe6432dea00b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Oct 2024 13:25:58 +0300 Subject: [PATCH 049/261] Calibrated shooter --- src/main/java/frc/trigon/robot/RobotContainer.java | 14 +++++++------- .../robot/subsystems/shooter/ShooterConstants.java | 8 ++++---- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index d8bdbe37..07f00f1a 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -51,12 +51,12 @@ private void configureBindings() { } private void bindDefaultCommands() { - SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); - INTAKE.setDefaultCommand(CommandConstants.DEFAULT_INTAKE_COMMAND); - CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); - AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); - PITCHER.setDefaultCommand(GeneralCommands.getDefaultPitcherCommand()); - SHOOTER.setDefaultCommand(ShooterCommands.getStopCommand()); +// SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); +// INTAKE.setDefaultCommand(CommandConstants.DEFAULT_INTAKE_COMMAND); +// CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); +// AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); +// PITCHER.setDefaultCommand(GeneralCommands.getDefaultPitcherCommand()); +// SHOOTER.setDefaultCommand(ShooterCommands.getStopCommand()); } private void bindControllerCommands() { @@ -89,7 +89,7 @@ private void bindControllerCommands() { OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand()); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); - configureSysIdBindings(SHOOTER); + configureSysIdBindings(AMP_ALIGNER); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 06bc1468..92e1898b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -28,12 +28,12 @@ public class ShooterConstants { LEFT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double - P = RobotHardwareStats.isSimulation() ? 15 : 0, + P = RobotHardwareStats.isSimulation() ? 15 : 12, I = RobotHardwareStats.isSimulation() ? 0 : 0, D = RobotHardwareStats.isSimulation() ? 0 : 0, - KS = RobotHardwareStats.isSimulation() ? 0.35586 : 0, - KV = RobotHardwareStats.isSimulation() ? 0 : 0, - KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0; + KS = RobotHardwareStats.isSimulation() ? 0.35586 : 4.019, + KV = RobotHardwareStats.isSimulation() ? 0 : 0.071122, + KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0.99531; private static final double GEAR_RATIO = 1; private static final int From b21eb8f92685d98d46e5163b4ec2588ba14a41d2 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Oct 2024 14:42:06 +0300 Subject: [PATCH 050/261] Updated pitcher constants. Still needs calibration --- .../subsystems/pitcher/PitcherConstants.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 07156a56..f53194d8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -43,23 +43,23 @@ public class PitcherConstants { private static final boolean FOLLOWER_OPPOSES_MASTER = true; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - P = RobotHardwareStats.isSimulation() ? 100 : 0, + P = RobotHardwareStats.isSimulation() ? 100 : 1, I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 20 : 0, + D = RobotHardwareStats.isSimulation() ? 20 : 4, KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.18078, KV = RobotHardwareStats.isSimulation() ? 32 : 26.981, KA = RobotHardwareStats.isSimulation() ? 0 : 1.4593, KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.25961; private static final double - EXPO_KV = KV + 80, - EXPO_KA = 0.2; + EXPO_KV = KV, + EXPO_KA = KA; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; static final double GEAR_RATIO = 227.77777; private static final Rotation2d - REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(12), - FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(73); + REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), + FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(72); private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.Clockwise_Positive; private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.13898 + 0.5 + Conversions.degreesToRotations(90 + 12); private static final AbsoluteSensorRangeValue ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; @@ -71,8 +71,8 @@ public class PitcherConstants { PITCHER_LENGTH_METERS = 0.5, PITCHER_MASS_KILOGRAMS = 11; private static final Rotation2d - PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(12), - PITCHER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(73); + PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(13), + PITCHER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(72); private static final boolean SIMULATE_GRAVITY = true; private static final SingleJointedArmSimulation SIMULATION = new SingleJointedArmSimulation( GEARBOX, @@ -98,8 +98,8 @@ public class PitcherConstants { new Color8Bit(Color.kGreen) ); - public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(12); - static final Rotation2d PITCH_TOLERANCE = Rotation2d.fromDegrees(2); + public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(13); + static final Rotation2d PITCH_TOLERANCE = Rotation2d.fromDegrees(1); public static final Transform3d VISUALIZATION_PITCHER_PIVOT_POINT_TO_HELD_NOTE = new Transform3d(0.24, 0, 0.02, new Rotation3d()); static { From 2344e020df780c045aace2cd1f24cb0bce5eb6a3 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 1 Oct 2024 17:17:51 +0300 Subject: [PATCH 051/261] updated gear ratio --- .../frc/trigon/robot/subsystems/pitcher/PitcherConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index f53194d8..172f22c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -56,7 +56,7 @@ public class PitcherConstants { private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; - static final double GEAR_RATIO = 227.77777; + static final double GEAR_RATIO = 279; private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(72); From 871c0f949429a39b31b594edf63ec81a810c5ae5 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sat, 5 Oct 2024 23:38:58 +0300 Subject: [PATCH 052/261] pitcher is still overshooting by 2 degrees --- .../subsystems/pitcher/PitcherConstants.java | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 172f22c2..179b7bcc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -20,7 +20,6 @@ import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.SingleJointedArmSimulation; -import org.trigon.utilities.Conversions; import org.trigon.utilities.mechanisms.DoubleJointedArmMechanism2d; public class PitcherConstants { @@ -43,15 +42,15 @@ public class PitcherConstants { private static final boolean FOLLOWER_OPPOSES_MASTER = true; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - P = RobotHardwareStats.isSimulation() ? 100 : 1, + P = RobotHardwareStats.isSimulation() ? 100 : 80, I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 20 : 4, - KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.18078, - KV = RobotHardwareStats.isSimulation() ? 32 : 26.981, - KA = RobotHardwareStats.isSimulation() ? 0 : 1.4593, - KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.25961; + D = RobotHardwareStats.isSimulation() ? 20 : 5, + KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.055145, + KV = RobotHardwareStats.isSimulation() ? 32 : 30.85, + KA = RobotHardwareStats.isSimulation() ? 0 : 0.67839, + KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.19735; private static final double - EXPO_KV = KV, + EXPO_KV = KV + 15, EXPO_KA = KA; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; @@ -61,7 +60,7 @@ public class PitcherConstants { REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(72); private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.Clockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.13898 + 0.5 + Conversions.degreesToRotations(90 + 12); + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.34932; private static final AbsoluteSensorRangeValue ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final boolean FOC_ENABLED = true; From 309c1785f5dc08c2366eb85caa460616cd574bda Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 6 Oct 2024 01:22:33 +0300 Subject: [PATCH 053/261] added shooter FF --- .../subsystems/shooter/ShooterConstants.java | 39 +++++++++++-------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 92e1898b..51f34f25 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -24,16 +24,23 @@ public class ShooterConstants { LEFT_MOTOR = new TalonFXMotor(LEFT_MOTOR_ID, LEFT_MOTOR_NAME); private static final InvertedValue - RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive, + RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive, LEFT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double - P = RobotHardwareStats.isSimulation() ? 15 : 12, - I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 0 : 0, - KS = RobotHardwareStats.isSimulation() ? 0.35586 : 4.019, - KV = RobotHardwareStats.isSimulation() ? 0 : 0.071122, - KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0.99531; + RIGHT_P = RobotHardwareStats.isSimulation() ? 15 : 0.30587, + RIGHT_I = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_D = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_KS = RobotHardwareStats.isSimulation() ? 0.35586 : 6.3793, + RIGHT_KV = RobotHardwareStats.isSimulation() ? 0 : 0.071122, + RIGHT_KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0.90291; + private static final double + LEFT_P = RobotHardwareStats.isSimulation() ? 15 : 0.18528, + LEFT_I = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_D = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_KS = RobotHardwareStats.isSimulation() ? 0.35586 : 5.0484, + LEFT_KV = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0.79623; private static final double GEAR_RATIO = 1; private static final int @@ -63,11 +70,11 @@ public class ShooterConstants { static final double VELOCITY_TOLERANCE = 3; static { - configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_SIMULATION); - configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_SIMULATION); + configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_SIMULATION, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_KS, RIGHT_KV, RIGHT_KA); + configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_SIMULATION, LEFT_P, LEFT_I, LEFT_D, LEFT_KS, LEFT_KV, LEFT_KA); } - private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, FlywheelSimulation simulation) { + private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, FlywheelSimulation simulation, double P, double I, double D, double KS, double KV, double KA) { final TalonFXConfiguration config = new TalonFXConfiguration(); config.Audio.BeepOnBoot = false; @@ -76,12 +83,12 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal config.MotorOutput.Inverted = invertedValue; config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; - config.Slot0.kP = P; - config.Slot0.kI = I; - config.Slot0.kD = D; - config.Slot0.kS = KS; - config.Slot0.kV = KV; - config.Slot0.kA = KA; + config.Slot0.kP = RIGHT_P; + config.Slot0.kI = RIGHT_I; + config.Slot0.kD = RIGHT_D; + config.Slot0.kS = RIGHT_KS; + config.Slot0.kV = RIGHT_KV; + config.Slot0.kA = RIGHT_KA; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; From 1295df8254ff7cbe851fe32f4602831c026b0f2c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 09:51:34 +0300 Subject: [PATCH 054/261] Completed shooter PID --- .../frc/trigon/robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 51f34f25..9cdc5ecd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -28,14 +28,14 @@ public class ShooterConstants { LEFT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double - RIGHT_P = RobotHardwareStats.isSimulation() ? 15 : 0.30587, + RIGHT_P = RobotHardwareStats.isSimulation() ? 15 : 10, RIGHT_I = RobotHardwareStats.isSimulation() ? 0 : 0, RIGHT_D = RobotHardwareStats.isSimulation() ? 0 : 0, RIGHT_KS = RobotHardwareStats.isSimulation() ? 0.35586 : 6.3793, RIGHT_KV = RobotHardwareStats.isSimulation() ? 0 : 0.071122, RIGHT_KA = RobotHardwareStats.isSimulation() ? 0.59136 : 0.90291; private static final double - LEFT_P = RobotHardwareStats.isSimulation() ? 15 : 0.18528, + LEFT_P = RobotHardwareStats.isSimulation() ? 15 : 10, LEFT_I = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_D = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_KS = RobotHardwareStats.isSimulation() ? 0.35586 : 5.0484, From 53105c3a92c0e7216fc5f3d60bd25a566dac4ee2 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 10:48:09 +0300 Subject: [PATCH 055/261] Calibrated AmpAligner --- .../ampaligner/AmpAlignerConstants.java | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java index e07e79ea..ddc2bddd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java @@ -18,21 +18,21 @@ public class AmpAlignerConstants { private static final String MOTOR_NAME = "AmpAlignerMotor"; static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); - private static final InvertedValue INVERTED_VALUE = InvertedValue.Clockwise_Positive; + private static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final ForwardLimitTypeValue FORWARD_LIMIT_TYPE_VALUE = ForwardLimitTypeValue.NormallyOpen; private static final ForwardLimitSourceValue FORWARD_LIMIT_SOURCE_VALUE = ForwardLimitSourceValue.LimitSwitchPin; private static final double - P = RobotHardwareStats.isSimulation() ? 50 : 0, + P = RobotHardwareStats.isSimulation() ? 50 : 20, I = RobotHardwareStats.isSimulation() ? 0 : 0, D = RobotHardwareStats.isSimulation() ? 0 : 0, - KS = RobotHardwareStats.isSimulation() ? 0.055582 : 0, - KV = RobotHardwareStats.isSimulation() ? 6 : 0, - KA = RobotHardwareStats.isSimulation() ? 0.12886 : 0; - static final double KG = RobotHardwareStats.isSimulation() ? 0.11971 : 0; + KS = RobotHardwareStats.isSimulation() ? 0.055582 : 0.37527, + KV = RobotHardwareStats.isSimulation() ? 6 : 4.5959, + KA = RobotHardwareStats.isSimulation() ? 0.12886 : 0.16118; + static final double KG = RobotHardwareStats.isSimulation() ? 0.11971 : 0.13609; private static final double - MOTION_MAGIC_ACCELERATION = RobotHardwareStats.isSimulation() ? 6 : 0, - MOTION_MAGIC_CRUISE_VELOCITY = RobotHardwareStats.isSimulation() ? 6 : 0; + MOTION_MAGIC_ACCELERATION = RobotHardwareStats.isSimulation() ? 6 : 5, + MOTION_MAGIC_CRUISE_VELOCITY = RobotHardwareStats.isSimulation() ? 6 : 5; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; private static final double GEAR_RATIO = 48; @@ -43,9 +43,9 @@ public class AmpAlignerConstants { private static final double AMP_ALIGNER_MASS_KILOGRAMS = 1.1; public static final double AMP_ALIGNER_LENGTH_METERS = 0.52; private static final Rotation2d - AMP_ALIGNER_MINIMUM_ANGLE = Rotation2d.fromDegrees(173.7 - 156), + AMP_ALIGNER_MINIMUM_ANGLE = Rotation2d.fromDegrees(17.7), AMP_ALIGNER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(173.7); - private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(173.7 - 156); + private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(17.7); private static final boolean SIMULATE_GRAVITY = true; private static final SingleJointedArmSimulation SIMULATION = new SingleJointedArmSimulation( GEARBOX, @@ -58,8 +58,8 @@ public class AmpAlignerConstants { ); static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(1.5).per(Units.Second.of(1)), - Units.Volts.of(3), + Units.Volts.of(1).per(Units.Second.of(1)), + Units.Volts.of(2), Units.Second.of(1000) ); @@ -117,7 +117,7 @@ private static void configureMotor() { } public enum AmpAlignerState { - OPEN(Rotation2d.fromDegrees(173.7 - 156)), + OPEN(Rotation2d.fromDegrees(17.7)), CLOSE(Rotation2d.fromDegrees(173.7)); public final Rotation2d targetAngle; From 4dabf6b60e5df9a11f031f941268485b472c93b4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 11:04:07 +0300 Subject: [PATCH 056/261] Updated amp shot values --- .../java/frc/trigon/robot/constants/ShootingConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 82924564..84679c70 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -13,11 +13,11 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, - AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 20, + AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, MANUAL_LOW_DELIVERY_SHOOTING_ROTATIONS_PER_SECOND = 45; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), - AMP_PITCH = Rotation2d.fromDegrees(45), + AMP_PITCH = Rotation2d.fromDegrees(47), MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(12); public static final Pose3d ROBOT_RELATIVE_PITCHER_PIVOT_POINT = new Pose3d(0.2521, 0, 0.15545, new Rotation3d(0, 0, Math.PI)); From 800662fe77e0c50e33e0e7b5f0c07a4c166b0d73 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 13:39:03 +0300 Subject: [PATCH 057/261] Calibrated Pitcher --- .../robot/subsystems/pitcher/PitcherConstants.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 179b7bcc..6bf75733 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -45,17 +45,17 @@ public class PitcherConstants { P = RobotHardwareStats.isSimulation() ? 100 : 80, I = RobotHardwareStats.isSimulation() ? 0 : 0, D = RobotHardwareStats.isSimulation() ? 20 : 5, - KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.055145, - KV = RobotHardwareStats.isSimulation() ? 32 : 30.85, - KA = RobotHardwareStats.isSimulation() ? 0 : 0.67839, - KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.19735; + KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.038694, + KV = RobotHardwareStats.isSimulation() ? 32 : 24.947, + KA = RobotHardwareStats.isSimulation() ? 0 : 1.8959, + KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.2976; private static final double EXPO_KV = KV + 15, - EXPO_KA = KA; + EXPO_KA = KA + 2; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; - static final double GEAR_RATIO = 279; + static final double GEAR_RATIO = 204; private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(72); From c6ac258157e1b0126de63a1d6b4eade327269164 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 13:41:19 +0300 Subject: [PATCH 058/261] Calibrated software limits for pitcher --- .../robot/subsystems/pitcher/PitcherConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 6bf75733..d15dd298 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -57,8 +57,8 @@ public class PitcherConstants { private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; static final double GEAR_RATIO = 204; private static final Rotation2d - REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), - FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(72); + REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(12), + FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(73); private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.Clockwise_Positive; private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.34932; private static final AbsoluteSensorRangeValue ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; @@ -70,8 +70,8 @@ public class PitcherConstants { PITCHER_LENGTH_METERS = 0.5, PITCHER_MASS_KILOGRAMS = 11; private static final Rotation2d - PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(13), - PITCHER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(72); + PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(12), + PITCHER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(73); private static final boolean SIMULATE_GRAVITY = true; private static final SingleJointedArmSimulation SIMULATION = new SingleJointedArmSimulation( GEARBOX, @@ -97,7 +97,7 @@ public class PitcherConstants { new Color8Bit(Color.kGreen) ); - public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(13); + public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(12); static final Rotation2d PITCH_TOLERANCE = Rotation2d.fromDegrees(1); public static final Transform3d VISUALIZATION_PITCHER_PIVOT_POINT_TO_HELD_NOTE = new Transform3d(0.24, 0, 0.02, new Rotation3d()); From 1a8ae304ce8a30183d33d96ab709a7d95d99f2a0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 17:19:00 +0300 Subject: [PATCH 059/261] Finished Pitcher calibrations --- build.gradle | 2 +- .../robot/constants/ShootingConstants.java | 6 ++-- .../robot/subsystems/pitcher/Pitcher.java | 6 ++-- .../subsystems/pitcher/PitcherConstants.java | 36 ++++++++++--------- 4 files changed, 27 insertions(+), 23 deletions(-) diff --git a/build.gradle b/build.gradle index ed54dec6..756f8df6 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:a7894fd5a48b5154e717cde3514c6ffd6ac1de69' + implementation 'com.github.Programming-TRIGON:TRIGONLib:v2024.2.11' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 84679c70..f68d1e52 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -13,12 +13,12 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, - AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, + AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 35, MANUAL_LOW_DELIVERY_SHOOTING_ROTATIONS_PER_SECOND = 45; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), - AMP_PITCH = Rotation2d.fromDegrees(47), - MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(12); + AMP_PITCH = Rotation2d.fromDegrees(46), + MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(13); public static final Pose3d ROBOT_RELATIVE_PITCHER_PIVOT_POINT = new Pose3d(0.2521, 0, 0.15545, new Rotation3d(0, 0, Math.PI)); public static final Transform3d PITCHER_PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.4209, 0, -0.0165, new Rotation3d()); diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index 3628dc03..66ca2136 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -1,6 +1,6 @@ package frc.trigon.robot.subsystems.pitcher; -import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,7 +23,7 @@ public class Pitcher extends MotorSubsystem { private final TalonFXMotor masterMotor = PitcherConstants.MASTER_MOTOR, followerMotor = PitcherConstants.FOLLOWER_MOTOR; - private final MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED); + private final MotionMagicVoltage positionRequest = new MotionMagicVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED); private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(PitcherConstants.FOC_ENABLED); private Rotation2d targetPitch = PitcherConstants.DEFAULT_PITCH; @@ -34,7 +34,7 @@ public Pitcher() { @Override public void updateLog(SysIdRoutineLog log) { log.motor("Pitcher") - .angularPosition(Units.Rotations.of(masterMotor.getSignal(TalonFXSignal.POSITION))) + .angularPosition(Units.Rotations.of(masterMotor.getSignal(TalonFXSignal.ROTOR_POSITION) / PitcherConstants.GEAR_RATIO)) .angularVelocity(Units.RotationsPerSecond.of(masterMotor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / PitcherConstants.GEAR_RATIO)) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index d15dd298..5a1e49fc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -20,6 +20,7 @@ import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.SingleJointedArmSimulation; +import org.trigon.utilities.Conversions; import org.trigon.utilities.mechanisms.DoubleJointedArmMechanism2d; public class PitcherConstants { @@ -42,25 +43,26 @@ public class PitcherConstants { private static final boolean FOLLOWER_OPPOSES_MASTER = true; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - P = RobotHardwareStats.isSimulation() ? 100 : 80, + P = RobotHardwareStats.isSimulation() ? 100 : 0.3, I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 20 : 5, - KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.038694, - KV = RobotHardwareStats.isSimulation() ? 32 : 24.947, - KA = RobotHardwareStats.isSimulation() ? 0 : 1.8959, - KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.2976; + D = RobotHardwareStats.isSimulation() ? 20 : 0.1, + KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.38, + KV = RobotHardwareStats.isSimulation() ? 32 : 22, + KA = RobotHardwareStats.isSimulation() ? 0 : 0, + KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.308; private static final double - EXPO_KV = KV + 15, - EXPO_KA = KA + 2; + MOTION_MAGIC_CRUISE_VELOCITY = 0.35, + MOTION_MAGIC_ACCELERATION = 3, + MOTION_MAGIC_JERK = 32; private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; - private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseVelocitySign; + private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseClosedLoopSign; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; - static final double GEAR_RATIO = 204; + static final double GEAR_RATIO = 227.77777; private static final Rotation2d - REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(12), + REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(73); private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.Clockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.34932; + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.34932 + Conversions.degreesToRotations(360); private static final AbsoluteSensorRangeValue ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; static final boolean FOC_ENABLED = true; @@ -70,7 +72,7 @@ public class PitcherConstants { PITCHER_LENGTH_METERS = 0.5, PITCHER_MASS_KILOGRAMS = 11; private static final Rotation2d - PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(12), + PITCHER_MINIMUM_ANGLE = Rotation2d.fromDegrees(13), PITCHER_MAXIMUM_ANGLE = Rotation2d.fromDegrees(73); private static final boolean SIMULATE_GRAVITY = true; private static final SingleJointedArmSimulation SIMULATION = new SingleJointedArmSimulation( @@ -97,7 +99,7 @@ public class PitcherConstants { new Color8Bit(Color.kGreen) ); - public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(12); + public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(13); static final Rotation2d PITCH_TOLERANCE = Rotation2d.fromDegrees(1); public static final Transform3d VISUALIZATION_PITCHER_PIVOT_POINT_TO_HELD_NOTE = new Transform3d(0.24, 0, 0.02, new Rotation3d()); @@ -124,8 +126,9 @@ private static void configureMasterMotor() { config.Slot0.kA = KA; config.Slot0.kG = KG; - config.MotionMagic.MotionMagicExpo_kV = EXPO_KV; - config.MotionMagic.MotionMagicExpo_kA = EXPO_KA; + config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_CRUISE_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; config.Slot0.GravityType = GRAVITY_TYPE_VALUE; config.Slot0.StaticFeedforwardSign = STATIC_FEEDFORWARD_SIGN_VALUE; @@ -148,6 +151,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); } private static void configureFollowerMotor() { From aa524bbfad8f88e37cb9639180ea11235b2081b0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 19:23:02 +0300 Subject: [PATCH 060/261] Actually finished pitcher calibrations --- .../trigon/robot/constants/ShootingConstants.java | 2 +- .../robot/subsystems/pitcher/PitcherConstants.java | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index f68d1e52..0dc70d34 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -13,7 +13,7 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, - AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 35, + AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, MANUAL_LOW_DELIVERY_SHOOTING_ROTATIONS_PER_SECOND = 45; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 5a1e49fc..4677a4f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -43,13 +43,13 @@ public class PitcherConstants { private static final boolean FOLLOWER_OPPOSES_MASTER = true; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - P = RobotHardwareStats.isSimulation() ? 100 : 0.3, + P = RobotHardwareStats.isSimulation() ? 100 : 0.5, I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 20 : 0.1, - KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.38, - KV = RobotHardwareStats.isSimulation() ? 32 : 22, + D = RobotHardwareStats.isSimulation() ? 20 : 0.6, + KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.3, + KV = RobotHardwareStats.isSimulation() ? 32 : 23, KA = RobotHardwareStats.isSimulation() ? 0 : 0, - KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.308; + KG = RobotHardwareStats.isSimulation() ? 0.2 : 0.37; private static final double MOTION_MAGIC_CRUISE_VELOCITY = 0.35, MOTION_MAGIC_ACCELERATION = 3, @@ -57,7 +57,7 @@ public class PitcherConstants { private static final GravityTypeValue GRAVITY_TYPE_VALUE = GravityTypeValue.Arm_Cosine; private static final StaticFeedforwardSignValue STATIC_FEEDFORWARD_SIGN_VALUE = StaticFeedforwardSignValue.UseClosedLoopSign; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; - static final double GEAR_RATIO = 227.77777; + static final double GEAR_RATIO = 200; private static final Rotation2d REVERSE_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(13), FORWARD_SOFT_LIMIT_THRESHOLD = Rotation2d.fromDegrees(73); From 85ee672041decaa15da45cf9f44bcc55d83d44be Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Oct 2024 21:12:29 +0300 Subject: [PATCH 061/261] Updated `RobotContainer` --- src/main/java/frc/trigon/robot/RobotContainer.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 8bf76ad0..c57c725c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -18,7 +18,6 @@ import frc.trigon.robot.subsystems.ampaligner.AmpAligner; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; -import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.pitcher.Pitcher; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; @@ -53,13 +52,12 @@ private void configureBindings() { } private void bindDefaultCommands() { - SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); +// SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); INTAKE.setDefaultCommand(CommandConstants.DEFAULT_INTAKE_COMMAND); - CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); +// CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); PITCHER.setDefaultCommand(GeneralCommands.getDefaultPitcherCommand()); SHOOTER.setDefaultCommand(ShooterCommands.getStopCommand()); - LEDStrip.setDefaultCommandForAllLEDS(CommandConstants.DEFAULT_LEDS_COMMAND); } private void bindControllerCommands() { @@ -91,8 +89,6 @@ private void bindControllerCommands() { OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand()); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); - - configureSysIdBindings(AMP_ALIGNER); } private void configureSysIdBindings(MotorSubsystem subsystem) { From 78936ba60c60930aa50b1e276d75997c3923e226 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 6 Oct 2024 21:23:36 +0300 Subject: [PATCH 062/261] added DIstance Sensor Channel --- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index e7c46438..2ce77dd0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -20,7 +20,7 @@ public class IntakeConstants { private static final int MASTER_MOTOR_ID = 16, FOLLOWER_MOTOR_ID = 17, - DISTANCE_SENSOR_CHANNEL = 0; + DISTANCE_SENSOR_CHANNEL = 2; private static final String MASTER_MOTOR_NAME = "MasterIntakeMotor", FOLLOWER_MOTOR_NAME = "FollowerIntakeMotor", From 983b1b4a1ff570c23ad6a3f20f542690e4e2a3b6 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 6 Oct 2024 21:25:50 +0300 Subject: [PATCH 063/261] readded LEDs default command --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index c57c725c..2717bd3e 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.trigon.robot.subsystems.ampaligner.AmpAligner; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; +import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.pitcher.Pitcher; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; @@ -58,6 +59,7 @@ private void bindDefaultCommands() { AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); PITCHER.setDefaultCommand(GeneralCommands.getDefaultPitcherCommand()); SHOOTER.setDefaultCommand(ShooterCommands.getStopCommand()); + LEDStrip.setDefaultCommandForAllLEDS(CommandConstants.DEFAULT_LEDS_COMMAND); } private void bindControllerCommands() { From a61adb3e977f990d80b19a8a2679f906bc5acb9f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 10:42:56 +0300 Subject: [PATCH 064/261] Removed error --- .../robot/poseestimation/poseestimator/PoseEstimator.java | 2 +- .../trigon/robot/subsystems/swerve/SwerveConstants.java | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 8efb7c69..13c12f3e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -107,7 +107,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera return new PoseEstimator6328.VisionObservation( robotPose, aprilTagCamera.getLatestResultTimestampSeconds(), - aprilTagCamera.getStandardDeviations() + aprilTagCamera.calculateStandardDeviations() ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 5dfb031f..7a12c5e9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -27,10 +27,10 @@ public class SwerveConstants { GYRO_MOUNT_POSITION_PITCH = 0, GYRO_MOUNT_POSITION_ROLL = 0; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(225.263672 - 360), - FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-256.904297 + 360), - REAR_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(108.369141), - REAR_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-36.035156); + FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0), + FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0), + REAR_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0), + REAR_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0); private static final int FRONT_LEFT_ID = 1, FRONT_RIGHT_ID = 2, From 56bf439f81a877d05695ffc31b35e25fa13c874a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 10:46:47 +0300 Subject: [PATCH 065/261] Fix --- .../trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java index ddc2bddd..90e38765 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java @@ -118,7 +118,7 @@ private static void configureMotor() { public enum AmpAlignerState { OPEN(Rotation2d.fromDegrees(17.7)), - CLOSE(Rotation2d.fromDegrees(173.7)); + CLOSE(Rotation2d.fromDegrees(173)); public final Rotation2d targetAngle; From 1c91765038903646abe3d99de6dd570327619598 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 11:01:31 +0300 Subject: [PATCH 066/261] Replaced deleted lines from vision fixes --- src/main/java/frc/trigon/robot/RobotContainer.java | 3 ++- .../frc/trigon/robot/commands/CommandConstants.java | 1 + .../frc/trigon/robot/constants/CameraConstants.java | 5 ++++- .../frc/trigon/robot/constants/OperatorConstants.java | 1 + .../poseestimation/poseestimator/PoseEstimator.java | 11 +++++++++++ 5 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2717bd3e..d78b797b 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -53,7 +53,7 @@ private void configureBindings() { } private void bindDefaultCommands() { -// SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); + SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); INTAKE.setDefaultCommand(CommandConstants.DEFAULT_INTAKE_COMMAND); // CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); @@ -64,6 +64,7 @@ private void bindDefaultCommands() { private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); + OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.whileTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 3f1473e4..0cafd238 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -45,6 +45,7 @@ public class CommandConstants { () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), false), new Rotation2d()).get())), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 7726b546..888e8c67 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -3,6 +3,9 @@ import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; public class CameraConstants { + public static final double + TRANSLATIONS_STD_EXPONENT = 0.005, + THETA_STD_EXPONENT = 0.01; + public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); - //todo: implement the rest of the CameraConstants } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f6815125..ab9460dc 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -21,6 +21,7 @@ public class OperatorConstants { public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER = DRIVER_CONTROLLER.b(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 13c12f3e..4b3447a5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -80,6 +80,17 @@ public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPos poseEstimator6328.addOdometryObservation(new PoseEstimator6328.OdometryObservation(swerveWheelPositions[i], gyroRotations[i], timestamps[i])); } + public void setGyroHeadingToBestSolvePNPHeading() { + int closestCameraToTag = 0; + for (int i = 0; i < aprilTagCameras.length; i++) { + if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) + closestCameraToTag = i; + } + + final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + } + private void updateFromVision() { getViableVisionObservations().stream() .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp)) From ff25df5e24714db2c1a9a33c91bf7f2b6033fc26 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 12:33:38 +0300 Subject: [PATCH 067/261] Added camera constants (not pitch) --- .../java/frc/trigon/robot/RobotContainer.java | 6 +++- .../robot/constants/CameraConstants.java | 33 ++++++++++++++++++- 2 files changed, 37 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index d78b797b..6012e2a6 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -12,6 +12,7 @@ import frc.trigon.robot.commands.factories.AmpCommands; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.commands.factories.ShootingCommands; +import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -32,7 +33,10 @@ public class RobotContainer { public static final Pitcher PITCHER = new Pitcher(); public static final Shooter SHOOTER = new Shooter(); public static final AmpAligner AMP_ALIGNER = new AmpAligner(); - public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator(); + public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( + CameraConstants.FRONT_TAG_CAMERA, + CameraConstants.REAR_TAG_CAMERA + ); private LoggedDashboardChooser autoChooser; public RobotContainer() { diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 888e8c67..047a0904 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -1,11 +1,42 @@ package frc.trigon.robot.constants; +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.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; public class CameraConstants { public static final double TRANSLATIONS_STD_EXPONENT = 0.005, THETA_STD_EXPONENT = 0.01; - + + private static final Transform3d + FRONT_CENTER_TO_CAMERA = new Transform3d( + new Translation3d(0.0465, 0.325, 0), + new Rotation3d(0, Units.degreesToRadians(0), 0) + ), + REAR_CENTER_TO_CAMERA = new Transform3d( + new Translation3d(0, 0.325 - 0.00975, 0), + new Rotation3d(0, Units.degreesToRadians(0), Units.degreesToRadians(180)) + ); + + public static final AprilTagCamera + FRONT_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "frontTagCamera", + FRONT_CENTER_TO_CAMERA, + THETA_STD_EXPONENT, + TRANSLATIONS_STD_EXPONENT + ), + REAR_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "rearTagCamera", + REAR_CENTER_TO_CAMERA, + THETA_STD_EXPONENT, + TRANSLATIONS_STD_EXPONENT + ); public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); } From cbda21dba706350ca533e1a105a461f51c9df362 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 13:26:46 +0300 Subject: [PATCH 068/261] Updated camera heights --- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 047a0904..4ebdf849 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -15,11 +15,11 @@ public class CameraConstants { private static final Transform3d FRONT_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0.0465, 0.325, 0), + new Translation3d(0.0465, 0.325, 0.192), new Rotation3d(0, Units.degreesToRadians(0), 0) ), REAR_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0, 0.325 - 0.00975, 0), + new Translation3d(0, 0.325 - 0.00975, 0.95), new Rotation3d(0, Units.degreesToRadians(0), Units.degreesToRadians(180)) ); From 8f7743c221f2af5d9ec6b93c0c31490b4b97358f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 14:01:57 +0300 Subject: [PATCH 069/261] Updated Photon lib --- .../java/frc/trigon/robot/RobotContainer.java | 5 +- .../io/AprilTagPhotonCameraIO.java | 4 +- vendordeps/photonlib.json | 110 +++++++++--------- 3 files changed, 59 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 6012e2a6..c8ddb5b0 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -12,7 +12,6 @@ import frc.trigon.robot.commands.factories.AmpCommands; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.commands.factories.ShootingCommands; -import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -34,8 +33,8 @@ public class RobotContainer { public static final Shooter SHOOTER = new Shooter(); public static final AmpAligner AMP_ALIGNER = new AmpAligner(); public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( - CameraConstants.FRONT_TAG_CAMERA, - CameraConstants.REAR_TAG_CAMERA +// CameraConstants.FRONT_TAG_CAMERA, +// CameraConstants.REAR_TAG_CAMERA ); private LoggedDashboardChooser autoChooser; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 0b8657d9..8f9e59de 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -81,8 +81,8 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { * @return the estimated pose */ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; + if (result.getMultiTagResult().isPresent()) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 0e80a16c..44d5c47b 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,57 +1,57 @@ { - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.3.1", - "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.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2024.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.3.1" - } - ] + "fileName": "photonlib.json", + "name": "photonlib", + "version": "dev-v2024.3.0-84-gcd9dd072", + "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": "dev-v2024.3.0-84-gcd9dd072", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "dev-v2024.3.0-84-gcd9dd072", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "dev-v2024.3.0-84-gcd9dd072" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "dev-v2024.3.0-84-gcd9dd072" + } + ] } \ No newline at end of file From 94d8996bfd85dfea1730ed0fd771b4123dc12a2a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 18:01:10 +0300 Subject: [PATCH 070/261] Updated TRIGONLib --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 756f8df6..ca4b07cb 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:v2024.2.11' + implementation 'com.github.Programming-TRIGON:TRIGONLib:v2024.2.12' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" From d05366fdbe88a39967684f1d0f917e4b9f70aef9 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 20:50:55 +0300 Subject: [PATCH 071/261] huigch --- .../frc/trigon/robot/subsystems/intake/Intake.java | 7 ++++++- .../robot/subsystems/intake/IntakeConstants.java | 10 +++++----- .../robot/subsystems/swerve/SwerveConstants.java | 9 ++++----- .../trigon/robot/subsystems/swerve/SwerveModule.java | 4 ++-- .../robot/subsystems/swerve/SwerveModuleConstants.java | 4 ++-- 5 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 20674f31..51a37633 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -10,9 +10,12 @@ import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; +import org.littletonrobotics.junction.Logger; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; +import static frc.trigon.robot.subsystems.intake.IntakeConstants.DISTANCE_SENSOR; + public class Intake extends MotorSubsystem { private final TalonFXMotor masterMotor = IntakeConstants.MASTER_MOTOR; private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(IntakeConstants.FOC_ENABLED); @@ -26,12 +29,14 @@ public Intake() { @Override public void updatePeriodically() { masterMotor.update(); - IntakeConstants.DISTANCE_SENSOR.updateSensor(); + DISTANCE_SENSOR.updateSensor(); } @Override public void updateMechanism() { IntakeConstants.MECHANISM.update(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); + Logger.recordOutput("HasNote", hasNote()); + Logger.recordOutput("distanceSensorScaledValue", DISTANCE_SENSOR.getScaledValue()); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index d2cfb7ae..3822296c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -49,8 +49,8 @@ public class IntakeConstants { GEAR_RATIO, MOMENT_OF_INERTIA ); - private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 5; - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER = () -> SimulationObjectDetectionCameraIO.HAS_OBJECTS ? NOTE_DISTANCE_THRESHOLD_CENTIMETERS - 1 : NOTE_DISTANCE_THRESHOLD_CENTIMETERS + 1; + private static final double NOTE_DISTANCE_THRESHOLD = 825; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER = () -> SimulationObjectDetectionCameraIO.HAS_OBJECTS ? NOTE_DISTANCE_THRESHOLD - 1 : NOTE_DISTANCE_THRESHOLD + 1; private static final double MAX_DISPLAYABLE_VELOCITY = 12; static final SpeedMechanism2d MECHANISM = new SpeedMechanism2d( @@ -59,10 +59,10 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.6; + static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.22; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS + () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); static final double NOTE_STOPPING_SECONDS = 1; private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate @@ -122,7 +122,7 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(5), //TODO: calibrate + COLLECT(2.5), //TODO: calibrate EJECT(-2), //TODO: calibrate STOP(0), FEED_SHOOTING(8), //TODO: calibrate diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index b1bf8934..1e68cf31 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -15,7 +15,6 @@ import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Gyro; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Signal; -import org.trigon.utilities.Conversions; import java.util.function.DoubleSupplier; @@ -27,10 +26,10 @@ public class SwerveConstants { GYRO_MOUNT_POSITION_PITCH = 0, GYRO_MOUNT_POSITION_ROLL = 0; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0), - FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0), - REAR_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0), - REAR_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(0); + FRONT_LEFT_STEER_ENCODER_OFFSET = 0.936, + FRONT_RIGHT_STEER_ENCODER_OFFSET = 0.562, + REAR_LEFT_STEER_ENCODER_OFFSET = 0.956, + REAR_RIGHT_STEER_ENCODER_OFFSET = -0.007; private static final int FRONT_LEFT_ID = 1, FRONT_RIGHT_ID = 2, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index 4766cde2..c33811b8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -33,7 +33,7 @@ public class SwerveModule { public SwerveModule(int moduleID, double offsetRotations) { driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME); steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME); - steerEncoder = new CANcoderEncoder(moduleID, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); + steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); configureHardware(offsetRotations); } @@ -182,7 +182,7 @@ private void configureSignals() { steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - + steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 85b215f0..42b20d4c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -55,8 +55,8 @@ public class SwerveModuleConstants { STEER_MOTOR_GEARBOX = DCMotor.getFalcon500Foc(STEER_MOTOR_AMOUNT); static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(5).per(Units.Second), - Units.Volts.of(20), + Units.Volts.of(3).per(Units.Second), + Units.Volts.of(2), Units.Second.of(1000) ); From f21ffd3089d4d10ecb738aca1a66a7d6c6dfc012 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 20:57:38 +0300 Subject: [PATCH 072/261] 3 > 2.5 --- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 3822296c..2bd2b6e9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -122,7 +122,7 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(2.5), //TODO: calibrate + COLLECT(3), //TODO: calibrate EJECT(-2), //TODO: calibrate STOP(0), FEED_SHOOTING(8), //TODO: calibrate From 68d442b79a52f47988bef887820bd6193cbcee25 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 7 Oct 2024 21:08:39 +0300 Subject: [PATCH 073/261] corrected collection indication rumbling --- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 51a37633..fa73d828 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -85,7 +85,7 @@ void setTargetVoltage(double targetVoltage) { * Indicates to the driver that a note has been collected by rumbling the controller and flashing the robot's LEDs. */ void indicateCollection() { - if (DriverStation.isAutonomous()) + if (!DriverStation.isAutonomous()) OperatorConstants.DRIVER_CONTROLLER.rumble(IntakeConstants.RUMBLE_DURATION_SECONDS, IntakeConstants.RUMBLE_POWER); getCollectionIndicationLEDsCommand().schedule(); } From 5a3b2cec335d4b33ca79055569fca5d99d4e13f1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 7 Oct 2024 23:13:31 +0300 Subject: [PATCH 074/261] Swerve Offsets --- .../trigon/robot/subsystems/swerve/SwerveConstants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 1e68cf31..15c9dfeb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -26,10 +26,10 @@ public class SwerveConstants { GYRO_MOUNT_POSITION_PITCH = 0, GYRO_MOUNT_POSITION_ROLL = 0; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = 0.936, - FRONT_RIGHT_STEER_ENCODER_OFFSET = 0.562, - REAR_LEFT_STEER_ENCODER_OFFSET = 0.956, - REAR_RIGHT_STEER_ENCODER_OFFSET = -0.007; + FRONT_LEFT_STEER_ENCODER_OFFSET = 3.94, + FRONT_RIGHT_STEER_ENCODER_OFFSET = 5.058, + REAR_LEFT_STEER_ENCODER_OFFSET = 4.959, + REAR_RIGHT_STEER_ENCODER_OFFSET = 3.488; private static final int FRONT_LEFT_ID = 1, FRONT_RIGHT_ID = 2, From 5521507967945e951ca12dca4871a32f8c97c1c6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 8 Oct 2024 11:19:35 +0300 Subject: [PATCH 075/261] after first field test --- src/main/java/frc/trigon/robot/RobotContainer.java | 9 +++++---- .../trigon/robot/constants/CameraConstants.java | 2 +- .../trigon/robot/constants/ShootingConstants.java | 4 ++-- .../poseestimator/PoseEstimator.java | 2 ++ .../robot/subsystems/intake/IntakeConstants.java | 4 ++-- .../robot/subsystems/swerve/SwerveConstants.java | 14 +++++++------- .../subsystems/swerve/SwerveModuleConstants.java | 4 ++-- 7 files changed, 21 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 12287bfa..0ebfe6b2 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -13,7 +13,6 @@ import frc.trigon.robot.commands.factories.AutonomousCommands; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.commands.factories.ShootingCommands; -import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -29,8 +28,8 @@ public class RobotContainer { public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( - CameraConstants.FRONT_TAG_CAMERA, - CameraConstants.REAR_TAG_CAMERA +// CameraConstants.FRONT_TAG_CAMERA, +// CameraConstants.REAR_TAG_CAMERA ); public static final Swerve SWERVE = new Swerve(); public static final Intake INTAKE = new Intake(); @@ -73,7 +72,7 @@ private void bindControllerCommands() { OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); +// OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); OperatorConstants.ALIGN_TO_SPEAKER_TRIGGER.whileTrue(CommandConstants.FACE_SPEAKER_COMMAND); OperatorConstants.ALIGN_TO_RIGHT_STAGE.whileTrue(CommandConstants.ALIGN_TO_RIGHT_STAGE_COMMAND); OperatorConstants.ALIGN_TO_LEFT_STAGE.whileTrue(CommandConstants.ALIGN_TO_LEFT_STAGE_COMMAND); @@ -99,6 +98,8 @@ private void bindControllerCommands() { OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); + +// configureSysIdBindings(SWERVE); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 4ebdf849..5989d3a3 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -20,7 +20,7 @@ public class CameraConstants { ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(0, 0.325 - 0.00975, 0.95), - new Rotation3d(0, Units.degreesToRadians(0), Units.degreesToRadians(180)) + new Rotation3d(180, Units.degreesToRadians(0), Units.degreesToRadians(180)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 8dd776af..1e9bb5d3 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -13,13 +13,13 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, - AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 20, + AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 45, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), - AMP_PITCH = Rotation2d.fromDegrees(45), + AMP_PITCH = Rotation2d.fromDegrees(49), MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(12), EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12), CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 4b3447a5..b768efb7 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -81,6 +81,8 @@ public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPos } public void setGyroHeadingToBestSolvePNPHeading() { + if (aprilTagCameras.length == 0) + return; int closestCameraToTag = 0; for (int i = 0; i < aprilTagCameras.length; i++) { if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 2bd2b6e9..50601d45 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -59,7 +59,7 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.22; + static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.01; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD @@ -122,7 +122,7 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(3), //TODO: calibrate + COLLECT(4), //TODO: calibrate EJECT(-2), //TODO: calibrate STOP(0), FEED_SHOOTING(8), //TODO: calibrate diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 15c9dfeb..d905bc54 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -22,14 +22,14 @@ public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_YAW = 0, - GYRO_MOUNT_POSITION_PITCH = 0, - GYRO_MOUNT_POSITION_ROLL = 0; + GYRO_MOUNT_POSITION_YAW = 101.195068, + GYRO_MOUNT_POSITION_PITCH = 0.175781, + GYRO_MOUNT_POSITION_ROLL = 0.043945; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = 3.94, - FRONT_RIGHT_STEER_ENCODER_OFFSET = 5.058, - REAR_LEFT_STEER_ENCODER_OFFSET = 4.959, - REAR_RIGHT_STEER_ENCODER_OFFSET = 3.488; + FRONT_LEFT_STEER_ENCODER_OFFSET = -1.561, + FRONT_RIGHT_STEER_ENCODER_OFFSET = -3.431, + REAR_LEFT_STEER_ENCODER_OFFSET = -1.537, + REAR_RIGHT_STEER_ENCODER_OFFSET = -2.008; private static final int FRONT_LEFT_ID = 1, FRONT_RIGHT_ID = 2, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 42b20d4c..4c491a41 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -55,8 +55,8 @@ public class SwerveModuleConstants { STEER_MOTOR_GEARBOX = DCMotor.getFalcon500Foc(STEER_MOTOR_AMOUNT); static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(3).per(Units.Second), - Units.Volts.of(2), + Units.Volts.of(5).per(Units.Second), + Units.Volts.of(8), Units.Second.of(1000) ); From 50a68988536dc89afc5c45bd4142eab2bc596811 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Tue, 8 Oct 2024 11:32:16 +0300 Subject: [PATCH 076/261] corrected scaling --- .../robot/subsystems/intake/IntakeConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 50601d45..17d64abc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -37,8 +37,8 @@ public class IntakeConstants { private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double GEAR_RATIO = 1.5; private static final double - DISTANCE_SENSOR_SCALING_SLOPE = 0.0004, - DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = 400; + DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; static final boolean FOC_ENABLED = true; private static final int MOTOR_AMOUNT = 2; @@ -49,8 +49,8 @@ public class IntakeConstants { GEAR_RATIO, MOMENT_OF_INERTIA ); - private static final double NOTE_DISTANCE_THRESHOLD = 825; - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER = () -> SimulationObjectDetectionCameraIO.HAS_OBJECTS ? NOTE_DISTANCE_THRESHOLD - 1 : NOTE_DISTANCE_THRESHOLD + 1; + private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 5; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER = () -> SimulationObjectDetectionCameraIO.HAS_OBJECTS ? NOTE_DISTANCE_THRESHOLD_CENTIMETERS - 1 : NOTE_DISTANCE_THRESHOLD_CENTIMETERS + 1; private static final double MAX_DISPLAYABLE_VELOCITY = 12; static final SpeedMechanism2d MECHANISM = new SpeedMechanism2d( @@ -62,7 +62,7 @@ public class IntakeConstants { static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.01; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD + () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); static final double NOTE_STOPPING_SECONDS = 1; private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate From 5ea797718d5c26f0a1b041dd808ad6299f8d225d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 8 Oct 2024 12:18:32 +0300 Subject: [PATCH 077/261] Implemented amp fix (yay) --- .../trigon/robot/commands/factories/AmpCommands.java | 11 +++++++---- .../frc/trigon/robot/constants/ShootingConstants.java | 3 ++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 1fcecc9c..9e7652c2 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -1,6 +1,7 @@ package frc.trigon.robot.commands.factories; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.CommandConstants; @@ -16,6 +17,8 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; public class AmpCommands { + private static boolean IS_FEEDING_NOTE = false; + public static Command getAutonomousScoreInAmpCommand() { return new ParallelCommandGroup( getAutonomousPrepareForAmpCommand(), @@ -25,7 +28,7 @@ public static Command getAutonomousScoreInAmpCommand() { } public static Command getScoreInAmpCommand() { - return new ParallelCommandGroup( + return new InstantCommand(() -> IS_FEEDING_NOTE = false).andThen(new ParallelCommandGroup( getPrepareForAmpCommand(), GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) @@ -44,7 +47,7 @@ private static Command getAutonomousPrepareForAmpCommand() { return new ParallelCommandGroup( GeneralCommands.runWhen( AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN) - .alongWith(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.AMP_PITCH)), + .alongWith(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.PREPARE_AMP_PITCH)), () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation().getDistance( FieldConstants.IN_FRONT_OF_AMP_POSE.get().getTranslation()) < FieldConstants.MINIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS ), @@ -55,13 +58,13 @@ private static Command getAutonomousPrepareForAmpCommand() { private static Command getPrepareForAmpCommand() { return new ParallelCommandGroup( AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN), - PitcherCommands.getSetTargetPitchCommand(ShootingConstants.AMP_PITCH), + GeneralCommands.getContinuousConditionalCommand(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.SHOOT_AMP_PITCH), PitcherCommands.getSetTargetPitchCommand(ShootingConstants.PREPARE_AMP_PITCH), () -> IS_FEEDING_NOTE), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND) ); } private static Command getFeedToAmpCommand() { - return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch() && RobotContainer.AMP_ALIGNER.atTargetState()); + return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()).alongWith(new InstantCommand(() -> IS_FEEDING_NOTE = true)), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch());// && RobotContainer.AMP_ALIGNER.atTargetState()); } private static Command getPathfindToAmpCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 1e9bb5d3..c422efd5 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -19,7 +19,8 @@ public class ShootingConstants { CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), - AMP_PITCH = Rotation2d.fromDegrees(49), + PREPARE_AMP_PITCH = Rotation2d.fromDegrees(55), + SHOOT_AMP_PITCH = Rotation2d.fromDegrees(45), MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(12), EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12), CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12); From 8ca44fe0b6106296435972ae85ce486206e79b33 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 8 Oct 2024 13:47:05 +0300 Subject: [PATCH 078/261] Set some intake stuff --- .../robot/subsystems/intake/IntakeConstants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 17d64abc..01910def 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -37,8 +37,8 @@ public class IntakeConstants { private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double GEAR_RATIO = 1.5; private static final double - DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, - DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; + DISTANCE_SENSOR_SCALING_SLOPE = 0.002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -2000; static final boolean FOC_ENABLED = true; private static final int MOTOR_AMOUNT = 2; @@ -49,7 +49,7 @@ public class IntakeConstants { GEAR_RATIO, MOMENT_OF_INERTIA ); - private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 5; + private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 27; private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER = () -> SimulationObjectDetectionCameraIO.HAS_OBJECTS ? NOTE_DISTANCE_THRESHOLD_CENTIMETERS - 1 : NOTE_DISTANCE_THRESHOLD_CENTIMETERS + 1; private static final double MAX_DISPLAYABLE_VELOCITY = 12; @@ -59,7 +59,7 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.01; + static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS @@ -122,7 +122,7 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(4), //TODO: calibrate + COLLECT(3), //TODO: calibrate EJECT(-2), //TODO: calibrate STOP(0), FEED_SHOOTING(8), //TODO: calibrate From fdfec5cf0f781b6eec99f3ddea0c36db88ad0f74 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 8 Oct 2024 14:06:15 +0300 Subject: [PATCH 079/261] fix --- .../java/frc/trigon/robot/subsystems/intake/Intake.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index fa73d828..d64be31d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -14,8 +14,6 @@ import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import static frc.trigon.robot.subsystems.intake.IntakeConstants.DISTANCE_SENSOR; - public class Intake extends MotorSubsystem { private final TalonFXMotor masterMotor = IntakeConstants.MASTER_MOTOR; private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(IntakeConstants.FOC_ENABLED); @@ -29,14 +27,14 @@ public Intake() { @Override public void updatePeriodically() { masterMotor.update(); - DISTANCE_SENSOR.updateSensor(); + IntakeConstants.DISTANCE_SENSOR.updateSensor(); } @Override public void updateMechanism() { IntakeConstants.MECHANISM.update(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); Logger.recordOutput("HasNote", hasNote()); - Logger.recordOutput("distanceSensorScaledValue", DISTANCE_SENSOR.getScaledValue()); + Logger.recordOutput("distanceSensorScaledValue", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); } @Override From dfcb892652f8491840147add5627ca4a23c74b3a Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 8 Oct 2024 17:59:51 +0300 Subject: [PATCH 080/261] added missing bracket --- .../java/frc/trigon/robot/commands/factories/AmpCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 9e7652c2..1ec2ae0f 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -32,7 +32,7 @@ public static Command getScoreInAmpCommand() { getPrepareForAmpCommand(), GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) - ); + )); } /** From f72c4e2d65b7e33966f16ba0604831f35525e681 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 8 Oct 2024 18:35:10 +0300 Subject: [PATCH 081/261] moved --- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index d64be31d..3e4d4be5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -28,13 +28,13 @@ public Intake() { public void updatePeriodically() { masterMotor.update(); IntakeConstants.DISTANCE_SENSOR.updateSensor(); + Logger.recordOutput("HasNote", hasNote()); + Logger.recordOutput("distanceSensorScaledValue", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); } @Override public void updateMechanism() { IntakeConstants.MECHANISM.update(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); - Logger.recordOutput("HasNote", hasNote()); - Logger.recordOutput("distanceSensorScaledValue", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); } @Override From 1d22941ce70edac58cda5ef26c62f8c71cb510b9 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 8 Oct 2024 20:47:53 +0300 Subject: [PATCH 082/261] Intake works (yay) --- .../trigon/robot/commands/factories/AmpCommands.java | 4 ++-- .../robot/commands/factories/GeneralCommands.java | 5 ++++- .../frc/trigon/robot/constants/ShootingConstants.java | 3 ++- .../trigon/robot/subsystems/intake/IntakeCommands.java | 2 +- .../robot/subsystems/intake/IntakeConstants.java | 10 +++++----- 5 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 1ec2ae0f..475b5982 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -30,8 +30,8 @@ public static Command getAutonomousScoreInAmpCommand() { public static Command getScoreInAmpCommand() { return new InstantCommand(() -> IS_FEEDING_NOTE = false).andThen(new ParallelCommandGroup( getPrepareForAmpCommand(), - GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), - GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) + GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()) +// GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) )); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 5acb142b..4c93b86c 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -7,6 +7,7 @@ import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.VisualizeNoteShootingCommand; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; @@ -16,6 +17,7 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.pitcher.PitcherConstants; +import frc.trigon.robot.subsystems.shooter.ShooterCommands; import org.littletonrobotics.junction.Logger; import java.util.function.BooleanSupplier; @@ -40,7 +42,8 @@ public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), - IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT) + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), + ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.SHOOTER_VELOCITY_FOR_INTAKE).until(RobotContainer.INTAKE::hasNote) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); } diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index c422efd5..9f0e601d 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -16,7 +16,8 @@ public class ShootingConstants { AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 45, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, - CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; + CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5, + SHOOTER_VELOCITY_FOR_INTAKE = -10; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), PREPARE_AMP_PITCH = Rotation2d.fromDegrees(55), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index dd445a3d..53cf2bfb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -59,6 +59,6 @@ private static Command getStopIntakeCommand() { private static Command getWaitForNoteToStopCommand() { - return new WaitCommand(IntakeConstants.NOTE_STOPPING_SECONDS); + return getSetTargetVoltageCommand(-3).withTimeout(0.1); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 01910def..ee54a7ae 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -37,8 +37,8 @@ public class IntakeConstants { private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; private static final double GEAR_RATIO = 1.5; private static final double - DISTANCE_SENSOR_SCALING_SLOPE = 0.002, - DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -2000; + DISTANCE_SENSOR_SCALING_SLOPE = 0.0002, + DISTANCE_SENSOR_SCALING_INTERCEPT_POINT = -200; static final boolean FOC_ENABLED = true; private static final int MOTOR_AMOUNT = 2; @@ -49,7 +49,7 @@ public class IntakeConstants { GEAR_RATIO, MOMENT_OF_INERTIA ); - private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 27; + private static final double NOTE_DISTANCE_THRESHOLD_CENTIMETERS = 14; private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER = () -> SimulationObjectDetectionCameraIO.HAS_OBJECTS ? NOTE_DISTANCE_THRESHOLD_CENTIMETERS - 1 : NOTE_DISTANCE_THRESHOLD_CENTIMETERS + 1; private static final double MAX_DISPLAYABLE_VELOCITY = 12; @@ -59,7 +59,7 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0; + static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.03; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS @@ -122,7 +122,7 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(3), //TODO: calibrate + COLLECT(4.8), //TODO: calibrate EJECT(-2), //TODO: calibrate STOP(0), FEED_SHOOTING(8), //TODO: calibrate From 3ad63edad7d058a1b5577efba1dcd82c1e303cb1 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Tue, 8 Oct 2024 23:37:43 +0300 Subject: [PATCH 083/261] stuff --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 ++ .../frc/trigon/robot/commands/CommandConstants.java | 4 ++-- .../frc/trigon/robot/constants/CameraConstants.java | 4 ++-- .../poseestimation/apriltagcamera/AprilTagCamera.java | 10 ++++++---- .../apriltagcamera/AprilTagCameraConstants.java | 2 +- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 4 ++-- vendordeps/photonlib.json | 10 +++++----- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0ebfe6b2..9edbf068 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.trigon.robot.subsystems.ampaligner.AmpAligner; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; +import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.pitcher.Pitcher; import frc.trigon.robot.subsystems.shooter.Shooter; @@ -79,6 +80,7 @@ private void bindControllerCommands() { OperatorConstants.ALIGN_TO_MIDDLE_STAGE.whileTrue(CommandConstants.ALIGN_TO_MIDDLE_STAGE_COMMAND); OperatorConstants.EJECT_NOTE_TRIGGER.whileTrue(CommandConstants.EJECT_COMMAND); + OperatorConstants.OPERATOR_CONTROLLER.l().whileTrue(IntakeCommands.getSetTargetVoltageCommand(2)); OperatorConstants.COLLECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getNoteCollectionCommand()); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 0cafd238..543f97c1 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -35,8 +35,8 @@ public class CommandConstants { public static final Command FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), - () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), + () -> -calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), + () -> -calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 5989d3a3..b6eb57e7 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -26,14 +26,14 @@ public class CameraConstants { public static final AprilTagCamera FRONT_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "frontTagCamera", + "FrontTagCamera", FRONT_CENTER_TO_CAMERA, THETA_STD_EXPONENT, TRANSLATIONS_STD_EXPONENT ), REAR_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "rearTagCamera", + "RearTagCamera", REAR_CENTER_TO_CAMERA, THETA_STD_EXPONENT, TRANSLATIONS_STD_EXPONENT diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 45c69b9c..a961e9ef 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -122,12 +122,14 @@ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { if (!isWithinBestTagRangeForSolvePNP()) return new Pose2d(fieldRelativeRobotTranslation, gyroHeading); - final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.toPose2d().getRotation().minus(robotCenterToCamera.getRotation().toRotation2d()); return new Pose2d(fieldRelativeRobotTranslation, solvePNPHeading); } private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { - final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); + final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]);//.plus(AprilTagCameraConstants.TAG_OFFSET); + if (bestTagPose == null) + return null; final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(gyroHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); @@ -194,8 +196,8 @@ private boolean isWithinBestTagRangeForSolvePNP() { private void logCameraInfo() { Logger.processInputs("Cameras/" + name, inputs); - if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) - logUsedTags(); +// if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) +// logUsedTags(); if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) { logEstimatedRobotPose(); logSolvePNPPose(); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index a9cddad5..7ecc846e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -9,7 +9,7 @@ import java.util.function.Function; public class AprilTagCameraConstants { - public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0.03, 0, new Rotation3d(0, 0, 0)); static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 8f9e59de..0b8657d9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -81,8 +81,8 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { * @return the estimated pose */ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { - if (result.getMultiTagResult().isPresent()) { - final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; + if (result.getMultiTagResult().estimatedPose.isPresent) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 44d5c47b..8f8e893e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "dev-v2024.3.0-84-gcd9dd072", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "dev-v2024.3.0-84-gcd9dd072", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "dev-v2024.3.0-84-gcd9dd072", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "dev-v2024.3.0-84-gcd9dd072" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "dev-v2024.3.0-84-gcd9dd072" + "version": "v2024.3.1" } ] } \ No newline at end of file From 7be0131b327da7335c2b7d559f01e2ef782254b7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 9 Oct 2024 00:07:20 +0300 Subject: [PATCH 084/261] cleaned up code --- .../robot/subsystems/intake/IntakeCommands.java | 6 +++--- .../robot/subsystems/intake/IntakeConstants.java | 11 ++++++----- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 53cf2bfb..0df2162c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -46,7 +46,7 @@ private static Command getCollectionCommand() { RobotContainer.INTAKE::hasNote, RobotContainer.INTAKE ), - getWaitForNoteToStopCommand().andThen(getStopIntakeCommand()) + getCorrectNotePositionCommand().andThen(getStopIntakeCommand()) ); } @@ -58,7 +58,7 @@ private static Command getStopIntakeCommand() { } - private static Command getWaitForNoteToStopCommand() { - return getSetTargetVoltageCommand(-3).withTimeout(0.1); + private static Command getCorrectNotePositionCommand() { + return getSetTargetStateCommand(IntakeConstants.IntakeState.CORRECT_NOTE_POSITION).withTimeout(IntakeConstants.CORRECT_NOTE_POSITION_TIMEOUT_SECONDS); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index ee54a7ae..1f096dda 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -64,7 +64,7 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); - static final double NOTE_STOPPING_SECONDS = 1; + static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 1; private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.25; //TODO: calibrate static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( @@ -122,11 +122,12 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(4.8), //TODO: calibrate - EJECT(-2), //TODO: calibrate + COLLECT(4.8), + EJECT(-2), STOP(0), - FEED_SHOOTING(8), //TODO: calibrate - FEED_AMP(4); //TODO: calibrate + FEED_SHOOTING(8), + FEED_AMP(4), + CORRECT_NOTE_POSITION(-3); public final double voltage; From 8e0d6841af4fe285245703b6cfe6cabe18b9bb0f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 9 Oct 2024 00:55:35 +0300 Subject: [PATCH 085/261] fix --- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 1f096dda..d87aef71 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -64,7 +64,7 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); - static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 1; + static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 0.1; private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.25; //TODO: calibrate static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( From 64c09cbe078b8917889129880e78d051364c0e07 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 9 Oct 2024 16:04:03 +0300 Subject: [PATCH 086/261] Fixed camera error --- src/main/java/frc/trigon/robot/RobotContainer.java | 5 +++-- .../robot/poseestimation/apriltagcamera/AprilTagCamera.java | 2 +- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 2 +- .../robot/poseestimation/poseestimator/PoseEstimator.java | 2 +- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 9edbf068..0efc4f8c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -13,6 +13,7 @@ import frc.trigon.robot.commands.factories.AutonomousCommands; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.commands.factories.ShootingCommands; +import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -29,8 +30,8 @@ public class RobotContainer { public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( -// CameraConstants.FRONT_TAG_CAMERA, -// CameraConstants.REAR_TAG_CAMERA + CameraConstants.FRONT_TAG_CAMERA, + CameraConstants.REAR_TAG_CAMERA ); public static final Swerve SWERVE = new Swerve(); public static final Intake INTAKE = new Intake(); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index a961e9ef..72958179 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -127,7 +127,7 @@ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { } private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { - final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]);//.plus(AprilTagCameraConstants.TAG_OFFSET); + final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); if (bestTagPose == null) return null; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 0b8657d9..3d7f082f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -95,7 +95,7 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { private int[] getVisibleTagIDs(PhotonPipelineResult result) { final int[] visibleTagIDs = new int[result.getTargets().size()]; - for (int i = 1; i < visibleTagIDs.length; i++) + for (int i = 0; i < visibleTagIDs.length; i++) visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); return visibleTagIDs; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index b768efb7..739c5f48 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -114,7 +114,7 @@ private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera if (!aprilTagCamera.hasNewResult()) return null; final Pose2d robotPose = aprilTagCamera.getEstimatedRobotPose(); - if (robotPose == null) + if (robotPose == null || robotPose.getTranslation() == null || robotPose.getRotation() == null) return null; return new PoseEstimator6328.VisionObservation( From a1ad05b8ceb2e7ad6e6b35645c688f83544e28db Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Wed, 9 Oct 2024 22:39:23 +0300 Subject: [PATCH 087/261] cameras and collection --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0efc4f8c..9ac14470 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -30,7 +30,7 @@ public class RobotContainer { public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( - CameraConstants.FRONT_TAG_CAMERA, +// CameraConstants.FRONT_TAG_CAMERA CameraConstants.REAR_TAG_CAMERA ); public static final Swerve SWERVE = new Swerve(); diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index b6eb57e7..e7eb9e0c 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -20,7 +20,7 @@ public class CameraConstants { ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(0, 0.325 - 0.00975, 0.95), - new Rotation3d(180, Units.degreesToRadians(0), Units.degreesToRadians(180)) + new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(25.8), 0) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index d87aef71..f89fe827 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -59,7 +59,7 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.03; + static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.2; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS @@ -122,7 +122,7 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(4.8), + COLLECT(5.5), EJECT(-2), STOP(0), FEED_SHOOTING(8), From 9b82d06836369026e74140d687a0d92c97032f74 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 10 Oct 2024 15:46:39 +0300 Subject: [PATCH 088/261] quick commit --- .../robot/commands/CommandConstants.java | 4 +-- .../robot/constants/CameraConstants.java | 36 +++++++++---------- .../robot/constants/OperatorConstants.java | 6 ++-- .../robot/constants/ShootingConstants.java | 2 +- .../apriltagcamera/AprilTagCamera.java | 21 ++++++----- .../io/AprilTagPhotonCameraIO.java | 6 ++-- .../subsystems/swerve/SwerveCommands.java | 2 +- .../subsystems/swerve/SwerveConstants.java | 6 ++-- 8 files changed, 42 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 543f97c1..0cafd238 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -35,8 +35,8 @@ public class CommandConstants { public static final Command FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> -calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), - () -> -calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index e7eb9e0c..187c949e 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -14,29 +14,29 @@ public class CameraConstants { THETA_STD_EXPONENT = 0.01; private static final Transform3d - FRONT_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0.0465, 0.325, 0.192), - new Rotation3d(0, Units.degreesToRadians(0), 0) - ), +// FRONT_CENTER_TO_CAMERA = new Transform3d( +// new Translation3d(0.0465, 0.325, 0.192), +// new Rotation3d(0, Units.degreesToRadians(0), 0) +// ), REAR_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0, 0.325 - 0.00975, 0.95), - new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(25.8), 0) - ); + new Translation3d(0.325 - 0.00975, 0, 0.095), + new Rotation3d(Math.PI, Units.degreesToRadians(-25.8), Math.PI) + ); public static final AprilTagCamera - FRONT_TAG_CAMERA = new AprilTagCamera( +// FRONT_TAG_CAMERA = new AprilTagCamera( +// AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, +// "FrontTagCamera", +// FRONT_CENTER_TO_CAMERA, +// THETA_STD_EXPONENT, +// TRANSLATIONS_STD_EXPONENT +// ), + REAR_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "FrontTagCamera", - FRONT_CENTER_TO_CAMERA, + "RearTagCamera", + REAR_CENTER_TO_CAMERA, THETA_STD_EXPONENT, TRANSLATIONS_STD_EXPONENT - ), - REAR_TAG_CAMERA = new AprilTagCamera( - AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "RearTagCamera", - REAR_CENTER_TO_CAMERA, - THETA_STD_EXPONENT, - TRANSLATIONS_STD_EXPONENT - ); + ); public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index aa2f51d8..1f5f473a 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -47,8 +47,8 @@ public class OperatorConstants { WARM_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.w().and(SPEAKER_SHOT_TRIGGER.negate()), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m(), - AMP_TRIGGER = OPERATOR_CONTROLLER.a(), + AMP_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.x()), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), - ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), - RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); + // ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), + RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 9f0e601d..ae0bf551 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -14,7 +14,7 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, - MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 45, + MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5, SHOOTER_VELOCITY_FOR_INTAKE = -10; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 72958179..c1231149 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -54,6 +54,8 @@ public void update() { aprilTagCameraIO.updateInputs(inputs); robotPose = calculateBestRobotPose(); + if (inputs.cameraSolvePNPPose != null) + Logger.recordOutput("SolvePNPPose", inputs.cameraSolvePNPPose); logCameraInfo(); } @@ -117,23 +119,20 @@ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult) return null; - final Translation2d fieldRelativeRobotTranslation = getFieldRelativeRobotTranslation(gyroHeading); - if (!isWithinBestTagRangeForSolvePNP()) - return new Pose2d(fieldRelativeRobotTranslation, gyroHeading); - - final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.toPose2d().getRotation().minus(robotCenterToCamera.getRotation().toRotation2d()); - return new Pose2d(fieldRelativeRobotTranslation, solvePNPHeading); + return new Pose2d(getFieldRelativeRobotTranslation(gyroHeading), gyroHeading); + final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + return new Pose2d(getFieldRelativeRobotTranslation(solvePNPHeading), solvePNPHeading); } - private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { + private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading) { final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); if (bestTagPose == null) return null; - final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(gyroHeading, bestTagPose); + final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); - final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(gyroHeading); + final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(currentHeading); return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); } @@ -160,8 +159,8 @@ private double projectToPlane(double targetAngleRadians, double cameraAngleRadia } private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { - double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); - double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); + final double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + final double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 3d7f082f..b23e5b0f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -10,6 +10,7 @@ import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.littletonrobotics.junction.Logger; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; @@ -39,8 +40,8 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Photon inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); - inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); + inputs.bestTargetRelativePitchRadians = -bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYawRadians = -bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); } @@ -88,6 +89,7 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()); final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); + Logger.recordOutput("TagPose", tagPose); final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 95ecaf8e..13c10c54 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -31,7 +31,7 @@ public class SwerveCommands { public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier) { return new InitExecuteCommand( () -> SWERVE.initializeDrive(true), - () -> SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), + () -> SWERVE.fieldRelativeDrive(-xSupplier.getAsDouble(), -ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), SWERVE ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index d905bc54..5b406895 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -22,9 +22,9 @@ public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_YAW = 101.195068, - GYRO_MOUNT_POSITION_PITCH = 0.175781, - GYRO_MOUNT_POSITION_ROLL = 0.043945; + GYRO_MOUNT_POSITION_YAW = 268.093872, + GYRO_MOUNT_POSITION_PITCH = 0.263672, + GYRO_MOUNT_POSITION_ROLL = 0.219727; private static final double FRONT_LEFT_STEER_ENCODER_OFFSET = -1.561, FRONT_RIGHT_STEER_ENCODER_OFFSET = -3.431, From 7641fbfa28ff5a634c442585ec1eecf7636f0308 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 11 Oct 2024 02:38:30 +0300 Subject: [PATCH 089/261] added SourceSide3NoteAutoRight --- .pathplanner/settings.json | 1 + .../autos/SourceSide3NoteAutoRight.auto | 111 ++++++++++++++++++ .../paths/SourceSide3NoteAutoRight2.path | 76 ++++++++++++ .../paths/SourceSide3NoteAutoRight3.path | 70 +++++++++++ .../paths/SourceSide3NoteAutoRight4.path | 76 ++++++++++++ .../paths/SourceSide3NoteAutoRight5.path | 70 +++++++++++ 6 files changed, 404 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto create mode 100644 src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path create mode 100644 src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path create mode 100644 src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path create mode 100644 src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 7d46ee87..ac17f925 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -8,6 +8,7 @@ "7NoteAuto", "FrontRow4NoteAuto", "MiddleRush3NoteAuto", + "SourceSide3NoteAutoRight", "SourceSide3NoteAuto", "SpeakerSide3NoteAuto", "Zayde3NoteAuto" diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto new file mode 100644 index 00000000..9dac3120 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -0,0 +1,111 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7547034812379487, + "y": 3.84449444955228 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight2" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight4" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path new file mode 100644 index 00000000..4715f55f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8981397630253705, + "y": 2.778681091803989 + }, + "prevControl": null, + "nextControl": { + "x": 4.049984437997045, + "y": 2.2980248777398966 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoFirstFeed" + }, + { + "anchor": { + "x": 8.284709527165534, + "y": 0.7804612792324187 + }, + "prevControl": { + "x": 7.374171368061045, + "y": 1.0117090656716528 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightFirstCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -21.447736327105403, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": -45.210000000000036, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path new file mode 100644 index 00000000..2d27fb09 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.284709527165534, + "y": 0.7804612792324187 + }, + "prevControl": null, + "nextControl": { + "x": 4.830445717229463, + "y": 2.0523241046482106 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightFirstCollection" + }, + { + "anchor": { + "x": 3.1683522521974625, + "y": 2.71716149066101 + }, + "prevControl": { + "x": 3.6019418517710275, + "y": 2.5003666908742277 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondFeed" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StopAligning", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAligningToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -43.74000000000001, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": -21.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path new file mode 100644 index 00000000..5d3c1194 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1683522521974625, + "y": 2.71716149066101 + }, + "prevControl": null, + "nextControl": { + "x": 5.0038815570588895, + "y": 1.560922558464836 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondFeed" + }, + { + "anchor": { + "x": 8.270256540513081, + "y": 2.442554744264419 + }, + "prevControl": { + "x": 6.2179324358648715, + "y": 1.5753755451172882 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 22.1663458220825, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": -44.00999999999999, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path new file mode 100644 index 00000000..576d6a77 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.270256540513081, + "y": 2.442554744264419 + }, + "prevControl": null, + "nextControl": { + "x": 6.333556329084489, + "y": 1.474204638550123 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondCollection" + }, + { + "anchor": { + "x": 3.038275372325393, + "y": 2.746067463965915 + }, + "prevControl": { + "x": 4.758180783967202, + "y": 1.5753755451172882 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StopAligning", + "waypointRelativePos": 0.1, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAligningToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -42.51999999999998, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": 37.87498365109831, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 0957fa7b01f61b56e77a2579b8b4f4da4e8b76d7 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 11 Oct 2024 03:01:45 +0300 Subject: [PATCH 090/261] added 3NoteAutoThroughAmpLeft and cleaned up autos --- .pathplanner/settings.json | 11 +- .../autos/3NoteAutoThroughAmp.auto | 2 +- .../autos/3NoteAutoThroughAmpLeft.auto | 129 ++++++++++++++++++ .../autos/4NoteAutoAroundStage.auto | 2 +- .../autos/5NoteAutoAroundStage.auto | 2 +- .../deploy/pathplanner/autos/7NoteAuto.auto | 2 +- .../pathplanner/autos/FrontRow4NoteAuto.auto | 2 +- .../autos/MiddleRush3NoteAuto.auto | 2 +- .../autos/SourceSide3NoteAuto.auto | 2 +- .../autos/SourceSide3NoteAutoRight.auto | 2 +- .../autos/SpeakerSide3NoteAuto.auto | 2 +- .../pathplanner/autos/Zayde3NoteAuto.auto | 2 +- .../paths/3NoteAutoThroughAmpLeft3.path | 76 +++++++++++ .../paths/3NoteAutoThroughAmpLeft4.path | 70 ++++++++++ .../paths/3NoteAutoThroughAmpLeft5.path | 76 +++++++++++ 15 files changed, 370 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto create mode 100644 src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path create mode 100644 src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path create mode 100644 src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index ac17f925..0f3fdaf6 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -8,12 +8,19 @@ "7NoteAuto", "FrontRow4NoteAuto", "MiddleRush3NoteAuto", - "SourceSide3NoteAutoRight", + "3NoteAutoThroughAmpLeft", "SourceSide3NoteAuto", + "SourceSide3NoteAutoRight", "SpeakerSide3NoteAuto", "Zayde3NoteAuto" ], - "autoFolders": [], + "autoFolders": [ + "3NoteAutoThroughAmpVarients", + "BackNotesAroundStageVarients", + "Misc", + "Backup", + "SourceSide3NoteAutoVarients" + ], "defaultMaxVel": 4.0, "defaultMaxAccel": 4.0, "defaultMaxAngVel": 400.0, diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index a1e8eeb5..defff574 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -124,6 +124,6 @@ ] } }, - "folder": null, + "folder": "3NoteAutoThroughAmpVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto new file mode 100644 index 00000000..548d6fdd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7531929553170285, + "y": 6.932078092184688 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft3" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft5" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "3NoteAutoThroughAmpVarients", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto index d75d1da0..fa7f4642 100644 --- a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto @@ -118,6 +118,6 @@ ] } }, - "folder": null, + "folder": "BackNotesAroundStageVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto index 485a8327..bf7b8acf 100644 --- a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto @@ -148,6 +148,6 @@ ] } }, - "folder": null, + "folder": "BackNotesAroundStageVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/7NoteAuto.auto b/src/main/deploy/pathplanner/autos/7NoteAuto.auto index 9011e633..b5af6441 100644 --- a/src/main/deploy/pathplanner/autos/7NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/7NoteAuto.auto @@ -184,6 +184,6 @@ ] } }, - "folder": null, + "folder": "Misc", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index faac7163..703d0b30 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -124,6 +124,6 @@ ] } }, - "folder": null, + "folder": "Backup", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index 096bb93b..d0d75406 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -194,6 +194,6 @@ ] } }, - "folder": null, + "folder": "Misc", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index 73a447bc..68fa5477 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -106,6 +106,6 @@ ] } }, - "folder": null, + "folder": "SourceSide3NoteAutoVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index 9dac3120..e8237783 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -106,6 +106,6 @@ ] } }, - "folder": null, + "folder": "SourceSide3NoteAutoVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto index 286efbda..33fa0796 100644 --- a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto @@ -150,6 +150,6 @@ ] } }, - "folder": null, + "folder": "Misc", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto index d36b7d2c..c556a8b5 100644 --- a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto @@ -150,6 +150,6 @@ ] } }, - "folder": null, + "folder": "Misc", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path new file mode 100644 index 00000000..6510cd24 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.879292519148419, + "y": 6.995245539786855 + }, + "prevControl": null, + "nextControl": { + "x": 4.772633770619654, + "y": 7.125322419658923 + }, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpFirstCollection" + }, + { + "anchor": { + "x": 8.255803553860629, + "y": 7.443288126012872 + }, + "prevControl": { + "x": 7.576513181195377, + "y": 7.298758259488349 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpLeftSecondCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 5.194428907734835, + "rotateFast": false + }, + "reversed": false, + "folder": "3NoteAutoThroughAmpLeft", + "previewStartingState": { + "rotation": 28.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path new file mode 100644 index 00000000..476e99c4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.255803553860629, + "y": 7.443288126012872 + }, + "prevControl": null, + "nextControl": { + "x": 7.489795261280664, + "y": 7.197587352921185 + }, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpLeftSecondCollection" + }, + { + "anchor": { + "x": 3.7171194532412826, + "y": 6.269094533438474 + }, + "prevControl": { + "x": 4.61365091744268, + "y": 6.576108926865741 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpThirdFeed" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StopAligning", + "waypointRelativePos": 0.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAligningToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 10.719999999999999, + "rotateFast": false + }, + "reversed": false, + "folder": "3NoteAutoThroughAmpLeft", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path new file mode 100644 index 00000000..742f1c95 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.7171194532412826, + "y": 6.269094533438474 + }, + "prevControl": null, + "nextControl": { + "x": 4.931616623796628, + "y": 6.590561913518194 + }, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpThirdFeed" + }, + { + "anchor": { + "x": 8.284709527165534, + "y": 5.76674167432842 + }, + "prevControl": { + "x": 7.229641501536524, + "y": 6.373767113731411 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3NoteAutoThroughAmpLeftThirdCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -20.136303428248176, + "rotateFast": false + }, + "reversed": false, + "folder": "3NoteAutoThroughAmpLeft", + "previewStartingState": { + "rotation": 10.759999999999991, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 89ab08d26ccb70166e623422723f5760fba41eb0 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 11 Oct 2024 03:20:57 +0300 Subject: [PATCH 091/261] changed folder name --- .pathplanner/settings.json | 4 ++-- src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto | 2 +- src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 0f3fdaf6..37091a07 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -16,9 +16,9 @@ ], "autoFolders": [ "3NoteAutoThroughAmpVarients", - "BackNotesAroundStageVarients", - "Misc", + "FarNotesAroundStageVarients", "Backup", + "Misc", "SourceSide3NoteAutoVarients" ], "defaultMaxVel": 4.0, diff --git a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto index fa7f4642..24d24fca 100644 --- a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto @@ -118,6 +118,6 @@ ] } }, - "folder": "BackNotesAroundStageVarients", + "folder": "FarNotesAroundStageVarients", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto index bf7b8acf..352bef25 100644 --- a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto @@ -148,6 +148,6 @@ ] } }, - "folder": "BackNotesAroundStageVarients", + "folder": "FarNotesAroundStageVarients", "choreoAuto": false } \ No newline at end of file From 4b840a33888c2f5fc89d4785a55576fd6658a1e6 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Fri, 11 Oct 2024 03:56:36 +0300 Subject: [PATCH 092/261] things --- .../commands/factories/GeneralCommands.java | 3 +- .../robot/constants/2024-crescendo.json | 296 ++++++++++++++++++ .../robot/constants/CameraConstants.java | 4 +- .../robot/constants/FieldConstants.java | 14 +- .../robot/constants/ShootingConstants.java | 3 +- .../apriltagcamera/AprilTagCamera.java | 6 +- .../AprilTagCameraConstants.java | 4 +- .../subsystems/intake/IntakeConstants.java | 6 +- .../subsystems/pitcher/PitcherConstants.java | 2 +- .../robot/subsystems/shooter/Shooter.java | 7 + .../subsystems/shooter/ShooterCommands.java | 10 + .../subsystems/shooter/ShooterConstants.java | 4 +- .../subsystems/swerve/SwerveCommands.java | 2 +- 13 files changed, 341 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/constants/2024-crescendo.json diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 4c93b86c..961aa26d 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -7,7 +7,6 @@ import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.VisualizeNoteShootingCommand; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; @@ -43,7 +42,7 @@ public static Command getNoteCollectionCommand() { new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.SHOOTER_VELOCITY_FOR_INTAKE).until(RobotContainer.INTAKE::hasNote) + ShooterCommands.getSetTargetVelocityCommand(-10).until(RobotContainer.INTAKE::hasNote) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); } diff --git a/src/main/java/frc/trigon/robot/constants/2024-crescendo.json b/src/main/java/frc/trigon/robot/constants/2024-crescendo.json new file mode 100644 index 00000000..122b7fba --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/2024-crescendo.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.98, + "z": 1.508 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.508 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 187c949e..27cd2e9c 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -19,8 +19,8 @@ public class CameraConstants { // new Rotation3d(0, Units.degreesToRadians(0), 0) // ), REAR_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0.325 - 0.00975, 0, 0.095), - new Rotation3d(Math.PI, Units.degreesToRadians(-25.8), Math.PI) + new Translation3d(-0.325 + 0.00975, 0, 0.095), + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33.2), Math.PI + Units.degreesToRadians(1.6)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 4a171cb1..cbbe4032 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -2,17 +2,27 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; +import java.io.IOException; import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT;//AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + + static { + try { + APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); + } catch (IOException e) { + System.out.println("test"); + throw new RuntimeException(e); + } + } + public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double FIELD_LENGTH_METERS = 16.54175, diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index ae0bf551..374568be 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -16,8 +16,7 @@ public class ShootingConstants { AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, - CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5, - SHOOTER_VELOCITY_FOR_INTAKE = -10; + CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), PREPARE_AMP_PITCH = Rotation2d.fromDegrees(55), diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c1231149..5db07225 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -190,13 +190,13 @@ private double calculateStandardDeviations(double exponent, double distance, int } private boolean isWithinBestTagRangeForSolvePNP() { - return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; + return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS; } private void logCameraInfo() { Logger.processInputs("Cameras/" + name, inputs); -// if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) -// logUsedTags(); + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) { logEstimatedRobotPose(); logSolvePNPPose(); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 7ecc846e..9cedd24f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -9,8 +9,8 @@ import java.util.function.Function; public class AprilTagCameraConstants { - public static final Transform3d TAG_OFFSET = new Transform3d(0, 0.03, 0, new Rotation3d(0, 0, 0)); - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 2; + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index f89fe827..b7b69893 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -59,14 +59,14 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0.2; + static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 0.1; private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate - private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.25; //TODO: calibrate + private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.2; //TODO: calibrate static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> Math.abs(MASTER_MOTOR.getSignal(TalonFXSignal.TORQUE_CURRENT)) > IntakeConstants.NOTE_COLLECTION_CURRENT @@ -122,7 +122,7 @@ private static void configureDistanceSensor() { } public enum IntakeState { - COLLECT(5.5), + COLLECT(4.5), EJECT(-2), STOP(0), FEED_SHOOTING(8), diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index d35a7c89..52157ad5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -45,7 +45,7 @@ public class PitcherConstants { private static final double P = RobotHardwareStats.isSimulation() ? 100 : 0.5, I = RobotHardwareStats.isSimulation() ? 0 : 0, - D = RobotHardwareStats.isSimulation() ? 20 : 0.6, + D = RobotHardwareStats.isSimulation() ? 20 : 0.5, KS = RobotHardwareStats.isSimulation() ? 0.2 : 0.3, KV = RobotHardwareStats.isSimulation() ? 32 : 23, KA = RobotHardwareStats.isSimulation() ? 0 : 0, diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java index 822f3359..f48d6dc5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc.trigon.robot.subsystems.shooter; +import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import edu.wpi.first.units.Measure; @@ -19,6 +20,7 @@ public class Shooter extends MotorSubsystem { leftMotor = ShooterConstants.LEFT_MOTOR; private final VelocityTorqueCurrentFOC velocityRequest = new VelocityTorqueCurrentFOC(0); private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + private final StaticBrake staticBrakeRequest = new StaticBrake(); private double targetRightVelocityRotationsPerSecond = 0, targetLeftVelocityRotationsPerSecond = 0; @@ -102,6 +104,11 @@ void setTargetVelocity(double targetRightVelocityRotationsPerSecond, double targ setTargetLeftVelocity(targetLeftVelocityRotationsPerSecond); } + void sendStaticBrakeRequest() { + rightMotor.setControl(staticBrakeRequest); + leftMotor.setControl(staticBrakeRequest); + } + private boolean atVelocity(double currentVelocity, double targetVelocity) { return Math.abs(currentVelocity - targetVelocity) < ShooterConstants.VELOCITY_TOLERANCE; } diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java index 291ce691..da39ac74 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java @@ -40,6 +40,16 @@ public static Command getSetTargetVelocityCommand(double targetRightVelocityRota ); } + public static Command getSendStaticBreakRequestCommand() { + return new StartEndCommand( + RobotContainer.SHOOTER::sendStaticBrakeRequest, + () -> { + }, + RobotContainer.SHOOTER + ); + + } + public static Command getStopCommand() { return new StartEndCommand( RobotContainer.SHOOTER::stop, diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 9cdc5ecd..f79e3f3e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -66,8 +66,8 @@ public class ShooterConstants { LEFT_MECHANISM = new SpeedMechanism2d("LeftShooterMechanism", MAX_DISPLAYABLE_VELOCITY); public static final double WHEEL_DIAMETER_METERS = edu.wpi.first.math.util.Units.inchesToMeters(4); - public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1; - static final double VELOCITY_TOLERANCE = 3; + public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1.2; + static final double VELOCITY_TOLERANCE = 1; static { configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_SIMULATION, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_KS, RIGHT_KV, RIGHT_KA); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 13c10c54..155b515c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -48,7 +48,7 @@ public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSup public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier angleSupplier) { return new InitExecuteCommand( () -> SWERVE.initializeDrive(true), - () -> SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), angleSupplier.get()), + () -> SWERVE.fieldRelativeDrive(-xSupplier.getAsDouble(), -ySupplier.getAsDouble(), angleSupplier.get()), SWERVE ); } From 623d946b9b2037ba54c81f172f48b0ee5c478f35 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Fri, 11 Oct 2024 05:13:27 +0300 Subject: [PATCH 093/261] corrected LEDStripConstants --- .../trigon/robot/subsystems/ledstrip/LEDStripConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index 58094fb6..899de5d7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -8,10 +8,10 @@ import frc.trigon.robot.Robot; public class LEDStripConstants { - private static final int PORT = 0; + private static final int PORT = 9; private static final int - RIGHT_CLIMBER_NUMBER_OF_LEDS = 24, - LEFT_CLIMBER_NUMBER_OF_LEDS = 24; + RIGHT_CLIMBER_NUMBER_OF_LEDS = 22, + LEFT_CLIMBER_NUMBER_OF_LEDS = 23; private static final boolean RIGHT_CLIMBER_INVERTED = false, LEFT_CLIMBER_INVERTED = false; From 1b70a197c990740c0ee8f3b76692143abdaad9dd Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Sat, 12 Oct 2024 21:14:52 +0300 Subject: [PATCH 094/261] fixes --- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index b7b69893..643a9329 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -64,7 +64,7 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); - static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 0.1; + static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 0.07; private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.2; //TODO: calibrate static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index 1dea385a..58d7693a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -19,6 +19,7 @@ import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; +import org.trigon.utilities.Conversions; public class Pitcher extends MotorSubsystem { private final ShootingCalculations shootingCalculations = ShootingCalculations.getInstance(); @@ -57,6 +58,7 @@ public void stop() { public void updatePeriodically() { masterMotor.update(); encoder.update(); + Logger.recordOutput("PitcherAngleDegrees", Conversions.rotationsToDegrees(encoder.getSignal(CANcoderSignal.POSITION))); } @Override From d1986b793cec8f83b0eaf76ab2614bbfe5fcf85a Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Sun, 13 Oct 2024 01:23:39 +0300 Subject: [PATCH 095/261] Fixed stuff --- .../robot/commands/factories/GeneralCommands.java | 3 ++- .../frc/trigon/robot/constants/CameraConstants.java | 2 +- .../frc/trigon/robot/constants/ShootingConstants.java | 11 ++++++----- .../robot/subsystems/intake/IntakeConstants.java | 2 +- .../trigon/robot/subsystems/ledstrip/LEDStrip.java | 4 ++-- .../robot/subsystems/ledstrip/LEDStripConstants.java | 4 ++-- 6 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 961aa26d..e3be92e7 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -7,6 +7,7 @@ import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.VisualizeNoteShootingCommand; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; @@ -42,7 +43,7 @@ public static Command getNoteCollectionCommand() { new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - ShooterCommands.getSetTargetVelocityCommand(-10).until(RobotContainer.INTAKE::hasNote) + ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); } diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 27cd2e9c..8ead60fc 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -20,7 +20,7 @@ public class CameraConstants { // ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), - new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33.2), Math.PI + Units.degreesToRadians(1.6)) + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-32.5), Math.PI + Units.degreesToRadians(0)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 374568be..665a3bbf 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -14,16 +14,17 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, - MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, + MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 45, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, - CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; + CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5, + FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), PREPARE_AMP_PITCH = Rotation2d.fromDegrees(55), SHOOT_AMP_PITCH = Rotation2d.fromDegrees(45), - MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(12), - EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12), - CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(12); + MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(13), + EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13), + CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13); public static final Pose3d ROBOT_RELATIVE_PITCHER_PIVOT_POINT = new Pose3d(0.2521, 0, 0.15545, new Rotation3d(0, 0, Math.PI)); public static final Transform3d PITCHER_PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.4209, 0, -0.0165, new Rotation3d()); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 643a9329..1fc54ff8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -126,7 +126,7 @@ public enum IntakeState { EJECT(-2), STOP(0), FEED_SHOOTING(8), - FEED_AMP(4), + FEED_AMP(6), CORRECT_NOTE_POSITION(-3); public final double voltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index e5930d16..33a23208 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -29,7 +29,7 @@ public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { this.inverted = inverted; this.numberOfLEDs = numberOfLEDs; - addLEDStripToLEDStripsArrayArray(this); + addLEDStripToLEDStripsArray(this); } public static void setDefaultCommandForAllLEDS(Command command) { @@ -91,7 +91,7 @@ private void setLEDColor(Color color, int index) { LED.setData(LEDStripConstants.LED_BUFFER); } - private void addLEDStripToLEDStripsArrayArray(LEDStrip ledStrip) { + private void addLEDStripToLEDStripsArray(LEDStrip ledStrip) { final LEDStrip[] newLEDStrips = new LEDStrip[LED_STRIPS.length + 1]; System.arraycopy(LED_STRIPS, 0, newLEDStrips, 0, LED_STRIPS.length); newLEDStrips[LED_STRIPS.length] = ledStrip; diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index 899de5d7..e8fe49bc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -24,8 +24,8 @@ public class LEDStripConstants { static final double LOW_BATTERY_BLINKING_TIME_SECONDS = 5; public static final LEDStrip - RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, 0), - LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, RIGHT_CLIMBER_NUMBER_OF_LEDS - 1); + RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, LEFT_CLIMBER_NUMBER_OF_LEDS - 1), + LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, 0); static { LED.setLength(RIGHT_CLIMBER_NUMBER_OF_LEDS + LEFT_CLIMBER_NUMBER_OF_LEDS); From 761632412a34ff5f395e3eee348481274f8788d1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 13 Oct 2024 04:50:51 +0300 Subject: [PATCH 096/261] Updated camera stuff --- .../robot/constants/CameraConstants.java | 38 +++++++++---------- .../io/AprilTagPhotonCameraIO.java | 4 +- vendordeps/photonlib.json | 10 ++--- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 8ead60fc..79253cd8 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -14,29 +14,29 @@ public class CameraConstants { THETA_STD_EXPONENT = 0.01; private static final Transform3d -// FRONT_CENTER_TO_CAMERA = new Transform3d( -// new Translation3d(0.0465, 0.325, 0.192), -// new Rotation3d(0, Units.degreesToRadians(0), 0) -// ), + FRONT_CENTER_TO_CAMERA = new Transform3d( + new Translation3d(0.0465, 0.325, 0.192), + new Rotation3d(0, Units.degreesToRadians(0), 0) + ), REAR_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(-0.325 + 0.00975, 0, 0.095), - new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-32.5), Math.PI + Units.degreesToRadians(0)) - ); + new Translation3d(-0.325 + 0.00975, 0, 0.095), + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-32.5), Math.PI + Units.degreesToRadians(0)) + ); public static final AprilTagCamera -// FRONT_TAG_CAMERA = new AprilTagCamera( -// AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, -// "FrontTagCamera", -// FRONT_CENTER_TO_CAMERA, -// THETA_STD_EXPONENT, -// TRANSLATIONS_STD_EXPONENT -// ), - REAR_TAG_CAMERA = new AprilTagCamera( + FRONT_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "RearTagCamera", - REAR_CENTER_TO_CAMERA, + "FrontTagCamera", + FRONT_CENTER_TO_CAMERA, THETA_STD_EXPONENT, TRANSLATIONS_STD_EXPONENT - ); - public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); + ), + REAR_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "RearTagCamera", + REAR_CENTER_TO_CAMERA, + THETA_STD_EXPONENT, + TRANSLATIONS_STD_EXPONENT + ); + public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index b23e5b0f..c4ffcdde 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -82,8 +82,8 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { * @return the estimated pose */ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - final Transform3d cameraPoseTransform = result.getMultiTagResult().estimatedPose.best; + if (result.getMultiTagResult().isPresent()) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8f8e893e..8f3d842e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-89-gd9b6199c", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-89-gd9b6199c", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-89-gd9b6199c", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.3.1" + "version": "dev-v2024.3.0-89-gd9b6199c" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.3.1" + "version": "dev-v2024.3.0-89-gd9b6199c" } ] } \ No newline at end of file From 830be322e6cface7e1db857da92579d0ad206ad8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 07:10:25 +0300 Subject: [PATCH 097/261] Camera stuff and leds idek --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- .../java/frc/trigon/robot/commands/AlignToNoteCommand.java | 7 ++++++- .../java/frc/trigon/robot/constants/CameraConstants.java | 2 +- .../robot/subsystems/ledstrip/LEDStripConstants.java | 2 +- .../frc/trigon/robot/subsystems/swerve/SwerveCommands.java | 4 ++-- 5 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 9ac14470..0efc4f8c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -30,7 +30,7 @@ public class RobotContainer { public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( -// CameraConstants.FRONT_TAG_CAMERA + CameraConstants.FRONT_TAG_CAMERA, CameraConstants.REAR_TAG_CAMERA ); public static final Swerve SWERVE = new Swerve(); diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index b99eccd5..e730a578 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -15,6 +15,7 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.trigon.hardware.RobotHardwareStats; +import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.MirrorableRotation2d; public class AlignToNoteCommand extends ParallelCommandGroup { @@ -40,8 +41,12 @@ private Command getSetCurrentLEDColorCommand() { } private Command getDriveWhileAligningToNoteCommand() { +// XboxController controller = OperatorConstants.DRIVER_CONTROLLER; +// final double stickX = controller.getLeftX(); +// final double stickY = controller.getLeftY(); +// final double stickValue = Math.tan(stickY / stickX); return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),//stickValue), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 79253cd8..64d324e8 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -16,7 +16,7 @@ public class CameraConstants { private static final Transform3d FRONT_CENTER_TO_CAMERA = new Transform3d( new Translation3d(0.0465, 0.325, 0.192), - new Rotation3d(0, Units.degreesToRadians(0), 0) + new Rotation3d(0, Units.degreesToRadians(-31.5), 0) ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index e8fe49bc..faff71a2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -24,7 +24,7 @@ public class LEDStripConstants { static final double LOW_BATTERY_BLINKING_TIME_SECONDS = 5; public static final LEDStrip - RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, LEFT_CLIMBER_NUMBER_OF_LEDS - 1), + RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS - 1, LEFT_CLIMBER_NUMBER_OF_LEDS - 1), LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, 0); static { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 155b515c..95ecaf8e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -31,7 +31,7 @@ public class SwerveCommands { public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier) { return new InitExecuteCommand( () -> SWERVE.initializeDrive(true), - () -> SWERVE.fieldRelativeDrive(-xSupplier.getAsDouble(), -ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), + () -> SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), thetaSupplier.getAsDouble()), SWERVE ); } @@ -48,7 +48,7 @@ public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSup public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier angleSupplier) { return new InitExecuteCommand( () -> SWERVE.initializeDrive(true), - () -> SWERVE.fieldRelativeDrive(-xSupplier.getAsDouble(), -ySupplier.getAsDouble(), angleSupplier.get()), + () -> SWERVE.fieldRelativeDrive(xSupplier.getAsDouble(), ySupplier.getAsDouble(), angleSupplier.get()), SWERVE ); } From 9916217fb1ec6e8fab0d45db3e1cda81f75b5519 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 12:40:54 +0300 Subject: [PATCH 098/261] SysID'd climber --- .../subsystems/climber/ClimberConstants.java | 55 +++++++++++-------- .../subsystems/shooter/ShooterConstants.java | 14 ++--- 2 files changed, 38 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index dcffff8c..005c783d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -32,22 +32,29 @@ public class ClimberConstants { private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; static final boolean ENABLE_FOC = true; private static final double //TODO: calibrate - GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 0, - GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, - GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, - GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0, - GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 0, - GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0; + LEFT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 0.39596, + LEFT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, + LEFT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.078964, + LEFT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9056, + LEFT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.18439; private static final double //TODO: calibrate - ON_CHAIN_P = RobotHardwareStats.isSimulation() ? GROUNDED_P : 0, - ON_CHAIN_I = RobotHardwareStats.isSimulation() ? GROUNDED_I : 0, - ON_CHAIN_D = RobotHardwareStats.isSimulation() ? GROUNDED_D : 0, - ON_CHAIN_KS = RobotHardwareStats.isSimulation() ? GROUNDED_KS : 0, - ON_CHAIN_KV = RobotHardwareStats.isSimulation() ? GROUNDED_KV : 0, - ON_CHAIN_KA = RobotHardwareStats.isSimulation() ? GROUNDED_KA : 0; + RIGHT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 4.5626, + RIGHT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, + RIGHT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.079947, + RIGHT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9986, + RIGHT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.21705; + private static final double //TODO: calibrate + ON_CHAIN_P = RobotHardwareStats.isSimulation() ? RIGHT_GROUNDED_P : 0, + ON_CHAIN_I = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_I : 0, + ON_CHAIN_D = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_D : 0, + ON_CHAIN_KS = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KS : 0, + ON_CHAIN_KV = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KV : 0, + ON_CHAIN_KA = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KA : 0; static final double - MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / GROUNDED_KV : 0, - MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / GROUNDED_KA : 0, + MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KV : 0, + MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KA : 0, MAX_ON_CHAIN_VELOCITY = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KV) - 0.75 : 0, MAX_ON_CHAIN_ACCELERATION = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KA) - 50 : 0; static final int @@ -71,7 +78,7 @@ public class ClimberConstants { static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Second.of(1)), - Units.Volts.of(8), + Units.Volts.of(5), null, null ); @@ -119,11 +126,11 @@ public class ClimberConstants { static final double LIMIT_SWITCH_PRESSED_POSITION = 0; static { - configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_MOTOR_SIMULATION); - configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_MOTOR_SIMULATION); + configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_MOTOR_SIMULATION, RIGHT_GROUNDED_P, RIGHT_GROUNDED_I, RIGHT_GROUNDED_D, RIGHT_GROUNDED_KS, RIGHT_GROUNDED_KV, RIGHT_GROUNDED_KA); + configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_MOTOR_SIMULATION, LEFT_GROUNDED_P, LEFT_GROUNDED_I, LEFT_GROUNDED_D, LEFT_GROUNDED_KS, LEFT_GROUNDED_KV, LEFT_GROUNDED_KA); } - private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, SimpleMotorSimulation simulation) { + private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, SimpleMotorSimulation simulation, double groundedP, double groundedI, double groundedD, double groundedKS, double groundedKV, double groundedKA) { final TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = invertedValue; @@ -131,12 +138,12 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; - config.Slot0.kP = GROUNDED_P; - config.Slot0.kI = GROUNDED_I; - config.Slot0.kD = GROUNDED_D; - config.Slot0.kS = GROUNDED_KS; - config.Slot0.kV = GROUNDED_KV; - config.Slot0.kA = GROUNDED_KA; + config.Slot0.kP = groundedP; + config.Slot0.kI = groundedI; + config.Slot0.kD = groundedD; + config.Slot0.kS = groundedKS; + config.Slot0.kV = groundedKV; + config.Slot0.kA = groundedKA; config.Slot1.kP = ON_CHAIN_P; config.Slot1.kI = ON_CHAIN_I; diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index f79e3f3e..39169da5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -74,7 +74,7 @@ public class ShooterConstants { configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_SIMULATION, LEFT_P, LEFT_I, LEFT_D, LEFT_KS, LEFT_KV, LEFT_KA); } - private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, FlywheelSimulation simulation, double P, double I, double D, double KS, double KV, double KA) { + private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, FlywheelSimulation simulation, double p, double i, double d, double kS, double kV, double kA) { final TalonFXConfiguration config = new TalonFXConfiguration(); config.Audio.BeepOnBoot = false; @@ -83,12 +83,12 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal config.MotorOutput.Inverted = invertedValue; config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; - config.Slot0.kP = RIGHT_P; - config.Slot0.kI = RIGHT_I; - config.Slot0.kD = RIGHT_D; - config.Slot0.kS = RIGHT_KS; - config.Slot0.kV = RIGHT_KV; - config.Slot0.kA = RIGHT_KA; + config.Slot0.kP = p; + config.Slot0.kI = i; + config.Slot0.kD = d; + config.Slot0.kS = kS; + config.Slot0.kV = kV; + config.Slot0.kA = kA; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; From 907fda69251062dbf1bc8098750e89b25003a585 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 16:12:45 +0300 Subject: [PATCH 099/261] commit --- .../java/frc/trigon/robot/commands/AlignToNoteCommand.java | 6 +----- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 4 ++++ 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index e730a578..78730bd0 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -22,7 +22,7 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.NOTE_DETECTION_CAMERA; private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : - new PIDController(0, 0, 0); + new PIDController(0.01, 0, 0); public AlignToNoteCommand() { addCommands( @@ -41,10 +41,6 @@ private Command getSetCurrentLEDColorCommand() { } private Command getDriveWhileAligningToNoteCommand() { -// XboxController controller = OperatorConstants.DRIVER_CONTROLLER; -// final double stickX = controller.getLeftX(); -// final double stickY = controller.getLeftY(); -// final double stickValue = Math.tan(stickY / stickX); return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),//stickValue), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 1fc54ff8..b5078dac 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -92,6 +92,8 @@ private static void configureMasterMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; + config.HardwareLimitSwitch.ReverseLimitEnable = false; + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; MASTER_MOTOR.applyConfiguration(config); @@ -110,6 +112,8 @@ private static void configureFollowerMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; + config.HardwareLimitSwitch.ReverseLimitEnable = false; + FOLLOWER_MOTOR.applyConfiguration(config); final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); From b6cb3a2f081538d47d1917072c790e82c733cfe3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 13 Oct 2024 16:12:59 +0300 Subject: [PATCH 100/261] commit --- .../trigon/robot/subsystems/climber/Climber.java | 15 +++++++++------ .../subsystems/climber/ClimberConstants.java | 2 +- .../trigon/robot/subsystems/intake/Intake.java | 8 ++++++++ .../robot/subsystems/intake/IntakeConstants.java | 3 +++ 4 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index f30fedd5..8b56efd7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; @@ -133,12 +134,14 @@ void setTargetPosition(double targetRightPositionRotations, double targetLeftPos } private void configurePositionResettingTrigger(TalonFXMotor motor) { - final Trigger positionResettingTrigger = new Trigger(() -> hasHitReverseLimit(motor)).debounce(ClimberConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); - positionResettingTrigger.onFalse(new InstantCommand(() -> motor.setPosition(ClimberConstants.LIMIT_SWITCH_PRESSED_POSITION))); - } - - private boolean hasHitReverseLimit(TalonFXMotor motor) { - return motor.getSignal(TalonFXSignal.REVERSE_LIMIT) == 0; + final Trigger positionResettingTrigger; + if (motor.getID() == ClimberConstants.RIGHT_MOTOR_ID) + positionResettingTrigger = new Trigger(RobotContainer.INTAKE::hasHitRightClimberReverseLimit).debounce(ClimberConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); + else if (motor.getID() == ClimberConstants.LEFT_MOTOR_ID) + positionResettingTrigger = new Trigger(RobotContainer.INTAKE::hasHitLeftClimberReverseLimit).debounce(ClimberConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); + else + return; + positionResettingTrigger.onTrue(new InstantCommand(() -> motor.setPosition(ClimberConstants.LIMIT_SWITCH_PRESSED_POSITION))); } private DynamicMotionMagicVoltage determineRequest(boolean affectedByRobotWeight) { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 005c783d..d78d4672 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -16,7 +16,7 @@ import org.trigon.hardware.simulation.SimpleMotorSimulation; public class ClimberConstants { - private static final int + public static final int RIGHT_MOTOR_ID = 14, LEFT_MOTOR_ID = 15; private static final String diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 3e4d4be5..ead67a65 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -63,6 +63,14 @@ public boolean isEarlyNoteCollectionDetected() { return IntakeConstants.EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + public boolean hasHitRightClimberReverseLimit() { + return masterMotor.getSignal(TalonFXSignal.REVERSE_LIMIT) == 0; + } + + public boolean hasHitLeftClimberReverseLimit() { + return IntakeConstants.FOLLOWER_MOTOR.getSignal(TalonFXSignal.REVERSE_LIMIT) == 0; + } + void sendStaticBrakeRequest() { masterMotor.setControl(staticBrakeRequest); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 1fc54ff8..82d5415d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -100,6 +100,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); } private static void configureFollowerMotor() { @@ -114,6 +115,8 @@ private static void configureFollowerMotor() { final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); FOLLOWER_MOTOR.setControl(followerRequest); + + FOLLOWER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); } private static void configureDistanceSensor() { From 83109bf07f6a5e9eb4ae7605a804856e98a31f0c Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 16:20:18 +0300 Subject: [PATCH 101/261] Uncommented amp stuff --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0efc4f8c..6557feee 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -74,7 +74,7 @@ private void bindControllerCommands() { OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); -// OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); + OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); OperatorConstants.ALIGN_TO_SPEAKER_TRIGGER.whileTrue(CommandConstants.FACE_SPEAKER_COMMAND); OperatorConstants.ALIGN_TO_RIGHT_STAGE.whileTrue(CommandConstants.ALIGN_TO_RIGHT_STAGE_COMMAND); OperatorConstants.ALIGN_TO_LEFT_STAGE.whileTrue(CommandConstants.ALIGN_TO_LEFT_STAGE_COMMAND); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 1f5f473a..c250f9c0 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -47,8 +47,8 @@ public class OperatorConstants { WARM_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.w().and(SPEAKER_SHOT_TRIGGER.negate()), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m(), - AMP_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.x()), + AMP_TRIGGER = OPERATOR_CONTROLLER.a(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), - // ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), + ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file From ad05aafaf091ee375c1640da78a2e17fc31c506c Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 16:25:11 +0300 Subject: [PATCH 102/261] Added amp without aligning --- src/main/java/frc/trigon/robot/RobotContainer.java | 3 ++- .../frc/trigon/robot/commands/factories/AmpCommands.java | 6 +++--- .../trigon/robot/commands/factories/ShootingCommands.java | 2 +- .../java/frc/trigon/robot/constants/OperatorConstants.java | 1 + 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 6557feee..bccee9dd 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -97,7 +97,8 @@ private void bindControllerCommands() { OperatorConstants.DELIVERY_TRIGGER.whileTrue(CommandConstants.DELIVERY_COMMAND); OperatorConstants.MANUAL_LOW_DELIVERY_TRIGGER.whileTrue(ShootingCommands.getManualLowDeliveryCommand()); - OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand()); + OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(true)); + OperatorConstants.AMP_WITHOUT_ALIGN_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(false)); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 475b5982..42164dc2 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -27,11 +27,11 @@ public static Command getAutonomousScoreInAmpCommand() { ); } - public static Command getScoreInAmpCommand() { + public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { return new InstantCommand(() -> IS_FEEDING_NOTE = false).andThen(new ParallelCommandGroup( getPrepareForAmpCommand(), - GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()) -// GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) + GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), + GeneralCommands.runWhen(GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND), () -> shouldAlignToAmp) )); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java index 8bf7c3a4..31411379 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java @@ -43,7 +43,7 @@ public static Command getWarmSpeakerShotCommand() { public static Command getCloseSpeakerShotCommand() { return new ParallelCommandGroup( getPrepareCloseSpeakerShotCommand(), - getFeedNoteWhenPitcherAndShooterReadyCommand() + GeneralCommands.runWhenContinueTriggerPressed(getFeedNoteWhenPitcherAndShooterReadyCommand()) ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index c250f9c0..c3f49f82 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -48,6 +48,7 @@ public class OperatorConstants { DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m(), AMP_TRIGGER = OPERATOR_CONTROLLER.a(), + AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); From 57758ff92ed5cf7eda5971132d557f3e4ea55801 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 17:15:25 +0300 Subject: [PATCH 103/261] first auto --- .../autos/SourceSide3NoteAuto.auto | 172 +++++++++--------- .../robot/constants/OperatorConstants.java | 2 +- 2 files changed, 89 insertions(+), 85 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index 68fa5477..c1228187 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -12,89 +12,93 @@ "data": { "commands": [ { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto2" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto3" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto4" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto5" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto1" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto2" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto4" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 } }, { @@ -106,6 +110,6 @@ ] } }, - "folder": "SourceSide3NoteAutoVarients", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index c3f49f82..01f295a7 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -51,5 +51,5 @@ public class OperatorConstants { AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), - RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); + RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file From 1e670ac1fd574bedd3d62f7b454157163807f493 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 17:18:12 +0300 Subject: [PATCH 104/261] cvty --- .../autos/3NoteAutoThroughAmp.auto | 20 ++++++++++++++++++- .../autos/SourceSide3NoteAuto.auto | 10 +++++----- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index defff574..6b85ef0c 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -120,10 +120,28 @@ "data": { "name": "StopShooting" } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0 + } + }, + { + "type": "named", + "data": { + "name": null + } } ] } }, - "folder": "3NoteAutoThroughAmpVarients", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index c1228187..5bb636e2 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -12,21 +12,21 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "SourceSide3NoteAuto1" + "name": "PrepareForShooting" } }, { - "type": "named", + "type": "path", "data": { - "name": "PrepareForShooting" + "pathName": "SourceSide3NoteAuto1" } }, { "type": "wait", "data": { - "waitTime": 0.5 + "waitTime": 0.1 } }, { From 28a6f4068eeaed0a0bc9de3f29d22f2673dc58d9 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 17:24:35 +0300 Subject: [PATCH 105/261] Update 3NoteAutoThroughAmp.auto --- .../autos/3NoteAutoThroughAmp.auto | 202 +++++++++--------- 1 file changed, 97 insertions(+), 105 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index 6b85ef0c..5de53398 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -12,131 +12,123 @@ "data": { "commands": [ { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp2" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp3" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp4" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp5" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp6" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 } }, { "type": "named", "data": { - "name": "StopShooting" + "name": "FeedNote" } }, { "type": "path", "data": { - "pathName": "3NoteAutoThroughAmp1" + "pathName": "3NoteAutoThroughAmp2" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" } }, { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp3" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp5" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" } } ] From 0c134e261add62463422a3b42bc22ae7a66b525d Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 17:45:48 +0300 Subject: [PATCH 106/261] right --- .../autos/SourceSide3NoteAutoRight.auto | 150 ++++++++---------- .../paths/SourceSide3NoteAuto1.path | 20 ++- .../paths/SourceSide3NoteAutoRight3.path | 17 ++ .../paths/SourceSide3NoteAutoRight5.path | 17 ++ 4 files changed, 121 insertions(+), 83 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index e8237783..ae5000e4 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -12,89 +12,75 @@ "data": { "commands": [ { - "type": "race", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight2" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight3" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight4" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight5" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] + "pathName": "SourceSide3NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight2" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight4" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 } }, { diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 2c8e2fe2..f40a2867 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Prepare Shooting", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 2d27fb09..15fc8208 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -47,6 +47,23 @@ ] } } + }, + { + "name": "Prepare Shooting", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 576d6a77..1831354a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -47,6 +47,23 @@ ] } } + }, + { + "name": "Prepare Shooting", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + } } ], "globalConstraints": { From 3a2d6a50a384671a60863fac16b15b93a78d6201 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 13 Oct 2024 19:34:25 +0300 Subject: [PATCH 107/261] FIXED front notes auto --- .../pathplanner/autos/FrontRow4NoteAuto.auto | 146 ++++++++++++++---- .../factories/AutonomousCommands.java | 16 ++ .../robot/constants/AutonomousConstants.java | 3 +- 3 files changed, 131 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index 703d0b30..6af03abe 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForCloseShot" + "name": "PreparePitch" } }, { @@ -26,15 +26,35 @@ "data": { "commands": [ { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -50,15 +70,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -74,15 +114,35 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -98,15 +158,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 18e7b78c..251f260d 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.ShootingConstants; @@ -90,6 +91,21 @@ private static Command getPrepareForCloseShooterEjectionCommand() { ); } + public static Command getPreparePitchCommand(Rotation2d pitch) { + return new ParallelCommandGroup( + ShootingCommands.getUpdateShootingCalculationsCommand(false), + PitcherCommands.getSetTargetPitchCommand(pitch) + ); + } + + public static Command getPrepareShootingCommand(double velocity) { + return new StartEndCommand( + () -> ShooterCommands.getSetTargetVelocityCommand(velocity), + () -> { + } + ); + } + private static Optional calculateRotationOverride() { NOTE_DETECTION_CAMERA.trackObject(); if (RobotContainer.INTAKE.hasNote() || !NOTE_DETECTION_CAMERA.hasTargets()) diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index f409be51..fd551aa2 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -17,7 +17,8 @@ private static void registerCommands() { NamedCommands.registerCommand("Collect", AutonomousCommands.getNoteCollectionCommand()); NamedCommands.registerCommand("AlignToNote", AutonomousCommands.getAlignToNoteCommand()); NamedCommands.registerCommand("StopAligningToNote", AutonomousCommands.getStopAligningToNoteCommand()); - NamedCommands.registerCommand("PrepareForShooting", ShootingCommands.getWarmSpeakerShotCommand()); + NamedCommands.registerCommand("PrepareForShooting", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PreparePitch", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_SHOT_PITCH)); NamedCommands.registerCommand("StopShooting", ShooterCommands.getStopCommand()); NamedCommands.registerCommand("FeedNote", AutonomousCommands.getFeedNoteCommand()); NamedCommands.registerCommand("PrepareForCloseShot", ShootingCommands.getPrepareCloseSpeakerShotCommand()); From 42913655394f3ae01f6a16c04e1e65ecc4c60e5b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 13 Oct 2024 20:12:32 +0300 Subject: [PATCH 108/261] fixed other autos though not completly --- .../autos/3NoteAutoThroughAmp.auto | 2 +- .../autos/3NoteAutoThroughAmpLeft.auto | 146 +++++++++--- .../autos/4NoteAutoAroundStage.auto | 2 +- .../autos/5NoteAutoAroundStage.auto | 2 +- .../deploy/pathplanner/autos/7NoteAuto.auto | 2 +- .../pathplanner/autos/FrontRow4NoteAuto.auto | 10 +- .../autos/MiddleRush3NoteAuto.auto | 2 +- .../autos/SourceSide3NoteAuto.auto | 2 +- .../autos/SourceSide3NoteAutoRight.auto | 210 ++++++++++++------ .../autos/SpeakerSide3NoteAuto.auto | 2 +- .../pathplanner/autos/Zayde3NoteAuto.auto | 2 +- .../paths/SourceSide3NoteAuto1.path | 20 +- .../paths/SourceSide3NoteAutoRight3.path | 17 -- .../paths/SourceSide3NoteAutoRight5.path | 17 -- .../factories/AutonomousCommands.java | 18 +- .../robot/constants/AutonomousConstants.java | 6 +- 16 files changed, 285 insertions(+), 175 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index defff574..3a81957b 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index 548d6fdd..8ed98a23 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "preparePitch" } }, { @@ -32,15 +32,35 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -50,15 +70,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -74,15 +114,35 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -98,15 +158,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto index 24d24fca..220abbca 100644 --- a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto index 352bef25..387c046f 100644 --- a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/7NoteAuto.auto b/src/main/deploy/pathplanner/autos/7NoteAuto.auto index b5af6441..7bf3b73e 100644 --- a/src/main/deploy/pathplanner/autos/7NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/7NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index 6af03abe..6a71d0a8 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PreparePitch" + "name": "PreparePitchForCloseShot" } }, { @@ -32,7 +32,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { @@ -76,7 +76,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { @@ -120,7 +120,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { @@ -164,7 +164,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index d0d75406..df52d1d6 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -100,7 +100,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index 68fa5477..cd3741d2 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index ae5000e4..6d291f68 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -12,75 +12,149 @@ "data": { "commands": [ { - "type": "path", + "type": "race", "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight2" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight3" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight4" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight5" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 + "commands": [ + { + "type": "named", + "data": { + "name": "preparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto1" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight2" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight3" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight4" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight5" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] } }, { diff --git a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto index 33fa0796..997685d9 100644 --- a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto @@ -56,7 +56,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto index c556a8b5..ae6ad1d7 100644 --- a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto @@ -56,7 +56,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "PrepareForShootingCloseShot" } }, { diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index f40a2867..2c8e2fe2 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Prepare Shooting", - "waypointRelativePos": 0.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 15fc8208..2d27fb09 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -47,23 +47,6 @@ ] } } - }, - { - "name": "Prepare Shooting", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - } - ] - } - } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 1831354a..576d6a77 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -47,23 +47,6 @@ ] } } - }, - { - "name": "Prepare Shooting", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - } - ] - } - } } ], "globalConstraints": { diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 251f260d..27ac2fb4 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.ShootingConstants; @@ -91,6 +90,13 @@ private static Command getPrepareForCloseShooterEjectionCommand() { ); } + public static Command getPreparePitchCommand() { + return new ParallelCommandGroup( + ShootingCommands.getUpdateShootingCalculationsCommand(false), + PitcherCommands.getReachTargetPitchFromShootingCalculationsCommand() + ); + } + public static Command getPreparePitchCommand(Rotation2d pitch) { return new ParallelCommandGroup( ShootingCommands.getUpdateShootingCalculationsCommand(false), @@ -98,12 +104,12 @@ public static Command getPreparePitchCommand(Rotation2d pitch) { ); } + public static Command getPrepareShootingCommand() { + return ShooterCommands.getReachTargetShootingVelocityFromShootingCalculationsCommand(); + } + public static Command getPrepareShootingCommand(double velocity) { - return new StartEndCommand( - () -> ShooterCommands.getSetTargetVelocityCommand(velocity), - () -> { - } - ); + return ShooterCommands.getSetTargetVelocityCommand(velocity); } private static Optional calculateRotationOverride() { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index fd551aa2..6ea2e183 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -17,8 +17,10 @@ private static void registerCommands() { NamedCommands.registerCommand("Collect", AutonomousCommands.getNoteCollectionCommand()); NamedCommands.registerCommand("AlignToNote", AutonomousCommands.getAlignToNoteCommand()); NamedCommands.registerCommand("StopAligningToNote", AutonomousCommands.getStopAligningToNoteCommand()); - NamedCommands.registerCommand("PrepareForShooting", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND)); - NamedCommands.registerCommand("PreparePitch", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_SHOT_PITCH)); + NamedCommands.registerCommand("PrepareForShooting", AutonomousCommands.getPrepareShootingCommand()); + NamedCommands.registerCommand("PreparePitch", AutonomousCommands.getPreparePitchCommand()); + NamedCommands.registerCommand("PrepareForShootingCloseShot", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PreparePitchForCloseShot", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_SHOT_PITCH)); NamedCommands.registerCommand("StopShooting", ShooterCommands.getStopCommand()); NamedCommands.registerCommand("FeedNote", AutonomousCommands.getFeedNoteCommand()); NamedCommands.registerCommand("PrepareForCloseShot", ShootingCommands.getPrepareCloseSpeakerShotCommand()); From 52e40b9374912bb5ea516a775d472b1a6b208387 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 13 Oct 2024 20:35:31 +0300 Subject: [PATCH 109/261] fixed soruce auto --- .../autos/3NoteAutoThroughAmpLeft.auto | 2 +- .../autos/SourceSide3NoteAutoRight.auto | 2 +- src/main/deploy/pathplanner/autos/Test.auto | 38 +++++++++++++++++++ .../paths/SourceSide3NoteAutoRight5.path | 2 +- .../robot/constants/AutonomousConstants.java | 2 + 5 files changed, 43 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Test.auto diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index 8ed98a23..6c29b06f 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "preparePitch" + "name": "PreparePitch" } }, { diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index 6d291f68..d19c6a51 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "preparePitch" + "name": "PreparePitch" } }, { diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto new file mode 100644 index 00000000..04902b4f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test.auto @@ -0,0 +1,38 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7547034812379487, + "y": 3.84449444955228 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto1" + } + } + ] + } + } + ] + } + }, + "folder": "SourceSide3NoteAutoVarients", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 576d6a77..71d45383 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "StopAligning", - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.05, "command": { "type": "parallel", "data": { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 6ea2e183..d099b39f 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -2,6 +2,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.wpilibj2.command.RunCommand; import frc.trigon.robot.commands.factories.AutonomousCommands; import frc.trigon.robot.commands.factories.ShootingCommands; import frc.trigon.robot.subsystems.shooter.ShooterCommands; @@ -14,6 +15,7 @@ public class AutonomousConstants { } private static void registerCommands() { + NamedCommands.registerCommand("Print", new RunCommand(() -> System.out.println("thbda"))); NamedCommands.registerCommand("Collect", AutonomousCommands.getNoteCollectionCommand()); NamedCommands.registerCommand("AlignToNote", AutonomousCommands.getAlignToNoteCommand()); NamedCommands.registerCommand("StopAligningToNote", AutonomousCommands.getStopAligningToNoteCommand()); From 7d8afa166fa100d301f269835258b81296b7379f Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 20:40:50 +0300 Subject: [PATCH 110/261] a bunch of random stuff idk --- .../java/frc/trigon/robot/RobotContainer.java | 3 +-- .../robot/commands/factories/AmpCommands.java | 2 +- .../robot/constants/FieldConstants.java | 20 ++++++++++--------- .../robot/constants/ShootingConstants.java | 2 +- .../subsystems/ampaligner/AmpAligner.java | 4 +++- 5 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index bccee9dd..c2e3d100 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -30,7 +30,7 @@ public class RobotContainer { public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( - CameraConstants.FRONT_TAG_CAMERA, +// CameraConstants.FRONT_TAG_CAMERA, CameraConstants.REAR_TAG_CAMERA ); public static final Swerve SWERVE = new Swerve(); @@ -102,7 +102,6 @@ private void bindControllerCommands() { OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); - // configureSysIdBindings(SWERVE); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 42164dc2..2d0f2acd 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -31,7 +31,7 @@ public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { return new InstantCommand(() -> IS_FEEDING_NOTE = false).andThen(new ParallelCommandGroup( getPrepareForAmpCommand(), GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), - GeneralCommands.runWhen(GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND), () -> shouldAlignToAmp) + shouldAlignToAmp ? GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) : null )); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index cbbe4032..98b99cbc 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -2,6 +2,7 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -12,16 +13,17 @@ import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT;//AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); +// public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT;//AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - static { - try { - APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); - } catch (IOException e) { - System.out.println("test"); - throw new RuntimeException(e); - } - } +// static { +// try { +// APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); +// } catch (IOException e) { +// System.out.println("test"); +// throw new RuntimeException(e); +// } +// } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 665a3bbf..c3f09110 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -14,7 +14,7 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, - MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 45, + MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java index 8ed7a9c3..12765a21 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import org.trigon.hardware.RobotHardwareStats; @@ -113,7 +114,8 @@ private void configurePositionResettingTrigger() { } private boolean hasHitForwardLimit() { - return motor.getSignal(TalonFXSignal.FORWARD_LIMIT) == 0; + return OperatorConstants.OPERATOR_CONTROLLER.y().getAsBoolean(); +// return motor.getSignal(TalonFXSignal.FORWARD_LIMIT) == 0; } private double calculateKGOutput() { From 4463ee2cb8c76b87a37833df8bef0cc594baa67c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 13 Oct 2024 20:40:50 +0300 Subject: [PATCH 111/261] fixed 3NoteAutoThroughAmp --- .../autos/3NoteAutoThroughAmp.auto | 146 ++++++++++++++---- 1 file changed, 113 insertions(+), 33 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index 3a81957b..bae751b4 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -32,15 +32,35 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -50,15 +70,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -74,15 +114,35 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -98,15 +158,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] From ddb22c3d8b1a05db52c9080ecc6ab71275520500 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 21:06:43 +0300 Subject: [PATCH 112/261] vfi --- .../java/frc/trigon/robot/commands/AlignToNoteCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 78730bd0..c752cea6 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -27,7 +27,7 @@ public class AlignToNoteCommand extends ParallelCommandGroup { public AlignToNoteCommand() { addCommands( getSetCurrentLEDColorCommand().asProxy(), - GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.SELF_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), + GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), new RunCommand(CAMERA::trackObject) ); } @@ -41,7 +41,7 @@ private Command getSetCurrentLEDColorCommand() { } private Command getDriveWhileAligningToNoteCommand() { - return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + return SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),//stickValue), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle From a911e6dc6e390b8abbfd0959b63449e6be5006e7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 21:16:20 +0300 Subject: [PATCH 113/261] fctufv --- .../autos/SourceSide3NoteAuto.auto | 112 ++++++++++++++---- src/main/deploy/pathplanner/autos/Test.auto | 38 ------ .../paths/SourceSide3NoteAuto1.path | 2 +- 3 files changed, 87 insertions(+), 65 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Test.auto diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index cd3741d2..33053350 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -5,7 +5,7 @@ "x": 0.7547034812379487, "y": 3.84449444955228 }, - "rotation": 0 + "rotation": -38.65999999999997 }, "command": { "type": "sequential", @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -32,15 +32,35 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -56,15 +76,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -80,15 +120,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto deleted file mode 100644 index 04902b4f..00000000 --- a/src/main/deploy/pathplanner/autos/Test.auto +++ /dev/null @@ -1,38 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7547034812379487, - "y": 3.84449444955228 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShootingCloseShot" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - } - ] - } - } - ] - } - }, - "folder": "SourceSide3NoteAutoVarients", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 2c8e2fe2..46434bec 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -45,7 +45,7 @@ "reversed": false, "folder": "SourceSide3NoteAuto", "previewStartingState": { - "rotation": -38.659808254090066, + "rotation": -38.65999999999997, "velocity": 0 }, "useDefaultConstraints": true From 676ab4ac0ee35cda88a81c84f4352430659c3959 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 21:22:10 +0300 Subject: [PATCH 114/261] around --- .../autos/4NoteAutoAroundStage.auto | 146 ++++++++++--- .../autos/5NoteAutoAroundStage.auto | 204 ++++++++++++++---- 2 files changed, 281 insertions(+), 69 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto index 220abbca..6e8a7f5e 100644 --- a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -26,15 +26,35 @@ "data": { "commands": [ { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -50,15 +70,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -74,15 +114,35 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -98,15 +158,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto index 387c046f..3d35c3b0 100644 --- a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto @@ -26,15 +26,67 @@ "data": { "commands": [ { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.5 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -50,15 +102,35 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -74,15 +146,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -98,15 +190,35 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -122,15 +234,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] From 54433d7a9ad8880a30f13f2250f4ea692f74a96c Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 21:25:06 +0300 Subject: [PATCH 115/261] 7 --- .../deploy/pathplanner/autos/7NoteAuto.auto | 252 ++++++++++++++---- 1 file changed, 196 insertions(+), 56 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/7NoteAuto.auto b/src/main/deploy/pathplanner/autos/7NoteAuto.auto index 7bf3b73e..0faf9c97 100644 --- a/src/main/deploy/pathplanner/autos/7NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/7NoteAuto.auto @@ -26,15 +26,35 @@ "data": { "commands": [ { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -50,15 +70,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -68,15 +108,35 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -86,15 +146,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -110,15 +190,35 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -134,15 +234,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -158,15 +278,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] From df4f1287d43e888762d913f9daed035cef72d4a6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 21:29:25 +0300 Subject: [PATCH 116/261] h --- .../autos/4NoteAutoAroundStage.auto | 8 ++-- .../autos/5NoteAutoAroundStage.auto | 44 +++---------------- .../deploy/pathplanner/autos/7NoteAuto.auto | 16 +++---- 3 files changed, 18 insertions(+), 50 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto index 6e8a7f5e..6bd7b7bc 100644 --- a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto @@ -26,7 +26,7 @@ "data": { "commands": [ { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -70,7 +70,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -114,7 +114,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -158,7 +158,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { diff --git a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto index 3d35c3b0..6ffa7e45 100644 --- a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -26,39 +26,7 @@ "data": { "commands": [ { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] - } - }, - { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -102,7 +70,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -146,7 +114,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -190,7 +158,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -234,7 +202,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { diff --git a/src/main/deploy/pathplanner/autos/7NoteAuto.auto b/src/main/deploy/pathplanner/autos/7NoteAuto.auto index 0faf9c97..e5a56407 100644 --- a/src/main/deploy/pathplanner/autos/7NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/7NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -26,7 +26,7 @@ "data": { "commands": [ { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -70,7 +70,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -108,7 +108,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -146,7 +146,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -190,7 +190,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -234,7 +234,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -278,7 +278,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { From 6c0a78cd89e1e8748c78aedca769635ac6463921 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 21:43:56 +0300 Subject: [PATCH 117/261] mrush --- .../autos/MiddleRush3NoteAuto.auto | 186 ++++++++++++++---- .../robot/constants/AutonomousConstants.java | 8 +- 2 files changed, 147 insertions(+), 47 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index df52d1d6..bd291681 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForCloseEjectFromShooter" + "name": "PreparePitchForCloseEjectFromShooter" } }, { @@ -32,15 +32,35 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.05 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShootingForCloseEjectFromShooter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -56,7 +76,7 @@ { "type": "named", "data": { - "name": "PrepareForEjectFromShooter" + "name": "PreparePitchForEjectFromShooter" } }, { @@ -76,15 +96,35 @@ } }, { - "type": "named", - "data": { - "name": "FeedNote" - } - }, - { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.05 + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShootingForEjectFromShooter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -100,7 +140,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -120,15 +160,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -144,15 +204,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -168,15 +248,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index d099b39f..6f61b4ec 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -15,7 +15,6 @@ public class AutonomousConstants { } private static void registerCommands() { - NamedCommands.registerCommand("Print", new RunCommand(() -> System.out.println("thbda"))); NamedCommands.registerCommand("Collect", AutonomousCommands.getNoteCollectionCommand()); NamedCommands.registerCommand("AlignToNote", AutonomousCommands.getAlignToNoteCommand()); NamedCommands.registerCommand("StopAligningToNote", AutonomousCommands.getStopAligningToNoteCommand()); @@ -25,8 +24,9 @@ private static void registerCommands() { NamedCommands.registerCommand("PreparePitchForCloseShot", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_SHOT_PITCH)); NamedCommands.registerCommand("StopShooting", ShooterCommands.getStopCommand()); NamedCommands.registerCommand("FeedNote", AutonomousCommands.getFeedNoteCommand()); - NamedCommands.registerCommand("PrepareForCloseShot", ShootingCommands.getPrepareCloseSpeakerShotCommand()); - NamedCommands.registerCommand("PrepareForEjectFromShooter", AutonomousCommands.getPrepareForShooterEjectionCommand(false)); - NamedCommands.registerCommand("PrepareForCloseEjectFromShooter", AutonomousCommands.getPrepareForShooterEjectionCommand(true)); + NamedCommands.registerCommand("PreparePitchForEjectFromShooter", AutonomousCommands.getPreparePitchCommand(ShootingConstants.EJECT_FROM_SHOOTER_PITCH)); + NamedCommands.registerCommand("PrepareShootingForEjectFromShooter", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PreparePitchForCloseEjectFromShooter", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_PITCH)); + NamedCommands.registerCommand("PrepareShootingForCloseEjectFromShooter", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); } } \ No newline at end of file From 7c36cd5ec5ffcccaad88aa2112e6caf5da3b2886 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 21:48:48 +0300 Subject: [PATCH 118/261] zayde --- .../pathplanner/autos/Zayde3NoteAuto.auto | 139 +++++++++++++----- 1 file changed, 103 insertions(+), 36 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto index ae6ad1d7..f982d0de 100644 --- a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 2, - "y": 2 + "x": 1.2894639873786793, + "y": 2.0234181313433055 }, "rotation": 0 }, @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForCloseEjectFromShooter" + "name": "PreparePitchForCloseEjectFromShooter" } }, { @@ -32,15 +32,22 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.05 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] @@ -56,7 +63,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -76,15 +83,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -100,15 +127,35 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -124,15 +171,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] From 42f6befdb9c429759c7a0bb4403f380d4176b93a Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 22:03:01 +0300 Subject: [PATCH 119/261] fixed speaker auto --- .../autos/3NoteAutoThroughAmp.auto | 4 +- .../autos/SpeakerSide3NoteAuto.auto | 148 ++++++++++++++---- 2 files changed, 116 insertions(+), 36 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index bae751b4..1edf3ae7 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -130,7 +130,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -174,7 +174,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.4 } }, { diff --git a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto index 997685d9..acfae16e 100644 --- a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "PrepareForCloseEjectFromShooter" + "name": "PreparePitchForCloseEjectFromShooter" } }, { @@ -32,15 +32,35 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.05 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShootingForCloseEjectFromShooter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] @@ -56,7 +76,7 @@ { "type": "named", "data": { - "name": "PrepareForShootingCloseShot" + "name": "PreparePitch" } }, { @@ -76,15 +96,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -100,15 +140,35 @@ } }, { - "type": "wait", + "type": "race", "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } }, { @@ -124,15 +184,35 @@ } }, { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] } } ] From 89580fbec4a8e075a8ef07eb7541526c68b87d5f Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 22:14:39 +0300 Subject: [PATCH 120/261] added pid --- .pathplanner/settings.json | 8 +- .../deploy/pathplanner/autos/7NoteAuto.auto | 2 +- .../autos/MiddleRush3NoteAuto.auto | 2 +- src/main/deploy/pathplanner/autos/PID.auto | 121 ++++++++++++++++++ .../autos/SpeakerSide3NoteAuto.auto | 2 +- .../pathplanner/autos/Zayde3NoteAuto.auto | 2 +- .../deploy/pathplanner/paths/PIDAuto0.path | 76 +++++++++++ .../deploy/pathplanner/paths/PIDAuto1.path | 70 ++++++++++ 8 files changed, 276 insertions(+), 7 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/PID.auto create mode 100644 src/main/deploy/pathplanner/paths/PIDAuto0.path create mode 100644 src/main/deploy/pathplanner/paths/PIDAuto1.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 37091a07..3a5cadb1 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,11 +4,12 @@ "holonomicMode": true, "pathFolders": [ "3NoteAutoThroughAmp", + "3NoteAutoThroughAmpLeft", "5NoteAutoAroundStage", "7NoteAuto", "FrontRow4NoteAuto", "MiddleRush3NoteAuto", - "3NoteAutoThroughAmpLeft", + "PIDAuto", "SourceSide3NoteAuto", "SourceSide3NoteAutoRight", "SpeakerSide3NoteAuto", @@ -16,9 +17,10 @@ ], "autoFolders": [ "3NoteAutoThroughAmpVarients", - "FarNotesAroundStageVarients", "Backup", - "Misc", + "FarNotesAroundStageVarients", + "misc", + "OtherAutos", "SourceSide3NoteAutoVarients" ], "defaultMaxVel": 4.0, diff --git a/src/main/deploy/pathplanner/autos/7NoteAuto.auto b/src/main/deploy/pathplanner/autos/7NoteAuto.auto index e5a56407..42b905bb 100644 --- a/src/main/deploy/pathplanner/autos/7NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/7NoteAuto.auto @@ -324,6 +324,6 @@ ] } }, - "folder": "Misc", + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index bd291681..3299f7aa 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -294,6 +294,6 @@ ] } }, - "folder": "Misc", + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PID.auto b/src/main/deploy/pathplanner/autos/PID.auto new file mode 100644 index 00000000..a8387446 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PID.auto @@ -0,0 +1,121 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.8939146666891922, + "y": 5.547612425391122 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "PIDAuto0" + } + }, + { + "type": "path", + "data": { + "pathName": "PIDAuto1" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "misc", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto index acfae16e..6971b900 100644 --- a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto @@ -230,6 +230,6 @@ ] } }, - "folder": "Misc", + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto index f982d0de..f189e90b 100644 --- a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto @@ -217,6 +217,6 @@ ] } }, - "folder": "Misc", + "folder": "OtherAutos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path new file mode 100644 index 00000000..4f530194 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8939146666891922, + "y": 5.547612425391122 + }, + "prevControl": null, + "nextControl": { + "x": 2.371652819315141, + "y": 5.762107106161956 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4148769485187436, + "y": 6.2105959841373375 + }, + "prevControl": { + "x": 2.8981397630253705, + "y": 6.035100336233927 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "PIDfirstcollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 12.094757077012037, + "rotateFast": false + }, + "reversed": false, + "folder": "PIDAuto", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path new file mode 100644 index 00000000..eeba84ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/PIDAuto1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.4148769485187436, + "y": 6.2105959841373375 + }, + "prevControl": null, + "nextControl": { + "x": 4.029111716180679, + "y": 6.191096467703624 + }, + "isLocked": false, + "linkedName": "PIDfirstcollection" + }, + { + "anchor": { + "x": 4.584847934541476, + "y": 6.015600819800215 + }, + "prevControl": { + "x": 4.077860507264958, + "y": 6.113098401968776 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAligningToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 5.389999999999986, + "rotateFast": false + }, + "reversed": false, + "folder": "PIDAuto", + "previewStartingState": { + "rotation": 24.102234501160947, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From e1fe7a9acfd649c2103c9f25a86d087f385037fc Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 22:16:12 +0300 Subject: [PATCH 121/261] removed redudnet --- .../factories/AutonomousCommands.java | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 27ac2fb4..a611cee9 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -72,24 +71,6 @@ public static Command getStopAligningToNoteCommand() { ); } - public static Command getPrepareForShooterEjectionCommand(boolean isClose) { - return isClose ? getPrepareForCloseShooterEjectionCommand() : getPrepareForShooterEjectionCommand(); - } - - private static Command getPrepareForShooterEjectionCommand() { - return new ParallelCommandGroup( - PitcherCommands.getSetTargetPitchCommand(ShootingConstants.EJECT_FROM_SHOOTER_PITCH), - ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND) - ); - } - - private static Command getPrepareForCloseShooterEjectionCommand() { - return new ParallelCommandGroup( - PitcherCommands.getSetTargetPitchCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_PITCH), - ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND) - ); - } - public static Command getPreparePitchCommand() { return new ParallelCommandGroup( ShootingCommands.getUpdateShootingCalculationsCommand(false), From 8700c7fec74a56201ec81af5484f059a25b8ac30 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 13 Oct 2024 23:35:15 +0300 Subject: [PATCH 122/261] fix error --- .../java/frc/trigon/robot/commands/factories/AmpCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 2d0f2acd..88948e37 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -31,7 +31,7 @@ public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { return new InstantCommand(() -> IS_FEEDING_NOTE = false).andThen(new ParallelCommandGroup( getPrepareForAmpCommand(), GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), - shouldAlignToAmp ? GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) : null + GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND).onlyIf(() -> shouldAlignToAmp) )); } From fb583acc4ae668e43a0e488030d921987793950e Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 00:20:24 +0300 Subject: [PATCH 123/261] changed climber inverted values --- .../frc/trigon/robot/subsystems/climber/ClimberConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index d78d4672..e3366432 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -27,8 +27,8 @@ public class ClimberConstants { LEFT_MOTOR = new TalonFXMotor(LEFT_MOTOR_ID, LEFT_MOTOR_NAME); private static final InvertedValue - RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive, - LEFT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive, + LEFT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; static final boolean ENABLE_FOC = true; private static final double //TODO: calibrate From aba21e36fc3b15d50691f90cc087a1733b93e4b5 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 14 Oct 2024 02:48:46 +0300 Subject: [PATCH 124/261] Made AlignToNoteCommand better --- .../trigon/robot/commands/AlignToNoteCommand.java | 12 ++++++++++-- .../frc/trigon/robot/subsystems/swerve/Swerve.java | 10 +++++----- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index c752cea6..c97c3b82 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -41,8 +41,8 @@ private Command getSetCurrentLEDColorCommand() { } private Command getDriveWhileAligningToNoteCommand() { - return SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),//stickValue), + return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> CommandConstants.calculateDriveStickAxisValue(getScaledJoystickValue()), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); @@ -56,4 +56,12 @@ private MirrorableRotation2d getTargetAngle() { private boolean hasTarget() { return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } + + private double getScaledJoystickValue() { + final XboxController controller = OperatorConstants.DRIVER_CONTROLLER; + final Rotation2d robotRelativeToFieldRelativeRotation = Rotation2d.fromDegrees(90); + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().plus(robotRelativeToFieldRelativeRotation); + + return controller.getLeftX() * -robotHeading.getCos() - controller.getLeftY() * -robotHeading.getSin(); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index e1fad94b..5b13b9c8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -134,6 +134,11 @@ public double[] getDriveWheelPositionsRadians() { return swerveModulesPositions; } + public Rotation2d getDriveRelativeAngle() { + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; + } + public void runWheelRadiusCharacterization(double omegaRadiansPerSecond) { selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)); } @@ -329,11 +334,6 @@ private ChassisSpeeds fieldRelativeSpeedsToSelfRelativeSpeeds(ChassisSpeeds fiel return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getDriveRelativeAngle()); } - private Rotation2d getDriveRelativeAngle() { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; - } - private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaPower) { return new ChassisSpeeds( xPower * SwerveConstants.MAX_SPEED_METERS_PER_SECOND, From 455c3d129fdc01f69388e12e8845b73be8432b15 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 14 Oct 2024 02:54:19 +0300 Subject: [PATCH 125/261] Reset for home speaker --- .../robot/constants/FieldConstants.java | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 98b99cbc..f00343b1 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -2,7 +2,6 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -13,17 +12,17 @@ import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); -// public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT;//AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + // public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; -// static { -// try { -// APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); -// } catch (IOException e) { -// System.out.println("test"); -// throw new RuntimeException(e); -// } -// } + static { + try { + APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); + } catch (IOException e) { + System.out.println("test"); + throw new RuntimeException(e); + } + } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double From e6adc58dc46a38040dbb130fbaecdee3e0175207 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 04:12:02 +0300 Subject: [PATCH 126/261] climber stuff --- .../java/frc/trigon/robot/RobotContainer.java | 4 +- .../commands/factories/GeneralCommands.java | 2 +- .../robot/subsystems/climber/Climber.java | 14 +----- .../subsystems/climber/ClimberConstants.java | 47 ++++++++++--------- .../robot/subsystems/intake/Intake.java | 8 ---- .../subsystems/intake/IntakeConstants.java | 11 +++-- 6 files changed, 36 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index c2e3d100..dbdd4dde 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -61,7 +61,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); INTAKE.setDefaultCommand(CommandConstants.DEFAULT_INTAKE_COMMAND); -// CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); + CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); PITCHER.setDefaultCommand(GeneralCommands.getDefaultPitcherCommand()); SHOOTER.setDefaultCommand(ShooterCommands.getStopCommand()); @@ -102,7 +102,7 @@ private void bindControllerCommands() { OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); -// configureSysIdBindings(SWERVE); +// configureSysIdBindings(CLIMBER); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index e3be92e7..c05d6ada 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -33,7 +33,7 @@ public static Command getClimbCommand() { Logger.recordOutput("IsClimbing", true); } ), - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB).until(() -> OperatorConstants.CONTINUE_TRIGGER.getAsBoolean() && RobotContainer.CLIMBER.atTargetState()), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB).until(OperatorConstants.CONTINUE_TRIGGER), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB) ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 8b56efd7..9c6e2bc1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Voltage; @@ -39,8 +40,6 @@ public class Climber extends MotorSubsystem { public Climber() { setName("Climber"); GeneralCommands.getDelayedCommand(3, this::configureChangingDefaultCommand).schedule(); - configurePositionResettingTrigger(rightMotor); - configurePositionResettingTrigger(leftMotor); } @Override @@ -133,17 +132,6 @@ void setTargetPosition(double targetRightPositionRotations, double targetLeftPos .withPosition(targetLeftPositionRotations)); } - private void configurePositionResettingTrigger(TalonFXMotor motor) { - final Trigger positionResettingTrigger; - if (motor.getID() == ClimberConstants.RIGHT_MOTOR_ID) - positionResettingTrigger = new Trigger(RobotContainer.INTAKE::hasHitRightClimberReverseLimit).debounce(ClimberConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); - else if (motor.getID() == ClimberConstants.LEFT_MOTOR_ID) - positionResettingTrigger = new Trigger(RobotContainer.INTAKE::hasHitLeftClimberReverseLimit).debounce(ClimberConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); - else - return; - positionResettingTrigger.onTrue(new InstantCommand(() -> motor.setPosition(ClimberConstants.LIMIT_SWITCH_PRESSED_POSITION))); - } - private DynamicMotionMagicVoltage determineRequest(boolean affectedByRobotWeight) { return affectedByRobotWeight ? onChainPositionRequest : groundedPositionRequest; } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index e3366432..83204530 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -1,6 +1,7 @@ package frc.trigon.robot.subsystems.climber; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; @@ -10,6 +11,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -46,23 +48,22 @@ public class ClimberConstants { RIGHT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9986, RIGHT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.21705; private static final double //TODO: calibrate - ON_CHAIN_P = RobotHardwareStats.isSimulation() ? RIGHT_GROUNDED_P : 0, + ON_CHAIN_P = RobotHardwareStats.isSimulation() ? RIGHT_GROUNDED_P : 4.5626, ON_CHAIN_I = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_I : 0, ON_CHAIN_D = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_D : 0, - ON_CHAIN_KS = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KS : 0, - ON_CHAIN_KV = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KV : 0, - ON_CHAIN_KA = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KA : 0; + ON_CHAIN_KS = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KS : 0.079947, + ON_CHAIN_KV = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KV : 7.9986, + ON_CHAIN_KA = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KA : 0.21705; static final double - MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KV : 0, - MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KA : 0, - MAX_ON_CHAIN_VELOCITY = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KV) - 0.75 : 0, - MAX_ON_CHAIN_ACCELERATION = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KA) - 50 : 0; + MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KV : 1, + MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KA : 1, + MAX_ON_CHAIN_VELOCITY = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KV) - 0.75 : 1, + MAX_ON_CHAIN_ACCELERATION = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KA) - 50 : 1; static final int GROUNDED_SLOT = 0, ON_CHAIN_SLOT = 1; - private static final double - REVERSE_SOFT_LIMIT_POSITION_ROTATIONS = 0, - FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3.183; + private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = -2.9; + private static final double LIMIT_SWITCH_RESET_POSITION = 0; static final double GEAR_RATIO = 68.57; private static final int @@ -122,15 +123,13 @@ public class ClimberConstants { MOVE_CLIMBER_DOWN_VOLTAGE = -4, MOVE_CLIMBER_UP_VOLTAGE = 4; static final double CLIMBER_TOLERANCE_ROTATIONS = 0.01; - static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.1; - static final double LIMIT_SWITCH_PRESSED_POSITION = 0; static { - configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_MOTOR_SIMULATION, RIGHT_GROUNDED_P, RIGHT_GROUNDED_I, RIGHT_GROUNDED_D, RIGHT_GROUNDED_KS, RIGHT_GROUNDED_KV, RIGHT_GROUNDED_KA); - configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_MOTOR_SIMULATION, LEFT_GROUNDED_P, LEFT_GROUNDED_I, LEFT_GROUNDED_D, LEFT_GROUNDED_KS, LEFT_GROUNDED_KV, LEFT_GROUNDED_KA); + configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_MOTOR_SIMULATION, RIGHT_GROUNDED_P, RIGHT_GROUNDED_I, RIGHT_GROUNDED_D, RIGHT_GROUNDED_KS, RIGHT_GROUNDED_KV, RIGHT_GROUNDED_KA, IntakeConstants.MASTER_MOTOR_ID); + configureMotor(LEFT_MOTOR, LEFT_MOTOR_INVERTED_VALUE, LEFT_MOTOR_SIMULATION, LEFT_GROUNDED_P, LEFT_GROUNDED_I, LEFT_GROUNDED_D, LEFT_GROUNDED_KS, LEFT_GROUNDED_KV, LEFT_GROUNDED_KA, IntakeConstants.FOLLOWER_MOTOR_ID); } - private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, SimpleMotorSimulation simulation, double groundedP, double groundedI, double groundedD, double groundedKS, double groundedKV, double groundedKA) { + private static void configureMotor(TalonFXMotor motor, InvertedValue invertedValue, SimpleMotorSimulation simulation, double groundedP, double groundedI, double groundedD, double groundedKS, double groundedKV, double groundedKA, int limitSwitchID) { final TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = invertedValue; @@ -152,10 +151,15 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal config.Slot1.kV = ON_CHAIN_KV; config.Slot1.kA = ON_CHAIN_KA; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_SOFT_LIMIT_POSITION_ROTATIONS; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS; + config.HardwareLimitSwitch.ForwardLimitRemoteSensorID = limitSwitchID; + config.HardwareLimitSwitch.ForwardLimitSource = ForwardLimitSourceValue.RemoteTalonFX; + config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = true; + config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = LIMIT_SWITCH_RESET_POSITION; + + config.MotionMagic.MotionMagicAcceleration = MAX_GROUNDED_ACCELERATION; + config.MotionMagic.MotionMagicCruiseVelocity = MAX_GROUNDED_VELOCITY; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -168,12 +172,13 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal motor.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); motor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); motor.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); + motor.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); } public enum ClimberState { REST(0, false), - PREPARE_FOR_CLIMB(3.183, false), //TODO: calibrate - CLIMB(0.1, true); //TODO: calibrate + PREPARE_FOR_CLIMB(-2.9, false), //TODO: calibrate + CLIMB(-0.1, true); //TODO: calibrate public final double positionRotations; public final boolean affectedByRobotWeight; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index ead67a65..3e4d4be5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -63,14 +63,6 @@ public boolean isEarlyNoteCollectionDetected() { return IntakeConstants.EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } - public boolean hasHitRightClimberReverseLimit() { - return masterMotor.getSignal(TalonFXSignal.REVERSE_LIMIT) == 0; - } - - public boolean hasHitLeftClimberReverseLimit() { - return IntakeConstants.FOLLOWER_MOTOR.getSignal(TalonFXSignal.REVERSE_LIMIT) == 0; - } - void sendStaticBrakeRequest() { masterMotor.setControl(staticBrakeRequest); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index ab9b396a..cac27961 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -17,10 +17,10 @@ import java.util.function.DoubleSupplier; public class IntakeConstants { - private static final int + public static final int MASTER_MOTOR_ID = 16, - FOLLOWER_MOTOR_ID = 17, - DISTANCE_SENSOR_CHANNEL = 2; + FOLLOWER_MOTOR_ID = 17; + private static final int DISTANCE_SENSOR_CHANNEL = 2; private static final String MASTER_MOTOR_NAME = "MasterIntakeMotor", FOLLOWER_MOTOR_NAME = "FollowerIntakeMotor", @@ -103,6 +103,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); } private static void configureFollowerMotor() { @@ -116,11 +117,11 @@ private static void configureFollowerMotor() { config.HardwareLimitSwitch.ReverseLimitEnable = false; FOLLOWER_MOTOR.applyConfiguration(config); + FOLLOWER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); + FOLLOWER_MOTOR.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); FOLLOWER_MOTOR.setControl(followerRequest); - - FOLLOWER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); } private static void configureDistanceSensor() { From 7d340965b9554f7ddbbb7f14928c7274994ebae7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 06:45:37 +0300 Subject: [PATCH 127/261] Amp calib + pose resetting fix --- src/main/java/frc/trigon/robot/RobotContainer.java | 6 +++--- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- .../java/frc/trigon/robot/constants/ShootingConstants.java | 6 +++--- .../robot/poseestimation/poseestimator/PoseEstimator.java | 2 ++ .../frc/trigon/robot/subsystems/ampaligner/AmpAligner.java | 3 +-- .../robot/subsystems/ampaligner/AmpAlignerConstants.java | 2 +- .../trigon/robot/subsystems/climber/ClimberConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- 8 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index dbdd4dde..0e0f8f11 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -30,7 +30,7 @@ public class RobotContainer { public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( -// CameraConstants.FRONT_TAG_CAMERA, + CameraConstants.FRONT_TAG_CAMERA, CameraConstants.REAR_TAG_CAMERA ); public static final Swerve SWERVE = new Swerve(); @@ -61,7 +61,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); INTAKE.setDefaultCommand(CommandConstants.DEFAULT_INTAKE_COMMAND); - CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); +// CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); PITCHER.setDefaultCommand(GeneralCommands.getDefaultPitcherCommand()); SHOOTER.setDefaultCommand(ShooterCommands.getStopCommand()); @@ -74,7 +74,7 @@ private void bindControllerCommands() { OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); +// OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); OperatorConstants.ALIGN_TO_SPEAKER_TRIGGER.whileTrue(CommandConstants.FACE_SPEAKER_COMMAND); OperatorConstants.ALIGN_TO_RIGHT_STAGE.whileTrue(CommandConstants.ALIGN_TO_RIGHT_STAGE_COMMAND); OperatorConstants.ALIGN_TO_LEFT_STAGE.whileTrue(CommandConstants.ALIGN_TO_LEFT_STAGE_COMMAND); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 01f295a7..b5a54065 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -47,9 +47,9 @@ public class OperatorConstants { WARM_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.w().and(SPEAKER_SHOT_TRIGGER.negate()), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m(), - AMP_TRIGGER = OPERATOR_CONTROLLER.a(), + AMP_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.x()), AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), - ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), + //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index c3f09110..d60c1cd2 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -13,15 +13,15 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, - AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 30, + AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 40, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), - PREPARE_AMP_PITCH = Rotation2d.fromDegrees(55), - SHOOT_AMP_PITCH = Rotation2d.fromDegrees(45), + PREPARE_AMP_PITCH = Rotation2d.fromDegrees(50), + SHOOT_AMP_PITCH = Rotation2d.fromDegrees(44), MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(13), EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13), CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 739c5f48..e600ed0f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -85,6 +85,8 @@ public void setGyroHeadingToBestSolvePNPHeading() { return; int closestCameraToTag = 0; for (int i = 0; i < aprilTagCameras.length; i++) { + if (aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters() == 0) + closestCameraToTag++; if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) closestCameraToTag = i; } diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java index 12765a21..b2bfccc5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java @@ -114,8 +114,7 @@ private void configurePositionResettingTrigger() { } private boolean hasHitForwardLimit() { - return OperatorConstants.OPERATOR_CONTROLLER.y().getAsBoolean(); -// return motor.getSignal(TalonFXSignal.FORWARD_LIMIT) == 0; + return motor.getSignal(TalonFXSignal.FORWARD_LIMIT) == 0; } private double calculateKGOutput() { diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java index 90e38765..e139927f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java @@ -67,7 +67,7 @@ public class AmpAlignerConstants { static final Rotation2d READY_FOR_DEFAULT_PITCHER_MOVEMENT_ANGLE = Rotation2d.fromDegrees(80); static final Rotation2d LIMIT_SWITCH_PRESSED_ANGLE = Rotation2d.fromDegrees(173.7); - static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.1; + static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.01; static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.3); static { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 83204530..7ccd09eb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -34,7 +34,7 @@ public class ClimberConstants { private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; static final boolean ENABLE_FOC = true; private static final double //TODO: calibrate - LEFT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 0.39596, + LEFT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 1, LEFT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.078964, diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index cac27961..9327bbae 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -134,7 +134,7 @@ public enum IntakeState { EJECT(-2), STOP(0), FEED_SHOOTING(8), - FEED_AMP(6), + FEED_AMP(5), CORRECT_NOTE_POSITION(-3); public final double voltage; From c1908850b6491ad113229080dab2056e4a4dabbc Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 13:35:52 +0300 Subject: [PATCH 128/261] worked on LEDs --- .../subsystems/climber/ClimberConstants.java | 4 ++-- .../robot/subsystems/intake/Intake.java | 19 +++++++++++-------- .../robot/subsystems/ledstrip/LEDStrip.java | 10 ++++++++++ .../subsystems/ledstrip/LEDStripCommands.java | 6 ++++-- 4 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 7ccd09eb..8de23979 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -177,8 +177,8 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal public enum ClimberState { REST(0, false), - PREPARE_FOR_CLIMB(-2.9, false), //TODO: calibrate - CLIMB(-0.1, true); //TODO: calibrate + PREPARE_FOR_CLIMB(-2.9, false), + CLIMB(-0.1, true); public final double positionRotations; public final boolean affectedByRobotWeight; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 3e4d4be5..caafd080 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -5,7 +5,9 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; @@ -22,6 +24,7 @@ public class Intake extends MotorSubsystem { public Intake() { setName("Intake"); + configureLEDsTrigger(); } @Override @@ -89,16 +92,16 @@ void indicateCollection() { } private Command getCollectionIndicationLEDsCommand() { - return new SequentialCommandGroup( - LEDStripCommands.getBlinkingCommand(Color.kOrange, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS), - LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS) - ); + return LEDStripCommands.getBlinkingCommand(Color.kWhite, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); } private Command getFeedingIndicationLEDsCommand() { - return new SequentialCommandGroup( - LEDStripCommands.getBlinkingCommand(Color.kYellow, IntakeConstants.FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BLINKING_TIME_SECONDS), - LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS) - ); + return LEDStripCommands.getBlinkingCommand(Color.kYellow, IntakeConstants.FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BLINKING_TIME_SECONDS); + } + + private void configureLEDsTrigger() { + Trigger trigger = new Trigger(this::hasNote); + trigger.onTrue(new InstantCommand(() -> LEDStrip.changeDefaultCommandForAllLEDs(LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS)))); + trigger.onFalse(new InstantCommand(() -> LEDStrip.changeDefaultCommandForAllLEDs(LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS)))); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 33a23208..82b2f090 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.commands.factories.GeneralCommands; @@ -37,6 +38,15 @@ public static void setDefaultCommandForAllLEDS(Command command) { ledStrip.setDefaultCommand(command); } + public static void changeDefaultCommandForAllLEDs(Command newDefaultCommand) { + for (LEDStrip ledStrip : LED_STRIPS) { + final Command currentDefaultCommand = ledStrip.getDefaultCommand(); + if (currentDefaultCommand != null) + currentDefaultCommand.cancel(); + ledStrip.setDefaultCommand(newDefaultCommand); + } + } + public int getNumberOfLEDS() { return numberOfLEDs; } diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 0e9f895d..93f08d9d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; +import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.InitExecuteCommand; import java.util.function.Consumer; @@ -12,16 +13,17 @@ public class LEDStripCommands { public static Command getStaticColorCommand(Color color, LEDStrip... ledStrips) { return new StartEndCommand( - () -> runForLEDs((ledStrip -> ledStrip.staticColor(color)), ledStrips), () -> { }, + () -> runForLEDs((ledStrip -> ledStrip.staticColor(color)), ledStrips), ledStrips ).ignoringDisable(true); } public static Command getBlinkingCommand(Color color, double blinkingIntervalSeconds, LEDStrip... ledStrips) { - return new RunCommand( + return new ExecuteEndCommand( () -> runForLEDs((ledStrip -> ledStrip.blink(color, blinkingIntervalSeconds)), ledStrips), + () -> runForLEDs((LEDStrip::clearLedColors)), ledStrips ).ignoringDisable(true); } From 2575d6209aecb1dab5aa3efd586fe7e5146ec882 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 17:11:47 +0300 Subject: [PATCH 129/261] SysID swerve --- src/main/deploy/pathplanner/paths/PIDAuto0.path | 2 +- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- .../robot/subsystems/swerve/SwerveModuleConstants.java | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path index 4f530194..1e5793d4 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -63,7 +63,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 12.094757077012037, + "rotation": 44.16968451374187, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0e0f8f11..cb3fe6ca 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -102,7 +102,7 @@ private void bindControllerCommands() { OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); -// configureSysIdBindings(CLIMBER); +// configureSysIdBindings(SWERVE); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 4c491a41..d5c728c9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -32,12 +32,12 @@ public class SwerveModuleConstants { STEER_MOTOR_I = 0, STEER_MOTOR_D = 0; private static final double - DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 50, + DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 52, DRIVE_MOTOR_I = 0, DRIVE_MOTOR_D = 0, - DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 0, - DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0, - DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 0; + DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 6.2176, + DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0.0017378, + DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 2.4345; static final boolean ENABLE_FOC = true; static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = generateDriveConfiguration(), From bcdc2935358813fd37020d79eb59e709653d3fe8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 21:31:57 +0300 Subject: [PATCH 130/261] auto and other --- .../autos/3NoteAutoThroughAmpLeft.auto | 63 +++++++++++++---- .../pathplanner/autos/FrontRow4NoteAuto.auto | 70 ++++++++++++++++--- src/main/deploy/pathplanner/autos/PID.auto | 4 +- .../paths/3NoteAutoThroughAmp1.path | 8 +-- .../paths/3NoteAutoThroughAmp2.path | 20 +----- .../paths/3NoteAutoThroughAmpLeft3.path | 8 +-- .../paths/3NoteAutoThroughAmpLeft5.path | 8 +-- .../pathplanner/paths/FrontRow4NoteAuto1.path | 32 ++------- .../pathplanner/paths/FrontRow4NoteAuto2.path | 22 +++++- .../pathplanner/paths/FrontRow4NoteAuto3.path | 22 +----- .../pathplanner/paths/FrontRow4NoteAuto4.path | 22 +++++- .../pathplanner/paths/FrontRow4NoteAuto5.path | 24 +------ .../pathplanner/paths/FrontRow4NoteAuto6.path | 22 +++++- .../deploy/pathplanner/paths/PIDAuto0.path | 22 +++--- .../deploy/pathplanner/paths/PIDAuto1.path | 38 +++------- .../java/frc/trigon/robot/RobotContainer.java | 4 +- .../robot/commands/AlignToNoteCommand.java | 2 +- .../robot/commands/factories/AmpCommands.java | 2 +- .../factories/AutonomousCommands.java | 9 ++- .../robot/constants/ShootingConstants.java | 2 +- .../poseestimator/PoseEstimator.java | 5 +- .../subsystems/climber/ClimberConstants.java | 2 +- .../robot/subsystems/intake/Intake.java | 1 + .../subsystems/intake/IntakeConstants.java | 4 +- 24 files changed, 233 insertions(+), 183 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index 6c29b06f..0074e3b9 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7531929553170285, - "y": 6.932078092184688 + "x": 0.45095045059448935, + "y": 7.0490751907869615 }, "rotation": 0 }, @@ -32,7 +32,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -64,13 +64,26 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp2" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp2" + } + } + ] } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -102,9 +115,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmpLeft3" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft3" + } + } + ] } }, { @@ -114,7 +140,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -146,9 +172,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmpLeft5" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft5" + } + } + ] } }, { @@ -158,7 +197,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index 6a71d0a8..ff14fe32 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -58,9 +58,35 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto1" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto1" + } + } + ] } }, { @@ -86,7 +112,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -102,9 +128,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto3" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto3" + } + } + ] } }, { @@ -130,7 +169,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.5 } }, { @@ -146,9 +185,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto5" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto5" + } + } + ] } }, { @@ -174,7 +226,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { diff --git a/src/main/deploy/pathplanner/autos/PID.auto b/src/main/deploy/pathplanner/autos/PID.auto index a8387446..2dc3ba5a 100644 --- a/src/main/deploy/pathplanner/autos/PID.auto +++ b/src/main/deploy/pathplanner/autos/PID.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.8939146666891922, - "y": 5.547612425391122 + "x": 1.6306711948340775, + "y": 2.359441488479177 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path index 8a0c8c1a..96b52c8f 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7531929553170285, - "y": 6.932078092184688 + "x": 0.45095045059448935, + "y": 7.0490751907869615 }, "prevControl": null, "nextControl": { - "x": 1.7531929553170258, - "y": 6.932078092184688 + "x": 1.4509504505944886, + "y": 7.0490751907869615 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index e9427238..9f8ae2e0 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index 6510cd24..91b29753 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNote", "waypointRelativePos": 0.85, "command": { "type": "parallel", @@ -43,12 +43,6 @@ "data": { "name": "AlignToNote" } - }, - { - "type": "named", - "data": { - "name": "Collect" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index 742f1c95..48f32096 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNote", "waypointRelativePos": 0.85, "command": { "type": "parallel", @@ -43,12 +43,6 @@ "data": { "name": "AlignToNote" } - }, - { - "type": "named", - "data": { - "name": "Collect" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index 2ff87b9f..937806d5 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.0480159688234565, - "y": 6.323768972626285 + "x": 1.9719127324240409, + "y": 6.415340906691315 }, "isLocked": false, "linkedName": "CloseShotPoint" @@ -20,8 +20,8 @@ "y": 6.990384785778517 }, "prevControl": { - "x": 2.077024083376788, - "y": 6.657362289989598 + "x": 2.166907896761163, + "y": 6.5518375217273 }, "nextControl": null, "isLocked": false, @@ -30,34 +30,16 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 15.296223645005243, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index 4984fa91..80302efa 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -30,9 +30,27 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "PrepareForShooting", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + } + } + ], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index 144fb5fd..9c590f32 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -30,27 +30,9 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index 6659bb1c..cf4946b1 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -30,9 +30,27 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "PrepareForShootingCloseShot", + "waypointRelativePos": 0.1, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + } + } + ], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index 91ecdd87..e2a567ca 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -30,34 +30,16 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 1, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -8.487656913310161, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index b14fe81b..a3900415 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -30,9 +30,27 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "PrepareForShootingCloseSHot", + "waypointRelativePos": 0.1, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + } + } + ], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path index 1e5793d4..f6ea1247 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.8939146666891922, - "y": 5.547612425391122 + "x": 1.6306711948340775, + "y": 2.359441488479177 }, "prevControl": null, "nextControl": { - "x": 2.371652819315141, - "y": 5.762107106161956 + "x": 2.3521533028814288, + "y": 1.90120285228694 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.4148769485187436, - "y": 6.2105959841373375 + "x": 3.385627673868176, + "y": 1.8622038194195158 }, "prevControl": { - "x": 2.8981397630253705, - "y": 6.035100336233927 + "x": 2.8688904883748028, + "y": 1.6867081715161052 }, "nextControl": null, "isLocked": false, @@ -38,12 +38,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path index eeba84ec..39463d71 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto1.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.4148769485187436, - "y": 6.2105959841373375 + "x": 3.385627673868176, + "y": 1.8622038194195158 }, "prevControl": null, "nextControl": { - "x": 4.029111716180679, - "y": 6.191096467703624 + "x": 3.999862441530111, + "y": 1.8427043029858021 }, "isLocked": false, "linkedName": "PIDfirstcollection" }, { "anchor": { - "x": 4.584847934541476, - "y": 6.015600819800215 + "x": 4.818842131746022, + "y": 1.40396518322728 }, "prevControl": { - "x": 4.077860507264958, - "y": 6.113098401968776 + "x": 4.311854704469504, + "y": 1.5014627653958412 }, "nextControl": null, "isLocked": false, @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, @@ -57,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 5.389999999999986, + "rotation": -45.744059202888785, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index cb3fe6ca..aa2b4961 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -15,6 +15,7 @@ import frc.trigon.robot.commands.factories.ShootingCommands; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ampaligner.AmpAligner; @@ -23,6 +24,7 @@ import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.pitcher.Pitcher; +import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; @@ -61,7 +63,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); INTAKE.setDefaultCommand(CommandConstants.DEFAULT_INTAKE_COMMAND); -// CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); + CLIMBER.setDefaultCommand(CommandConstants.DEFAULT_CLIMBER_COMMAND); AMP_ALIGNER.setDefaultCommand(CommandConstants.DEFAULT_AMP_ALIGNER_COMMAND); PITCHER.setDefaultCommand(GeneralCommands.getDefaultPitcherCommand()); SHOOTER.setDefaultCommand(ShooterCommands.getStopCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index c97c3b82..829aaa38 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -22,7 +22,7 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.NOTE_DETECTION_CAMERA; private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : - new PIDController(0.01, 0, 0); + new PIDController(0.0001, 0, 0); public AlignToNoteCommand() { addCommands( diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 88948e37..5387cee7 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -31,7 +31,7 @@ public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { return new InstantCommand(() -> IS_FEEDING_NOTE = false).andThen(new ParallelCommandGroup( getPrepareForAmpCommand(), GeneralCommands.runWhenContinueTriggerPressed(getFeedToAmpCommand()), - GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND).onlyIf(() -> shouldAlignToAmp) + shouldAlignToAmp ? GeneralCommands.duplicate(CommandConstants.FACE_AMP_COMMAND) : GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND) )); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index a611cee9..db29211b 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -10,7 +10,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.AlignToNoteCommand; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.CameraConstants; +import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -42,7 +45,11 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { } public static Command getNoteCollectionCommand() { - return IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT).onlyIf(() -> !RobotContainer.INTAKE.hasNote()); + return new ParallelCommandGroup( + LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), + ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) + ); } public static Command getFeedNoteCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index d60c1cd2..a28f7d7a 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -12,7 +12,7 @@ public class ShootingConstants { DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; public static final double - CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, + CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 40, AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 40, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index e600ed0f..d41532f3 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -85,8 +85,11 @@ public void setGyroHeadingToBestSolvePNPHeading() { return; int closestCameraToTag = 0; for (int i = 0; i < aprilTagCameras.length; i++) { - if (aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters() == 0) + if (aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters() == 0) { closestCameraToTag++; + if (closestCameraToTag > aprilTagCameras.length - 1) + return; + } if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) closestCameraToTag = i; } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 8de23979..993fcd09 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -178,7 +178,7 @@ private static void configureMotor(TalonFXMotor motor, InvertedValue invertedVal public enum ClimberState { REST(0, false), PREPARE_FOR_CLIMB(-2.9, false), - CLIMB(-0.1, true); + CLIMB(0, true); public final double positionRotations; public final boolean affectedByRobotWeight; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index caafd080..3cd73b04 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -33,6 +33,7 @@ public void updatePeriodically() { IntakeConstants.DISTANCE_SENSOR.updateSensor(); Logger.recordOutput("HasNote", hasNote()); Logger.recordOutput("distanceSensorScaledValue", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); + Logger.recordOutput("currentnote", isEarlyNoteCollectionDetected()); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 9327bbae..c786a8a4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -65,8 +65,8 @@ public class IntakeConstants { () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 0.07; - private static final double NOTE_COLLECTION_CURRENT = 10; //TODO: calibrate - private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.2; //TODO: calibrate + private static final double NOTE_COLLECTION_CURRENT = 50; + private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.1; static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), () -> Math.abs(MASTER_MOTOR.getSignal(TalonFXSignal.TORQUE_CURRENT)) > IntakeConstants.NOTE_COLLECTION_CURRENT From 513a29e603dd6610a64dc735f85da998ad987d45 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 21:33:38 +0300 Subject: [PATCH 131/261] auto PID --- src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 829aaa38..63021880 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -22,7 +22,7 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.NOTE_DETECTION_CAMERA; private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : - new PIDController(0.0001, 0, 0); + new PIDController(0.0002, 0, 0); public AlignToNoteCommand() { addCommands( From adcf6ba5ec776e0c5b82fbcf32a8d29db5b72564 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 14 Oct 2024 21:37:14 +0300 Subject: [PATCH 132/261] grtwfgsdtfc --- src/main/java/frc/trigon/robot/constants/ShootingConstants.java | 2 +- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index a28f7d7a..d60c1cd2 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -12,7 +12,7 @@ public class ShootingConstants { DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; public static final double - CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 40, + CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 40, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 3cd73b04..caafd080 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -33,7 +33,6 @@ public void updatePeriodically() { IntakeConstants.DISTANCE_SENSOR.updateSensor(); Logger.recordOutput("HasNote", hasNote()); Logger.recordOutput("distanceSensorScaledValue", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); - Logger.recordOutput("currentnote", isEarlyNoteCollectionDetected()); } @Override From faae107f8ef45f25c84bff651b47963d98842b3c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 09:28:09 +0300 Subject: [PATCH 133/261] Swerve offset update --- .../frc/trigon/robot/subsystems/swerve/SwerveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 5b406895..71ed1759 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -28,7 +28,7 @@ public class SwerveConstants { private static final double FRONT_LEFT_STEER_ENCODER_OFFSET = -1.561, FRONT_RIGHT_STEER_ENCODER_OFFSET = -3.431, - REAR_LEFT_STEER_ENCODER_OFFSET = -1.537, + REAR_LEFT_STEER_ENCODER_OFFSET = -1.239, REAR_RIGHT_STEER_ENCODER_OFFSET = -2.008; private static final int FRONT_LEFT_ID = 1, From f0729fc59dd13c430caafb154213a6f520e3f54e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 10:16:17 +0300 Subject: [PATCH 134/261] Testing LED stuff --- .../robot/subsystems/ledstrip/LEDStrip.java | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 82b2f090..11d70fc1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -1,10 +1,10 @@ package frc.trigon.robot.subsystems.ledstrip; import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.commands.factories.GeneralCommands; @@ -87,20 +87,32 @@ void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); } - private void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { - for (int i = 0; i < ledsPerSection; i++) - setLEDColor(inverted ? thirdSectionColor : firstSectionColor, i); - for (int i = ledsPerSection; i < 2 * ledsPerSection; i++) - setLEDColor(secondSectionColor, i); - for (int i = 2 * ledsPerSection; i <= numberOfLEDs; i++) - setLEDColor(inverted ? firstSectionColor : thirdSectionColor, i); + public void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { + setLEDColors(inverted ? thirdSectionColor : firstSectionColor, 0, ledsPerSection); + setLEDColors(secondSectionColor, ledsPerSection, ledsPerSection * 2); + setLEDColors(inverted ? firstSectionColor : thirdSectionColor, ledsPerSection * 2, numberOfLEDs - 1); } - private void setLEDColor(Color color, int index) { + public void setLEDColor(Color color, int index) { LEDStripConstants.LED_BUFFER.setLED(index + indexOffset, color); LED.setData(LEDStripConstants.LED_BUFFER); } + public void setLEDColors(Color color, int startIndex, int endIndex) { + final AddressableLEDBuffer buffer = new AddressableLEDBuffer(LEDStripConstants.LED_BUFFER.getLength()); + for (int i = 0; i < buffer.getLength(); i++) { + if (i > startIndex + indexOffset && i < endIndex + indexOffset) { + buffer.setLED(i, color); + continue; + } + buffer.setLED(i, LEDStripConstants.LED_BUFFER.getLED(i)); + } + LED.setData(buffer); +// for (int i = 0; i < endIndex - startIndex; i++) { +// buffer.setLED(startIndex + indexOffset + i, color); +// } + } + private void addLEDStripToLEDStripsArray(LEDStrip ledStrip) { final LEDStrip[] newLEDStrips = new LEDStrip[LED_STRIPS.length + 1]; System.arraycopy(LED_STRIPS, 0, newLEDStrips, 0, LED_STRIPS.length); From aeab09455de65246d23e05bace80d3ab8995c661 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Tue, 15 Oct 2024 10:33:10 +0300 Subject: [PATCH 135/261] Removed ugly thing --- src/main/java/frc/trigon/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index aa2b4961..097cc134 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -83,7 +83,6 @@ private void bindControllerCommands() { OperatorConstants.ALIGN_TO_MIDDLE_STAGE.whileTrue(CommandConstants.ALIGN_TO_MIDDLE_STAGE_COMMAND); OperatorConstants.EJECT_NOTE_TRIGGER.whileTrue(CommandConstants.EJECT_COMMAND); - OperatorConstants.OPERATOR_CONTROLLER.l().whileTrue(IntakeCommands.getSetTargetVoltageCommand(2)); OperatorConstants.COLLECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getNoteCollectionCommand()); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); From 78872c800d9e63781821a4911e2bc6bf843ebf62 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 11:27:16 +0300 Subject: [PATCH 136/261] fixed frontRowAuto --- .../pathplanner/autos/FrontRow4NoteAuto.auto | 157 ++++++++++++++---- .../pathplanner/paths/FrontRow4NoteAuto1.path | 20 +-- .../pathplanner/paths/FrontRow4NoteAuto3.path | 20 +-- .../pathplanner/paths/FrontRow4NoteAuto5.path | 20 +-- .../factories/AutonomousCommands.java | 8 +- .../robot/constants/ShootingConstants.java | 3 +- .../robot/subsystems/shooter/Shooter.java | 7 + .../subsystems/shooter/ShooterCommands.java | 10 ++ 8 files changed, 153 insertions(+), 92 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index 6a71d0a8..de93630c 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -58,15 +58,35 @@ } }, { - "type": "path", - "data": { - "pathName": "FrontRow4NoteAuto1" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto2" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] } }, { @@ -74,19 +94,38 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShootingCloseShot" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto2" + } + }, { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -102,15 +141,22 @@ } }, { - "type": "path", - "data": { - "pathName": "FrontRow4NoteAuto3" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto4" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto3" + } + } + ] } }, { @@ -118,19 +164,38 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShootingCloseShot" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto4" + } + }, { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -146,15 +211,22 @@ } }, { - "type": "path", - "data": { - "pathName": "FrontRow4NoteAuto5" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "FrontRow4NoteAuto6" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto5" + } + } + ] } }, { @@ -162,19 +234,38 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShootingCloseShot" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto6" + } + }, { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index 2ff87b9f..c212a136 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index 144fb5fd..2ff4d408 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index 91ecdd87..79169821 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 1, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.0, "maxAcceleration": 2.0, diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index a611cee9..51bb3d46 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -10,7 +10,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.CameraConstants; +import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -42,7 +44,11 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { } public static Command getNoteCollectionCommand() { - return IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT).onlyIf(() -> !RobotContainer.INTAKE.hasNote()); + return new ParallelCommandGroup( + LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), + ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) + ); } public static Command getFeedNoteCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 7f814afc..603ff857 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -9,7 +9,8 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, - DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; + DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, + FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java index 822f3359..d953e334 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc.trigon.robot.subsystems.shooter; +import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import edu.wpi.first.units.Measure; @@ -19,6 +20,7 @@ public class Shooter extends MotorSubsystem { leftMotor = ShooterConstants.LEFT_MOTOR; private final VelocityTorqueCurrentFOC velocityRequest = new VelocityTorqueCurrentFOC(0); private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + private final StaticBrake staticBrakeRequest = new StaticBrake(); private double targetRightVelocityRotationsPerSecond = 0, targetLeftVelocityRotationsPerSecond = 0; @@ -90,6 +92,11 @@ public boolean atTargetLeftVelocity() { return atVelocity(getCurrentLeftMotorVelocityRotationsPerSecond(), targetLeftVelocityRotationsPerSecond); } + void sendStaticBrakeRequest() { + rightMotor.setControl(staticBrakeRequest); + leftMotor.setControl(staticBrakeRequest); + } + void reachTargetShootingVelocityFromShootingCalculations() { final double targetRightVelocityRotationsPerSecond = shootingCalculations.getTargetShootingState().targetShootingVelocityRotationsPerSecond(), diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java index 291ce691..616bf710 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java @@ -39,6 +39,16 @@ public static Command getSetTargetVelocityCommand(double targetRightVelocityRota RobotContainer.SHOOTER ); } + + public static Command getSendStaticBreakRequestCommand() { + return new StartEndCommand( + RobotContainer.SHOOTER::sendStaticBrakeRequest, + () -> { + }, + RobotContainer.SHOOTER + ); + + } public static Command getStopCommand() { return new StartEndCommand( From d6d851a0b6701217f80484645d6aa5ad05ff56a0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Tue, 15 Oct 2024 11:27:51 +0300 Subject: [PATCH 137/261] Removed SysID from `RobotContainer.java` --- src/main/java/frc/trigon/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 097cc134..414ca9fb 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -103,7 +103,6 @@ private void bindControllerCommands() { OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); -// configureSysIdBindings(SWERVE); } private void configureSysIdBindings(MotorSubsystem subsystem) { From c3b39eaee47036e0d39817c5d5759fa314d9f820 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 11:35:52 +0300 Subject: [PATCH 138/261] fixed bb auto --- .../autos/SourceSide3NoteAutoRight.auto | 173 +++++++++++++++--- .../paths/SourceSide3NoteAuto1.path | 2 +- .../paths/SourceSide3NoteAutoRight2.path | 8 +- .../paths/SourceSide3NoteAutoRight4.path | 8 +- 4 files changed, 148 insertions(+), 43 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index d19c6a51..8a956f6b 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -25,26 +25,39 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, { "type": "race", "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto1" + } + }, { "type": "wait", "data": { @@ -64,15 +77,48 @@ } }, { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight2" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "SourceSide3NoteAutoRight3" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { @@ -80,15 +126,34 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight3" + } + }, { "type": "wait", "data": { @@ -108,15 +173,48 @@ } }, { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAutoRight4" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "SourceSide3NoteAutoRight5" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { @@ -124,15 +222,34 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight5" + } + }, { "type": "wait", "data": { diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 46434bec..f60e2bf0 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -44.60000000000002, + "rotation": -45.25, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index 4715f55f..bfa9e651 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNote", "waypointRelativePos": 0.85, "command": { "type": "parallel", @@ -43,12 +43,6 @@ "data": { "name": "AlignToNote" } - }, - { - "type": "named", - "data": { - "name": "Collect" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index 5d3c1194..134d51cf 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNote", "waypointRelativePos": 0.85, "command": { "type": "parallel", @@ -43,12 +43,6 @@ "data": { "name": "AlignToNote" } - }, - { - "type": "named", - "data": { - "name": "Collect" - } } ] } From cf242ca222fcc8171589729c2bb6006ab6a5b8ee Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 11:51:22 +0300 Subject: [PATCH 139/261] fixed3NoteAytiKeftAuto --- .../autos/3NoteAutoThroughAmpLeft.auto | 226 +++++++++++++++--- .../paths/3NoteAutoThroughAmp2.path | 4 +- .../paths/3NoteAutoThroughAmpLeft3.path | 17 +- .../paths/3NoteAutoThroughAmpLeft5.path | 8 +- 4 files changed, 208 insertions(+), 47 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index 6c29b06f..5069f34c 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -5,7 +5,7 @@ "x": 0.7531929553170285, "y": 6.932078092184688 }, - "rotation": 0 + "rotation": 41.22999999999999 }, "command": { "type": "sequential", @@ -26,25 +26,38 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp1" - } - }, - { - "type": "parallel", + "type": "race", "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp1" + } + }, { "type": "wait", "data": { @@ -64,13 +77,52 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp2" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -102,31 +154,83 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmpLeft3" - } - }, - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmpLeft4" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft4" + } + }, { "type": "wait", "data": { @@ -146,31 +250,83 @@ } }, { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmpLeft5" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp4" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmpLeft5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp4" + } + }, { "type": "wait", "data": { diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index e9427238..0dfdad5b 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNote", "waypointRelativePos": 0.9, "command": { "type": "parallel", @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "Collect" + "name": "AlignToNote" } } ] diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index 6510cd24..fddfe193 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNote", "waypointRelativePos": 0.85, "command": { "type": "parallel", @@ -43,11 +43,22 @@ "data": { "name": "AlignToNote" } - }, + } + ] + } + } + }, + { + "name": "StopAligningToFirstNote", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ { "type": "named", "data": { - "name": "Collect" + "name": "StopAligningToNote" } } ] diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index 742f1c95..48f32096 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNote", "waypointRelativePos": 0.85, "command": { "type": "parallel", @@ -43,12 +43,6 @@ "data": { "name": "AlignToNote" } - }, - { - "type": "named", - "data": { - "name": "Collect" - } } ] } From 18ecadb7318ebafde703c45e8625ecea248362cd Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 12:07:02 +0300 Subject: [PATCH 140/261] fixed 3noteAutoThroughAmp --- .../autos/3NoteAutoThroughAmp.auto | 220 +++++++++++++++--- .../paths/3NoteAutoThroughAmp3.path | 25 +- .../paths/3NoteAutoThroughAmp5.path | 10 +- 3 files changed, 211 insertions(+), 44 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index 1edf3ae7..e4cf24f2 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -25,26 +25,39 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp1" - } - }, { "type": "race", "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp1" + } + }, { "type": "wait", "data": { @@ -64,9 +77,48 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp2" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { @@ -102,15 +154,48 @@ } }, { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp3" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp4" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { @@ -118,19 +203,38 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp4" + } + }, { "type": "wait", "data": { - "waitTime": 0.3 + "waitTime": 0.2 } }, { @@ -146,15 +250,48 @@ } }, { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp5" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "3NoteAutoThroughAmp6" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] } }, { @@ -162,19 +299,38 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareForShooting" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] } }, { "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "3NoteAutoThroughAmp6" + } + }, { "type": "wait", "data": { - "waitTime": 0.4 + "waitTime": 0.2 } }, { diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index 13331f8c..4b1d0441 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -28,11 +28,17 @@ "linkedName": "3NoteAutoThroughAmpSecondCollection" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": -20.0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { - "name": "Collect", + "name": "AlignToNOte", "waypointRelativePos": 0.9, "command": { "type": "parallel", @@ -43,11 +49,22 @@ "data": { "name": "AlignToNote" } - }, + } + ] + } + } + }, + { + "name": "StopALigningToFIrstNote", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ { "type": "named", "data": { - "name": "Collect" + "name": "StopAligningToNote" } } ] diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index 5ff5405f..39510746 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -32,18 +32,12 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Collect", - "waypointRelativePos": 0.95, + "name": "AlignTOnOte", + "waypointRelativePos": 0.9, "command": { "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - }, { "type": "named", "data": { From 29995c61d2be5f129f21a03485480f839675a70c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 12:08:06 +0300 Subject: [PATCH 141/261] made autos slower --- .pathplanner/settings.json | 4 ++-- src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path | 4 ++-- src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path | 4 ++-- src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path | 4 ++-- src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path | 4 ++-- src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path | 4 ++-- src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path | 4 ++-- .../deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path | 4 ++-- .../deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path | 4 ++-- .../deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path | 4 ++-- src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto1.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto10.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto2.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto3.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto4.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto5.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto6.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto7.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto8.path | 4 ++-- src/main/deploy/pathplanner/paths/7NoteAuto9.path | 4 ++-- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path | 2 +- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path | 2 +- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path | 2 +- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path | 2 +- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path | 2 +- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path | 2 +- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path | 4 ++-- src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path | 4 ++-- src/main/deploy/pathplanner/paths/PIDAuto0.path | 4 ++-- src/main/deploy/pathplanner/paths/PIDAuto1.path | 4 ++-- src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path | 4 ++-- src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path | 4 ++-- src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path | 4 ++-- src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path | 4 ++-- src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path | 4 ++-- .../deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path | 4 ++-- .../deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path | 4 ++-- .../deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path | 4 ++-- .../deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path | 4 ++-- src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path | 4 ++-- src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path | 4 ++-- src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path | 4 ++-- src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path | 4 ++-- src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path | 4 ++-- src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path | 4 ++-- src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path | 4 ++-- src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path | 4 ++-- src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path | 4 ++-- src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path | 4 ++-- src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path | 4 ++-- src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path | 4 ++-- src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path | 4 ++-- src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path | 4 ++-- 68 files changed, 130 insertions(+), 130 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 3a5cadb1..26719f91 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -23,8 +23,8 @@ "OtherAutos", "SourceSide3NoteAutoVarients" ], - "defaultMaxVel": 4.0, - "defaultMaxAccel": 4.0, + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 400.0, "defaultMaxAngAccel": 600.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path index 8a0c8c1a..fc74d7ac 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index 0dfdad5b..4ba2ea85 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index 4b1d0441..fecbc71b 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -73,8 +73,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path index dfc4f69c..9737a241 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index 39510746..dcf5783b 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path index 772b1882..1e67aaf7 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index fddfe193..0bd26da7 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -67,8 +67,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path index 476e99c4..911bdc47 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index 48f32096..58ddedea 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index c5d8c5bd..457717aa 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -83,8 +83,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path index f422cfe7..141da755 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index cb1650c4..3ebd7fe6 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path index c57fb19e..e00f004b 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index 3db82942..b3f969e2 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path index e3f4857e..e8566d30 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index 4409d6f9..22643e3f 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path index e8c20446..ce748388 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto1.path b/src/main/deploy/pathplanner/paths/7NoteAuto1.path index fd1ba5c3..6389f141 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto1.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto10.path b/src/main/deploy/pathplanner/paths/7NoteAuto10.path index 0d9ba444..30ec2a05 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto10.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto10.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto2.path b/src/main/deploy/pathplanner/paths/7NoteAuto2.path index 3e9c03e8..18bb36b9 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto3.path b/src/main/deploy/pathplanner/paths/7NoteAuto3.path index 5f0bf0c7..a94671c2 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto4.path b/src/main/deploy/pathplanner/paths/7NoteAuto4.path index 62d67501..b641481e 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto4.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index feeda5c4..61951a44 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto6.path b/src/main/deploy/pathplanner/paths/7NoteAuto6.path index c44fd3ef..3c318de5 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index 8a219a75..e527b7a5 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto8.path b/src/main/deploy/pathplanner/paths/7NoteAuto8.path index 4b2e2f5d..ea8fb762 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto8.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index 8a5a11c8..2a67a99f 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index c212a136..fb17f2e7 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index 4984fa91..a91958bf 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index 2ff4d408..9c590f32 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index 6659bb1c..d62b5f83 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index 79169821..4eff08ce 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index b14fe81b..4bae27ab 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path index 57ee08a9..0a125b75 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index 06caaf6b..7d443fc2 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path index 4cfc2e8f..a4d1a0a3 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index 111d74b7..b9262a9f 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path index 45b515ec..c7034b58 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 95a38025..616150e5 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path index f4c6b380..2d9b651d 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index b8525708..97ab6038 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path index 12de1af1..1bab242d 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path index 4f530194..5183cd89 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path index eeba84ec..7ec29e53 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto1.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto1.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index f60e2bf0..463907fb 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path index 907bbf0c..0decd1a1 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path index bf6a72f5..076e4ab2 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path index 63fbc590..7bceffea 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path index 2f2f8330..1b06792a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index bfa9e651..e52b1029 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 2d27fb09..7726f0bb 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index 134d51cf..dbad861e 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 71d45383..40b9436a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path index 1d925fff..bc6d054b 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index 0dc4c0f2..78bc74b1 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path index 9dc9c24f..089456c3 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index 6ee78d46..19e9f882 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path index 5be32108..e715e7d6 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index 7d81e619..b899f37d 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path index a48e02a0..cc0ad275 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path index f42b13ad..26f46f77 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index 205bc137..93b8dc39 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path index 8ae59228..9a04a02c 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index 36985c78..02ae8c58 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path index dc588074..45baa73b 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index ceb512de..5df399c5 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path index dc99d94b..c442bf8d 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, From 909fc72768764c548fe812508c000fb9ed0d81ea Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 12:14:57 +0300 Subject: [PATCH 142/261] fixed error --- .../frc/trigon/robot/subsystems/shooter/Shooter.java | 5 ----- .../robot/subsystems/shooter/ShooterCommands.java | 11 ----------- 2 files changed, 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java index 40e670a7..f48d6dc5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java @@ -92,11 +92,6 @@ public boolean atTargetLeftVelocity() { return atVelocity(getCurrentLeftMotorVelocityRotationsPerSecond(), targetLeftVelocityRotationsPerSecond); } - void sendStaticBrakeRequest() { - rightMotor.setControl(staticBrakeRequest); - leftMotor.setControl(staticBrakeRequest); - } - void reachTargetShootingVelocityFromShootingCalculations() { final double targetRightVelocityRotationsPerSecond = shootingCalculations.getTargetShootingState().targetShootingVelocityRotationsPerSecond(), diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java index ceb160c3..528a76a8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java @@ -39,16 +39,6 @@ public static Command getSetTargetVelocityCommand(double targetRightVelocityRota RobotContainer.SHOOTER ); } - - public static Command getSendStaticBreakRequestCommand() { - return new StartEndCommand( - RobotContainer.SHOOTER::sendStaticBrakeRequest, - () -> { - }, - RobotContainer.SHOOTER - ); - - } public static Command getSendStaticBreakRequestCommand() { return new StartEndCommand( @@ -57,7 +47,6 @@ public static Command getSendStaticBreakRequestCommand() { }, RobotContainer.SHOOTER ); - } public static Command getStopCommand() { From 18a5b3c30b2684145a65112f6e845025ed460e8c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 12:16:18 +0300 Subject: [PATCH 143/261] fixed error --- .../java/frc/trigon/robot/constants/ShootingConstants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index bcfe81c3..c79580a8 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -17,8 +17,7 @@ public class ShootingConstants { AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 40, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, - CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5, - FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; + CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; public static final Rotation2d CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), PREPARE_AMP_PITCH = Rotation2d.fromDegrees(50), From 5fb6402c6d2ae6cbfde370b2f638a9de9712ae9d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 12:32:52 +0300 Subject: [PATCH 144/261] fix front row auto --- .../pathplanner/autos/FrontRow4NoteAuto.auto | 85 ++++++++++++++++--- .../pathplanner/paths/FrontRow4NoteAuto2.path | 20 +---- .../pathplanner/paths/FrontRow4NoteAuto4.path | 20 +---- .../pathplanner/paths/FrontRow4NoteAuto6.path | 20 +---- .../factories/AutonomousCommands.java | 5 +- .../robot/constants/AutonomousConstants.java | 2 - 6 files changed, 80 insertions(+), 72 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index de93630c..7d7eafda 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -81,9 +81,22 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "Collect" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] } } ] @@ -145,15 +158,41 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "Collect" + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "FrontRow4NoteAuto3" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] } } ] @@ -215,15 +254,41 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "Collect" + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "FrontRow4NoteAuto5" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index 80302efa..a91958bf 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "PrepareForShooting", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShootingCloseShot" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index cf4946b1..d62b5f83 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "PrepareForShootingCloseShot", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShootingCloseShot" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index a3900415..4bae27ab 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "PrepareForShootingCloseSHot", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShootingCloseShot" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 51bb3d46..f97f7907 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; @@ -45,10 +44,10 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( - LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy(), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) - ); + ).onlyIf(() -> !RobotContainer.INTAKE.hasNote()); } public static Command getFeedNoteCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 6f61b4ec..eeeda3eb 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -2,9 +2,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.wpilibj2.command.RunCommand; import frc.trigon.robot.commands.factories.AutonomousCommands; -import frc.trigon.robot.commands.factories.ShootingCommands; import frc.trigon.robot.subsystems.shooter.ShooterCommands; public class AutonomousConstants { From bf5eaf901f11d043ccaf859352bdd1fe13520132 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Tue, 15 Oct 2024 13:43:20 +0300 Subject: [PATCH 145/261] stuff --- .../java/frc/trigon/robot/RobotContainer.java | 3 +++ .../commands/factories/GeneralCommands.java | 7 +++++++ .../robot/constants/CameraConstants.java | 2 +- .../robot/constants/OperatorConstants.java | 1 + .../robot/constants/ShootingConstants.java | 1 + .../robot/subsystems/ledstrip/LEDStrip.java | 18 +++++------------- .../subsystems/ledstrip/LEDStripConstants.java | 4 ++-- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 414ca9fb..07f80d41 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,6 +6,7 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; @@ -23,6 +24,7 @@ import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; +import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.pitcher.Pitcher; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.Shooter; @@ -83,6 +85,7 @@ private void bindControllerCommands() { OperatorConstants.ALIGN_TO_MIDDLE_STAGE.whileTrue(CommandConstants.ALIGN_TO_MIDDLE_STAGE_COMMAND); OperatorConstants.EJECT_NOTE_TRIGGER.whileTrue(CommandConstants.EJECT_COMMAND); + OperatorConstants.HIGH_EJECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getHighEjectNoteCommand()); OperatorConstants.COLLECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getNoteCollectionCommand()); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index c05d6ada..bb783b80 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -47,6 +47,13 @@ public static Command getNoteCollectionCommand() { ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); } + public static Command getHighEjectNoteCommand() { + return new ParallelCommandGroup( + PitcherCommands.getSetTargetPitchCommand(ShootingConstants.HIGH_EJECT_PITCH), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.EJECT) + ); + } + public static Command getVisualizeNoteShootingCommand() { return new InstantCommand( () -> new VisualizeNoteShootingCommand() diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 64d324e8..22ee7712 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -20,7 +20,7 @@ public class CameraConstants { ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), - new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-32.5), Math.PI + Units.degreesToRadians(0)) + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33.15), Math.PI + Units.degreesToRadians(0)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index b5a54065..82cdc001 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -42,6 +42,7 @@ public class OperatorConstants { BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), COLLECT_NOTE_TRIGGER = DRIVER_CONTROLLER.leftTrigger(), EJECT_NOTE_TRIGGER = OPERATOR_CONTROLLER.e(), + HIGH_EJECT_NOTE_TRIGGER = OPERATOR_CONTROLLER.l(), SPEAKER_SHOT_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.s()), CLOSE_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.x(), WARM_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.w().and(SPEAKER_SHOT_TRIGGER.negate()), diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index c79580a8..d735d3c6 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -22,6 +22,7 @@ public class ShootingConstants { CLOSE_SHOT_PITCH = Rotation2d.fromDegrees(57), PREPARE_AMP_PITCH = Rotation2d.fromDegrees(50), SHOOT_AMP_PITCH = Rotation2d.fromDegrees(44), + HIGH_EJECT_PITCH = Rotation2d.fromDegrees(45), MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(13), EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13), CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 11d70fc1..ddbada79 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -82,12 +82,12 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { + public void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { final int ledsPerSection = (int) Math.floor(numberOfLEDs / 3.0); setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); } - public void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { + private void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { setLEDColors(inverted ? thirdSectionColor : firstSectionColor, 0, ledsPerSection); setLEDColors(secondSectionColor, ledsPerSection, ledsPerSection * 2); setLEDColors(inverted ? firstSectionColor : thirdSectionColor, ledsPerSection * 2, numberOfLEDs - 1); @@ -99,18 +99,10 @@ public void setLEDColor(Color color, int index) { } public void setLEDColors(Color color, int startIndex, int endIndex) { - final AddressableLEDBuffer buffer = new AddressableLEDBuffer(LEDStripConstants.LED_BUFFER.getLength()); - for (int i = 0; i < buffer.getLength(); i++) { - if (i > startIndex + indexOffset && i < endIndex + indexOffset) { - buffer.setLED(i, color); - continue; - } - buffer.setLED(i, LEDStripConstants.LED_BUFFER.getLED(i)); + for (int i = 0; i < endIndex - startIndex + 1; i++) { + LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); } - LED.setData(buffer); -// for (int i = 0; i < endIndex - startIndex; i++) { -// buffer.setLED(startIndex + indexOffset + i, color); -// } + LED.setData(LEDStripConstants.LED_BUFFER); } private void addLEDStripToLEDStripsArray(LEDStrip ledStrip) { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index faff71a2..899de5d7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -24,8 +24,8 @@ public class LEDStripConstants { static final double LOW_BATTERY_BLINKING_TIME_SECONDS = 5; public static final LEDStrip - RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS - 1, LEFT_CLIMBER_NUMBER_OF_LEDS - 1), - LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, 0); + RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, 0), + LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, RIGHT_CLIMBER_NUMBER_OF_LEDS - 1); static { LED.setLength(RIGHT_CLIMBER_NUMBER_OF_LEDS + LEFT_CLIMBER_NUMBER_OF_LEDS); From 19cbdbeb986e9c5851d937905d892ff1a3818335 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Tue, 15 Oct 2024 16:28:22 +0300 Subject: [PATCH 146/261] auto and auto align stuf --- .pathplanner/settings.json | 4 +-- .../autos/3NoteAutoThroughAmp.auto | 4 +-- .../autos/3NoteAutoThroughAmpLeft.auto | 12 +++---- .../pathplanner/autos/FrontRow4NoteAuto.auto | 18 ++++++++++ .../paths/3NoteAutoThroughAmp1.path | 14 ++++---- .../paths/3NoteAutoThroughAmp2.path | 4 +-- .../paths/3NoteAutoThroughAmp3.path | 4 +-- .../paths/3NoteAutoThroughAmp4.path | 4 +-- .../paths/3NoteAutoThroughAmp5.path | 4 +-- .../paths/3NoteAutoThroughAmp6.path | 4 +-- .../paths/3NoteAutoThroughAmpLeft3.path | 34 ++++++++----------- .../paths/3NoteAutoThroughAmpLeft4.path | 4 +-- .../paths/3NoteAutoThroughAmpLeft5.path | 4 +-- .../paths/5NoteAutoAroundStage1.path | 4 +-- .../paths/5NoteAutoAroundStage2.path | 4 +-- .../paths/5NoteAutoAroundStage3.path | 4 +-- .../paths/5NoteAutoAroundStage4.path | 4 +-- .../paths/5NoteAutoAroundStage5.path | 4 +-- .../paths/5NoteAutoAroundStage6.path | 4 +-- .../paths/5NoteAutoAroundStage7.path | 4 +-- .../paths/5NoteAutoAroundStage8.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto1.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto10.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto2.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto3.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto4.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto5.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto6.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto7.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto8.path | 4 +-- .../deploy/pathplanner/paths/7NoteAuto9.path | 4 +-- .../pathplanner/paths/FrontRow4NoteAuto1.path | 4 +-- .../pathplanner/paths/FrontRow4NoteAuto2.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto3.path | 4 +-- .../pathplanner/paths/FrontRow4NoteAuto4.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto5.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto6.path | 2 +- .../paths/MiddleRush3NoteAuto1.path | 4 +-- .../paths/MiddleRush3NoteAuto2.path | 4 +-- .../paths/MiddleRush3NoteAuto3.path | 4 +-- .../paths/MiddleRush3NoteAuto4.path | 4 +-- .../paths/MiddleRush3NoteAuto5.path | 4 +-- .../paths/MiddleRush3NoteAuto6.path | 4 +-- .../paths/MiddleRush3NoteAuto7.path | 4 +-- .../paths/MiddleRush3NoteAuto8.path | 4 +-- .../paths/MiddleRush3NoteAuto9.path | 4 +-- .../deploy/pathplanner/paths/PIDAuto0.path | 4 +-- .../deploy/pathplanner/paths/PIDAuto1.path | 4 +-- .../paths/SourceSide3NoteAuto1.path | 4 +-- .../paths/SourceSide3NoteAuto2.path | 4 +-- .../paths/SourceSide3NoteAuto3.path | 4 +-- .../paths/SourceSide3NoteAuto4.path | 4 +-- .../paths/SourceSide3NoteAuto5.path | 4 +-- .../paths/SourceSide3NoteAutoRight2.path | 4 +-- .../paths/SourceSide3NoteAutoRight3.path | 4 +-- .../paths/SourceSide3NoteAutoRight4.path | 4 +-- .../paths/SourceSide3NoteAutoRight5.path | 4 +-- .../paths/SpeakerSide3NoteAuto1.path | 4 +-- .../paths/SpeakerSide3NoteAuto2.path | 4 +-- .../paths/SpeakerSide3NoteAuto3.path | 4 +-- .../paths/SpeakerSide3NoteAuto4.path | 4 +-- .../paths/SpeakerSide3NoteAuto5.path | 4 +-- .../paths/SpeakerSide3NoteAuto6.path | 4 +-- .../paths/SpeakerSide3NoteAuto7.path | 4 +-- .../pathplanner/paths/Zayde3NoteAuto1.path | 4 +-- .../pathplanner/paths/Zayde3NoteAuto2.path | 4 +-- .../pathplanner/paths/Zayde3NoteAuto3.path | 4 +-- .../pathplanner/paths/Zayde3NoteAuto4.path | 4 +-- .../pathplanner/paths/Zayde3NoteAuto5.path | 4 +-- .../pathplanner/paths/Zayde3NoteAuto6.path | 4 +-- .../pathplanner/paths/Zayde3NoteAuto7.path | 4 +-- .../robot/commands/AlignToNoteCommand.java | 12 +++++-- .../factories/AutonomousCommands.java | 2 +- .../subsystems/intake/IntakeConstants.java | 1 + .../subsystems/swerve/SwerveConstants.java | 2 +- 75 files changed, 187 insertions(+), 168 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 26719f91..cfa85e7b 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -23,8 +23,8 @@ "OtherAutos", "SourceSide3NoteAutoVarients" ], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 1.0, + "defaultMaxAccel": 2.0, "defaultMaxAngVel": 400.0, "defaultMaxAngAccel": 600.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index e4cf24f2..2eee8b64 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7531929553170285, - "y": 6.932078092184688 + "x": 0.6849446477990359, + "y": 6.688334136763285 }, "rotation": 60.1093232393867 }, diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index e4cf24f2..970f0680 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7531929553170285, - "y": 6.932078092184688 + "x": 0.6849446477990359, + "y": 6.688334136763285 }, "rotation": 60.1093232393867 }, @@ -183,7 +183,7 @@ { "type": "path", "data": { - "pathName": "3NoteAutoThroughAmp3" + "pathName": "3NoteAutoThroughAmpLeft3" } }, { @@ -228,7 +228,7 @@ { "type": "path", "data": { - "pathName": "3NoteAutoThroughAmp4" + "pathName": "3NoteAutoThroughAmpLeft4" } }, { @@ -279,7 +279,7 @@ { "type": "path", "data": { - "pathName": "3NoteAutoThroughAmp5" + "pathName": "3NoteAutoThroughAmpLeft5" } }, { @@ -324,7 +324,7 @@ { "type": "path", "data": { - "pathName": "3NoteAutoThroughAmp6" + "pathName": "3NoteAutoThroughAmp4" } }, { diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index 7d7eafda..fcf866ff 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -146,6 +146,12 @@ "data": { "name": "FeedNote" } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } } ] } @@ -242,6 +248,12 @@ "data": { "name": "FeedNote" } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } } ] } @@ -338,6 +350,12 @@ "data": { "name": "FeedNote" } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } } ] } diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path index fcf428b6..4ccf0cbd 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.45095045059448935, - "y": 7.0490751907869615 + "x": 0.6849446477990359, + "y": 6.688334136763285 }, "prevControl": null, "nextControl": { - "x": 1.4509504505944886, - "y": 7.0490751907869615 + "x": 1.6849446477990346, + "y": 6.688334136763285 }, "isLocked": false, "linkedName": null @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, @@ -45,7 +45,7 @@ "reversed": false, "folder": "3NoteAutoThroughAmp", "previewStartingState": { - "rotation": -1.3639275316029946, + "rotation": 59.74356283647073, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index 4ba2ea85..94f3479e 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index fecbc71b..e1efac06 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -73,8 +73,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path index 9737a241..852c566f 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index dcf5783b..a54a2de4 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path index 1e67aaf7..9b071f33 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index fecbc71b..48e0411a 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -8,38 +8,32 @@ }, "prevControl": null, "nextControl": { - "x": 5.837872416485573, - "y": 6.829386194384925 + "x": 5.793817953431632, + "y": 7.107573740088099 }, "isLocked": false, "linkedName": "3NoteAutoThroughAmpFirstCollection" }, { "anchor": { - "x": 8.270256540513081, - "y": 5.771473152964195 + "x": 8.255803553860629, + "y": 7.443288126012872 }, "prevControl": { - "x": 7.708937127275964, - "y": 6.080960310068768 + "x": 7.85101693718827, + "y": 7.380566970160069 }, "nextControl": null, "isLocked": false, - "linkedName": "3NoteAutoThroughAmpSecondCollection" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.45, - "rotationDegrees": -20.0, - "rotateFast": false + "linkedName": "3NoteAutoThroughAmpLeftSecondCollection" } ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [ { "name": "AlignToNOte", - "waypointRelativePos": 0.9, + "waypointRelativePos": 0.8, "command": { "type": "parallel", "data": { @@ -73,20 +67,20 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, "goalEndState": { "velocity": 0, - "rotation": -20.799028408841473, + "rotation": 7.643378071479572, "rotateFast": false }, "reversed": false, - "folder": "3NoteAutoThroughAmp", + "folder": "3NoteAutoThroughAmpLeft", "previewStartingState": { - "rotation": 25.769327624338693, + "rotation": 28.370000000000005, "velocity": 1.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path index 911bdc47..a5b0b09e 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index 58ddedea..e606c426 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index 457717aa..52d26200 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -83,8 +83,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path index 141da755..e767ad0f 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index 3ebd7fe6..d145d7b4 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path index e00f004b..da2da89c 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index b3f969e2..0364112f 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path index e8566d30..4c8711e6 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index 22643e3f..3cfef17d 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path index ce748388..6b7e8345 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto1.path b/src/main/deploy/pathplanner/paths/7NoteAuto1.path index 6389f141..86f97df5 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto1.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto10.path b/src/main/deploy/pathplanner/paths/7NoteAuto10.path index 30ec2a05..ff273dc0 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto10.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto10.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto2.path b/src/main/deploy/pathplanner/paths/7NoteAuto2.path index 18bb36b9..d8fdb0b5 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto3.path b/src/main/deploy/pathplanner/paths/7NoteAuto3.path index a94671c2..38278595 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto4.path b/src/main/deploy/pathplanner/paths/7NoteAuto4.path index b641481e..e71f1c0e 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto4.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index 61951a44..9a570e5d 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto6.path b/src/main/deploy/pathplanner/paths/7NoteAuto6.path index 3c318de5..b318e94f 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto6.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index e527b7a5..2b094baa 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto8.path b/src/main/deploy/pathplanner/paths/7NoteAuto8.path index ea8fb762..8f5b9e9a 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto8.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index 2a67a99f..ae292d15 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index 937806d5..c7539083 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, "goalEndState": { "velocity": 0, - "rotation": 15.296223645005243, + "rotation": 29.139259553040947, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index a91958bf..24bcb8f2 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index 9c590f32..e27190d1 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -9.652155522293434, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index d62b5f83..ca892025 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index e2a567ca..35a620d7 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index 4bae27ab..fd058163 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 1.5, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path index 0a125b75..637850ba 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index 7d443fc2..5e5ccf0b 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path index a4d1a0a3..6b610c68 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index b9262a9f..9806879a 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path index c7034b58..65e5857c 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 616150e5..423b05c1 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path index 2d9b651d..6687179a 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index 97ab6038..11b1bf1d 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path index 1bab242d..ef2a2bf0 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path index e1f94562..a3d42343 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path index ca9dd5a1..5f6e0df8 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto1.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 463907fb..6a83faea 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path index 0decd1a1..64db4624 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path index 076e4ab2..bed0e747 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path index 7bceffea..a38a4a90 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path index 1b06792a..c094b6cd 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index e52b1029..4b7bd4ea 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 7726f0bb..68e19968 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index dbad861e..3ed98bfe 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 40b9436a..5aaa07df 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path index bc6d054b..ce49abb5 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index 78bc74b1..c14dbdcc 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path index 089456c3..221f7f67 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index 19e9f882..81849e43 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path index e715e7d6..c6350208 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index b899f37d..bfc9c11d 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path index cc0ad275..cb1da68f 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path index 26f46f77..f1b07914 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index 93b8dc39..3abdb3c2 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path index 9a04a02c..feeceb32 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index 02ae8c58..3f7707c2 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -62,8 +62,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path index 45baa73b..16dd173b 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index 5df399c5..8c3ace25 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path index c442bf8d..f2ff78ee 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 63021880..a9a59ad8 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -11,6 +12,7 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -23,11 +25,12 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : new PIDController(0.0002, 0, 0); + private double lastTimeCameraHadObjectsSeconds = 0; public AlignToNoteCommand() { addCommands( getSetCurrentLEDColorCommand().asProxy(), - GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), + GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::shouldAlignToNote).asProxy(), new RunCommand(CAMERA::trackObject) ); } @@ -53,8 +56,11 @@ private MirrorableRotation2d getTargetAngle() { return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); } - private boolean hasTarget() { - return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); + private boolean shouldAlignToNote() { + lastTimeCameraHadObjectsSeconds = CAMERA.hasTargets() ? Timer.getFPGATimestamp() : lastTimeCameraHadObjectsSeconds; + if (CAMERA.hasTargets() || Timer.getFPGATimestamp() - lastTimeCameraHadObjectsSeconds < IntakeConstants.EXTRA_NOTE_ALIGNMENT_TIME_SECONDS) + return !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); + return false; } private double getScaledJoystickValue() { diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index f97f7907..2134c8ff 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -54,7 +54,7 @@ public static Command getFeedNoteCommand() { return new ParallelCommandGroup( IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_SHOOTING), GeneralCommands.getVisualizeNoteShootingCommand() - ).until(() -> !RobotContainer.INTAKE.hasNote()); + ).withTimeout(0.5); } public static Command getAlignToNoteCommand() { diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index c786a8a4..10bea0b1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -59,6 +59,7 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; + public static final double EXTRA_NOTE_ALIGNMENT_TIME_SECONDS = 1; static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 71ed1759..4ea026a6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -80,7 +80,7 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0.1) : - new PIDConstants(6.5, 0, 0), + new PIDConstants(5, 0, 0), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(2.5, 0, 0.2) : new PIDConstants(3, 0, 0); From 7058b3beac5bf9f3885c682e3eff2960dd225d26 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Tue, 15 Oct 2024 17:52:38 +0300 Subject: [PATCH 147/261] auto, alignment, and climb --- .../autos/3NoteAutoThroughAmp.auto | 6 ------ .../paths/3NoteAutoThroughAmp2.path | 20 +------------------ .../paths/3NoteAutoThroughAmp3.path | 17 ---------------- .../paths/3NoteAutoThroughAmpLeft3.path | 17 ---------------- .../robot/commands/AlignToNoteCommand.java | 7 +------ .../robot/constants/ShootingConstants.java | 2 +- .../subsystems/climber/ClimberConstants.java | 4 ++-- .../subsystems/intake/IntakeConstants.java | 1 - 8 files changed, 5 insertions(+), 69 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index 2eee8b64..467360b6 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -33,12 +33,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.05 - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index 94f3479e..ddf561b5 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "AlignToNote", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AlignToNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index e1efac06..28ed9e5b 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -53,23 +53,6 @@ ] } } - }, - { - "name": "StopALigningToFIrstNote", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index 48e0411a..08eb674b 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -47,23 +47,6 @@ ] } } - }, - { - "name": "StopALigningToFIrstNote", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAligningToNote" - } - } - ] - } - } } ], "globalConstraints": { diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index a9a59ad8..48d821d4 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -12,7 +11,6 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -57,10 +55,7 @@ private MirrorableRotation2d getTargetAngle() { } private boolean shouldAlignToNote() { - lastTimeCameraHadObjectsSeconds = CAMERA.hasTargets() ? Timer.getFPGATimestamp() : lastTimeCameraHadObjectsSeconds; - if (CAMERA.hasTargets() || Timer.getFPGATimestamp() - lastTimeCameraHadObjectsSeconds < IntakeConstants.EXTRA_NOTE_ALIGNMENT_TIME_SECONDS) - return !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); - return false; + return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } private double getScaledJoystickValue() { diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index d735d3c6..800586de 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -14,7 +14,7 @@ public class ShootingConstants { public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, - AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 40, + AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 37, MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 993fcd09..7595a5f3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -29,8 +29,8 @@ public class ClimberConstants { LEFT_MOTOR = new TalonFXMotor(LEFT_MOTOR_ID, LEFT_MOTOR_NAME); private static final InvertedValue - RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive, - LEFT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; + RIGHT_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive, + LEFT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; static final boolean ENABLE_FOC = true; private static final double //TODO: calibrate diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 10bea0b1..c786a8a4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -59,7 +59,6 @@ public class IntakeConstants { public static final double RUMBLE_DURATION_SECONDS = 0.6; public static final double RUMBLE_POWER = 1; - public static final double EXTRA_NOTE_ALIGNMENT_TIME_SECONDS = 1; static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), From 319d7b608b1b01de17d1f15e7bf6be5eea955677 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 18:16:09 +0300 Subject: [PATCH 148/261] =?UTF-8?q?`=C2=AF\=5F(=E3=83=84)=5F/=C2=AF`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 3 +-- .../trigon/robot/subsystems/ledstrip/LEDStripConstants.java | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index ddbada79..f7b3dc62 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -1,7 +1,6 @@ package frc.trigon.robot.subsystems.ledstrip; import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -99,7 +98,7 @@ public void setLEDColor(Color color, int index) { } public void setLEDColors(Color color, int startIndex, int endIndex) { - for (int i = 0; i < endIndex - startIndex + 1; i++) { + for (int i = 0; i <= endIndex - startIndex; i++) { LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); } LED.setData(LEDStripConstants.LED_BUFFER); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index 899de5d7..dbd2e301 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -25,7 +25,7 @@ public class LEDStripConstants { public static final LEDStrip RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, 0), - LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, RIGHT_CLIMBER_NUMBER_OF_LEDS - 1); + LEFT_CLIMBER_LEDS = new LEDStrip(LEFT_CLIMBER_INVERTED, LEFT_CLIMBER_NUMBER_OF_LEDS, RIGHT_CLIMBER_NUMBER_OF_LEDS); static { LED.setLength(RIGHT_CLIMBER_NUMBER_OF_LEDS + LEFT_CLIMBER_NUMBER_OF_LEDS); From 86f8f4844ff8d71ffc763a59f637955490344dab Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 18:16:54 +0300 Subject: [PATCH 149/261] Oops --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 07f80d41..e3a55e9a 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -106,6 +106,8 @@ private void bindControllerCommands() { OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); + LEDStrip.LED_STRIPS[0].threeSectionColor(Color.kDeepPink, Color.kHotPink, Color.kLightPink); + LEDStrip.LED_STRIPS[1].threeSectionColor(Color.kMediumPurple, Color.kPurple, Color.kPink); } private void configureSysIdBindings(MotorSubsystem subsystem) { From 6c0341532578815defdea6a6209f2906ce855354 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Tue, 15 Oct 2024 20:38:03 +0300 Subject: [PATCH 150/261] changed climber gains --- .../frc/trigon/robot/subsystems/climber/Climber.java | 8 +++++--- .../robot/subsystems/climber/ClimberConstants.java | 11 ++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 9c6e2bc1..0edf7ece 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -127,9 +127,11 @@ void setTargetState(ClimberConstants.ClimberState targetState) { void setTargetPosition(double targetRightPositionRotations, double targetLeftPositionRotations, boolean affectedByRobotWeight) { rightMotor.setControl(determineRequest(affectedByRobotWeight) - .withPosition(targetRightPositionRotations)); - leftMotor.setControl(determineRequest(affectedByRobotWeight) - .withPosition(targetLeftPositionRotations)); + .withPosition(targetRightPositionRotations) + .withFeedForward(affectedByRobotWeight ? ClimberConstants.ON_CHAIN_KG : 0)); + leftMotor.setControl(determineRequest(affectedByRobotWeight).withFeedForward(ClimberConstants.ON_CHAIN_KG) + .withPosition(targetLeftPositionRotations) + .withFeedForward(affectedByRobotWeight ? ClimberConstants.ON_CHAIN_KG : 0)); } private DynamicMotionMagicVoltage determineRequest(boolean affectedByRobotWeight) { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 7595a5f3..35f53c4e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -34,29 +34,30 @@ public class ClimberConstants { private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; static final boolean ENABLE_FOC = true; private static final double //TODO: calibrate - LEFT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 1, + LEFT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 50, LEFT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.078964, LEFT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9056, LEFT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.18439; private static final double //TODO: calibrate - RIGHT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 4.5626, + RIGHT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 50, RIGHT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, RIGHT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, RIGHT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.079947, RIGHT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9986, RIGHT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.21705; private static final double //TODO: calibrate - ON_CHAIN_P = RobotHardwareStats.isSimulation() ? RIGHT_GROUNDED_P : 4.5626, + ON_CHAIN_P = RobotHardwareStats.isSimulation() ? RIGHT_GROUNDED_P : 1, ON_CHAIN_I = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_I : 0, ON_CHAIN_D = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_D : 0, ON_CHAIN_KS = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KS : 0.079947, ON_CHAIN_KV = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KV : 7.9986, ON_CHAIN_KA = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_KA : 0.21705; + static final double ON_CHAIN_KG = 0.4; static final double - MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KV : 1, - MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KA : 1, + MAX_GROUNDED_VELOCITY = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KV : 1.5, + MAX_GROUNDED_ACCELERATION = RobotHardwareStats.isSimulation() ? 12 / LEFT_GROUNDED_KA : 1.5, MAX_ON_CHAIN_VELOCITY = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KV) - 0.75 : 1, MAX_ON_CHAIN_ACCELERATION = RobotHardwareStats.isSimulation() ? (12 / ON_CHAIN_KA) - 50 : 1; static final int From 07af26e61c0859301ff28b0cb6889114f8fc5c53 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 21:25:05 +0300 Subject: [PATCH 151/261] Small fix :) --- .../java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index f7b3dc62..4a52d5af 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -67,16 +67,16 @@ void blink(Color color, double blinkingIntervalSeconds) { } void staticColor(Color color) { - for (int index = 0; index <= numberOfLEDs; index++) + for (int index = 0; index < numberOfLEDs; index++) setLEDColor(color, index); } void rainbow() { - for (int led = 0; led <= numberOfLEDs; led++) { + for (int led = 0; led < numberOfLEDs; led++) { final int hue = (int) (rainbowFirstPixelHue + (led * 180 / numberOfLEDs) % 180); LEDStripConstants.LED_BUFFER.setHSV(led + indexOffset, hue, 255, 128); - LED.setData(LEDStripConstants.LED_BUFFER); } + LED.setData(LEDStripConstants.LED_BUFFER); rainbowFirstPixelHue += 3; rainbowFirstPixelHue %= 180; } From fef3b01e0d7b34ef12f59ef2037cbd92707cda65 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 21:26:05 +0300 Subject: [PATCH 152/261] Why are these even here? --- src/main/java/frc/trigon/robot/RobotContainer.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index e3a55e9a..eef3ba28 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -16,17 +16,13 @@ import frc.trigon.robot.commands.factories.ShootingCommands; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ampaligner.AmpAligner; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; -import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; -import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.pitcher.Pitcher; -import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; From c003c82aedf28ed2807317ffad0f062f9158e72f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 22:01:15 +0300 Subject: [PATCH 153/261] Probably fixed LEDs --- .../java/frc/trigon/robot/RobotContainer.java | 3 --- .../robot/commands/AlignToNoteCommand.java | 1 - .../robot/commands/CommandConstants.java | 2 +- .../commands/factories/GeneralCommands.java | 6 +++--- .../robot/subsystems/intake/Intake.java | 5 ++--- .../robot/subsystems/ledstrip/LEDStrip.java | 20 +++++++------------ 6 files changed, 13 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index eef3ba28..129d63b6 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,7 +6,6 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; @@ -102,8 +101,6 @@ private void bindControllerCommands() { OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); - LEDStrip.LED_STRIPS[0].threeSectionColor(Color.kDeepPink, Color.kHotPink, Color.kLightPink); - LEDStrip.LED_STRIPS[1].threeSectionColor(Color.kMediumPurple, Color.kPurple, Color.kPink); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 48d821d4..c70da222 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -23,7 +23,6 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : new PIDController(0.0002, 0, 0); - private double lastTimeCameraHadObjectsSeconds = 0; public AlignToNoteCommand() { addCommands( diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 0cafd238..3859cc90 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -85,7 +85,7 @@ public class CommandConstants { SHOULD_ALIGN_TO_NOTE = false; Logger.recordOutput("ShouldAlignToNote", false); }).ignoringDisable(true), - DEFAULT_LEDS_COMMAND = LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), + DEFAULT_LEDS_COMMAND = LEDStripCommands.getStaticColorCommand(Color.kBlue, LEDStrip.LED_STRIPS), DEFAULT_INTAKE_COMMAND = IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOP), DEFAULT_CLIMBER_COMMAND = ClimberCommands.getStopCommand(), MOVE_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index bb783b80..1d619a34 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -41,7 +41,7 @@ public static Command getClimbCommand() { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), - LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); @@ -99,9 +99,9 @@ public static Command getToggleBrakeCommand() { MotorSubsystem.setAllSubsystemsBrakeAsync(IS_BRAKING); if (IS_BRAKING) - CommandConstants.STATIC_WHITE_LED_COLOR_COMMAND.cancel(); - else CommandConstants.STATIC_WHITE_LED_COLOR_COMMAND.schedule(); + else + CommandConstants.STATIC_WHITE_LED_COLOR_COMMAND.cancel(); }).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index caafd080..7bbaff03 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -92,11 +91,11 @@ void indicateCollection() { } private Command getCollectionIndicationLEDsCommand() { - return LEDStripCommands.getBlinkingCommand(Color.kWhite, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); + return LEDStripCommands.getBlinkingCommand(Color.kOrangeRed, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); } private Command getFeedingIndicationLEDsCommand() { - return LEDStripCommands.getBlinkingCommand(Color.kYellow, IntakeConstants.FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BLINKING_TIME_SECONDS); + return LEDStripCommands.getBlinkingCommand(Color.kPurple, IntakeConstants.FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BLINKING_TIME_SECONDS); } private void configureLEDsTrigger() { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 4a52d5af..180ee303 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -1,6 +1,5 @@ package frc.trigon.robot.subsystems.ledstrip; -import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -9,7 +8,6 @@ public class LEDStrip extends SubsystemBase { public static LEDStrip[] LED_STRIPS = new LEDStrip[0]; - private static final AddressableLED LED = LEDStripConstants.LED; private final int indexOffset; private final boolean inverted; private final int numberOfLEDs; @@ -24,6 +22,11 @@ public class LEDStrip extends SubsystemBase { ); } + @Override + public void periodic() { + LEDStripConstants.LED.setData(LEDStripConstants.LED_BUFFER); + } + public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { this.indexOffset = indexOffset; this.inverted = inverted; @@ -67,8 +70,7 @@ void blink(Color color, double blinkingIntervalSeconds) { } void staticColor(Color color) { - for (int index = 0; index < numberOfLEDs; index++) - setLEDColor(color, index); + setLEDColors(color, 0, numberOfLEDs - 1); } void rainbow() { @@ -76,7 +78,6 @@ void rainbow() { final int hue = (int) (rainbowFirstPixelHue + (led * 180 / numberOfLEDs) % 180); LEDStripConstants.LED_BUFFER.setHSV(led + indexOffset, hue, 255, 128); } - LED.setData(LEDStripConstants.LED_BUFFER); rainbowFirstPixelHue += 3; rainbowFirstPixelHue %= 180; } @@ -92,16 +93,9 @@ private void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, C setLEDColors(inverted ? firstSectionColor : thirdSectionColor, ledsPerSection * 2, numberOfLEDs - 1); } - public void setLEDColor(Color color, int index) { - LEDStripConstants.LED_BUFFER.setLED(index + indexOffset, color); - LED.setData(LEDStripConstants.LED_BUFFER); - } - public void setLEDColors(Color color, int startIndex, int endIndex) { - for (int i = 0; i <= endIndex - startIndex; i++) { + for (int i = 0; i <= endIndex - startIndex; i++) LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); - } - LED.setData(LEDStripConstants.LED_BUFFER); } private void addLEDStripToLEDStripsArray(LEDStrip ledStrip) { From 4698459482ce9c07dfbb65d7b0c899d269407f02 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 22:25:57 +0300 Subject: [PATCH 154/261] Small fixes (more coming) --- .../frc/trigon/robot/commands/factories/GeneralCommands.java | 4 ++-- .../java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 1d619a34..be9e60f5 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -99,9 +99,9 @@ public static Command getToggleBrakeCommand() { MotorSubsystem.setAllSubsystemsBrakeAsync(IS_BRAKING); if (IS_BRAKING) - CommandConstants.STATIC_WHITE_LED_COLOR_COMMAND.schedule(); - else CommandConstants.STATIC_WHITE_LED_COLOR_COMMAND.cancel(); + else + CommandConstants.STATIC_WHITE_LED_COLOR_COMMAND.schedule(); }).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 180ee303..0f132e85 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -82,7 +82,7 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - public void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { + void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { final int ledsPerSection = (int) Math.floor(numberOfLEDs / 3.0); setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); } @@ -93,7 +93,7 @@ private void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, C setLEDColors(inverted ? firstSectionColor : thirdSectionColor, ledsPerSection * 2, numberOfLEDs - 1); } - public void setLEDColors(Color color, int startIndex, int endIndex) { + private void setLEDColors(Color color, int startIndex, int endIndex) { for (int i = 0; i <= endIndex - startIndex; i++) LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); } From 8c8c6045d53f3fa6e1170b3cc82a2321bf227272 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 22:27:57 +0300 Subject: [PATCH 155/261] Yay (LEDs working) --- .../java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 0f132e85..2076d8aa 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -1,5 +1,6 @@ package frc.trigon.robot.subsystems.ledstrip; +import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -8,6 +9,7 @@ public class LEDStrip extends SubsystemBase { public static LEDStrip[] LED_STRIPS = new LEDStrip[0]; + private static final AddressableLED LED = LEDStripConstants.LED; private final int indexOffset; private final boolean inverted; private final int numberOfLEDs; @@ -24,7 +26,7 @@ public class LEDStrip extends SubsystemBase { @Override public void periodic() { - LEDStripConstants.LED.setData(LEDStripConstants.LED_BUFFER); + LED.setData(LEDStripConstants.LED_BUFFER); } public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { From 2bd437a166c08c5a30ddee2c8a6d65c205074032 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 22:31:47 +0300 Subject: [PATCH 156/261] Final quick thingy for LEDs --- src/main/java/frc/trigon/robot/commands/CommandConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 3859cc90..0cafd238 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -85,7 +85,7 @@ public class CommandConstants { SHOULD_ALIGN_TO_NOTE = false; Logger.recordOutput("ShouldAlignToNote", false); }).ignoringDisable(true), - DEFAULT_LEDS_COMMAND = LEDStripCommands.getStaticColorCommand(Color.kBlue, LEDStrip.LED_STRIPS), + DEFAULT_LEDS_COMMAND = LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), DEFAULT_INTAKE_COMMAND = IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOP), DEFAULT_CLIMBER_COMMAND = ClimberCommands.getStopCommand(), MOVE_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), From 764960839da570a8b2d23ff98f060db4df496af8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 23:25:39 +0300 Subject: [PATCH 157/261] I hope this doesn't break stuff (: --- .../robot/subsystems/intake/Intake.java | 2 +- .../subsystems/intake/IntakeConstants.java | 5 ++--- .../robot/subsystems/ledstrip/LEDStrip.java | 21 +++++++++++++++++++ .../subsystems/ledstrip/LEDStripCommands.java | 7 +++++++ 4 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 7bbaff03..22b78741 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -95,7 +95,7 @@ private Command getCollectionIndicationLEDsCommand() { } private Command getFeedingIndicationLEDsCommand() { - return LEDStripCommands.getBlinkingCommand(Color.kPurple, IntakeConstants.FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BLINKING_TIME_SECONDS); + return LEDStripCommands.getBreathingCommand(Color.kPurple, 5, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS); } private void configureLEDsTrigger() { diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index c786a8a4..35a97076 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -74,9 +74,8 @@ public class IntakeConstants { static final double COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2, FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; - static final double - COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2, - FEEDING_INDICATION_BLINKING_TIME_SECONDS = 1; + static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; + public static final double FEEDING_INDICATION_BREATHING_TIME_SECONDS = 1; static { configureMasterMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 2076d8aa..07363560 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.commands.factories.GeneralCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; public class LEDStrip extends SubsystemBase { public static LEDStrip[] LED_STRIPS = new LEDStrip[0]; @@ -13,8 +14,10 @@ public class LEDStrip extends SubsystemBase { private final int indexOffset; private final boolean inverted; private final int numberOfLEDs; + private int lastBreatheLED; private double rainbowFirstPixelHue = 0; private boolean areLEDsOnForBlinking = false; + private double lastBreatheMovementTime = 0; private double lastBlinkTime = 0; static { @@ -34,6 +37,7 @@ public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { this.inverted = inverted; this.numberOfLEDs = numberOfLEDs; + lastBreatheLED = indexOffset; addLEDStripToLEDStripsArray(this); } @@ -84,6 +88,23 @@ void rainbow() { rainbowFirstPixelHue %= 180; } + void breathe(Color color, int breathingLEDs) { + double moveLEDTimeSeconds = IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS / numberOfLEDs; + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { + lastBreatheMovementTime = currentTime; + lastBreatheLED++; + } + if (lastBreatheLED >= numberOfLEDs + indexOffset) + lastBreatheLED = indexOffset; + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED, color); + if (lastBreatheLED - indexOffset < breathingLEDs) + LEDStripConstants.LED_BUFFER.setLED(numberOfLEDs + indexOffset - breathingLEDs + lastBreatheLED - indexOffset, Color.kBlack); + else + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - breathingLEDs, Color.kBlack); + + } + void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { final int ledsPerSection = (int) Math.floor(numberOfLEDs / 3.0); setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 93f08d9d..c8866fb3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -35,6 +35,13 @@ public static Command getRainbowCommand(LEDStrip... ledStrips) { ).ignoringDisable(true); } + public static Command getBreathingCommand(Color color, int breathingLEDs, LEDStrip... ledStrips) { + return new RunCommand( + () -> runForLEDs((ledStrip) -> ledStrip.breathe(color, breathingLEDs), ledStrips), + ledStrips + ).ignoringDisable(true); + } + public static Command getThreeSectionColorCommand(Supplier firstSectionColor, Supplier secondSectionColor, Supplier thirdSectionColor, LEDStrip... ledStrips) { return new InitExecuteCommand( () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), From db43f54cd46557aab8430461658f82c53abe1e18 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 15 Oct 2024 23:57:51 +0300 Subject: [PATCH 158/261] Fixed command --- .../robot/subsystems/intake/IntakeConstants.java | 3 +-- .../trigon/robot/subsystems/ledstrip/LEDStrip.java | 13 +++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 35a97076..345de2c9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -72,8 +72,7 @@ public class IntakeConstants { () -> Math.abs(MASTER_MOTOR.getSignal(TalonFXSignal.TORQUE_CURRENT)) > IntakeConstants.NOTE_COLLECTION_CURRENT ).debounce(NOTE_COLLECTION_TIME_THRESHOLD_SECONDS); static final double - COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2, - FEEDING_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; + COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; public static final double FEEDING_INDICATION_BREATHING_TIME_SECONDS = 1; diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 07363560..93ece311 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -89,6 +89,7 @@ void rainbow() { } void breathe(Color color, int breathingLEDs) { + clearLedColors(); double moveLEDTimeSeconds = IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS / numberOfLEDs; double currentTime = Timer.getFPGATimestamp(); if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { @@ -97,12 +98,12 @@ void breathe(Color color, int breathingLEDs) { } if (lastBreatheLED >= numberOfLEDs + indexOffset) lastBreatheLED = indexOffset; - LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED, color); - if (lastBreatheLED - indexOffset < breathingLEDs) - LEDStripConstants.LED_BUFFER.setLED(numberOfLEDs + indexOffset - breathingLEDs + lastBreatheLED - indexOffset, Color.kBlack); - else - LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - breathingLEDs, Color.kBlack); - + for (int i = 0; i < breathingLEDs; i++) { + if (lastBreatheLED - i >= indexOffset) + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i, color); + else + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i + numberOfLEDs, color); + } } void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { From cf132a3f0ad903ddc75ffdcaaef8f6e99c7c1b69 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Wed, 16 Oct 2024 07:34:15 +0300 Subject: [PATCH 159/261] auto changes and camera pitch --- .../pathplanner/autos/SourceSide3NoteAuto.auto | 6 +++--- .../autos/SourceSide3NoteAutoRight.auto | 14 ++++---------- .../pathplanner/paths/SourceSide3NoteAuto1.path | 16 ++++++++-------- .../paths/SourceSide3NoteAutoRight2.path | 16 ++++++++-------- .../trigon/robot/constants/CameraConstants.java | 4 ++-- .../robot/subsystems/intake/IntakeConstants.java | 2 +- 6 files changed, 26 insertions(+), 32 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index 33053350..ac5546ad 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7547034812379487, - "y": 3.84449444955228 + "x": 0.6946944060158917, + "y": 4.426390230452671 }, - "rotation": -38.65999999999997 + "rotation": -58.62699485989161 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index 8a956f6b..0316028d 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7547034812379487, - "y": 3.84449444955228 + "x": 0.6946944060158917, + "y": 4.426390230452671 }, - "rotation": 0 + "rotation": -60.10109816138545 }, "command": { "type": "sequential", @@ -52,12 +52,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, { "type": "wait", "data": { @@ -87,7 +81,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.2 } }, { diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 6a83faea..47c8dc18 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.7547034812379487, - "y": 3.84449444955228 + "x": 0.6946944060158917, + "y": 4.426390230452671 }, "prevControl": null, "nextControl": { - "x": 1.7086006002997924, - "y": 3.3530929033689056 + "x": 1.494174579798092, + "y": 3.7341573970558875 }, "isLocked": false, - "linkedName": null + "linkedName": "SourceSide3NoteAutoStartPoint" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 2.778681091803989 }, "prevControl": { - "x": 2.059866537183144, - "y": 3.1400057581152936 + "x": 2.1084093474600265, + "y": 3.3051680355142192 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": "SourceSide3NoteAuto", "previewStartingState": { - "rotation": -38.65999999999997, + "rotation": -59.03624346792651, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index 4b7bd4ea..45dda295 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.8981397630253705, - "y": 2.778681091803989 + "x": 0.6946944060158917, + "y": 4.426390230452671 }, "prevControl": null, "nextControl": { - "x": 4.049984437997045, - "y": 2.2980248777398966 + "x": 4.107109781915527, + "y": 1.6672086550823948 }, "isLocked": false, - "linkedName": "SourceSide3NoteAutoFirstFeed" + "linkedName": "SourceSide3NoteAutoStartPoint" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 0.7804612792324187 }, "prevControl": { - "x": 7.374171368061045, - "y": 1.0117090656716528 + "x": 6.242306831407013, + "y": 1.296717842841861 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "AlignToNote", - "waypointRelativePos": 0.85, + "waypointRelativePos": 0.75, "command": { "type": "parallel", "data": { diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 22ee7712..fecf5d51 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -16,11 +16,11 @@ public class CameraConstants { private static final Transform3d FRONT_CENTER_TO_CAMERA = new Transform3d( new Translation3d(0.0465, 0.325, 0.192), - new Rotation3d(0, Units.degreesToRadians(-31.5), 0) + new Rotation3d(0, Units.degreesToRadians(-32.65), 0) ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), - new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33.15), Math.PI + Units.degreesToRadians(0)) + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-31.38), Math.PI + Units.degreesToRadians(0)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 345de2c9..cf22d87a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -74,7 +74,7 @@ public class IntakeConstants { static final double COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; - public static final double FEEDING_INDICATION_BREATHING_TIME_SECONDS = 1; + public static final double FEEDING_INDICATION_BREATHING_TIME_SECONDS = 0.6; static { configureMasterMotor(); From b0ad0c731124ddcdf41771b46428f425d80fc90e Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Wed, 16 Oct 2024 09:44:40 +0300 Subject: [PATCH 160/261] Intake fix + 1/2 std calib --- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 4 ++-- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index fecf5d51..6d7e5e72 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -10,7 +10,7 @@ public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.005, + TRANSLATIONS_STD_EXPONENT = 0.0003, THETA_STD_EXPONENT = 0.01; private static final Transform3d @@ -20,7 +20,7 @@ public class CameraConstants { ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), - new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-31.38), Math.PI + Units.degreesToRadians(0)) + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33.5), Math.PI + Units.degreesToRadians(0)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index cf22d87a..b392f84f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -90,6 +90,7 @@ private static void configureMasterMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; + config.HardwareLimitSwitch.ForwardLimitEnable = false; config.HardwareLimitSwitch.ReverseLimitEnable = false; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -112,6 +113,7 @@ private static void configureFollowerMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; + config.HardwareLimitSwitch.ForwardLimitEnable = false; config.HardwareLimitSwitch.ReverseLimitEnable = false; FOLLOWER_MOTOR.applyConfiguration(config); From bb017155480d3d94355905f74b8dd4e8a492a118 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Wed, 16 Oct 2024 09:55:24 +0300 Subject: [PATCH 161/261] I had time so I did LED stuff --- .../trigon/robot/subsystems/intake/Intake.java | 2 +- .../robot/subsystems/ledstrip/LEDStrip.java | 18 ++++++++++++++++-- .../subsystems/ledstrip/LEDStripCommands.java | 11 +++++++++-- 3 files changed, 26 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 22b78741..a60b02a3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -95,7 +95,7 @@ private Command getCollectionIndicationLEDsCommand() { } private Command getFeedingIndicationLEDsCommand() { - return LEDStripCommands.getBreathingCommand(Color.kPurple, 5, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS); + return LEDStripCommands.getSingleBreatheCommand(Color.kPurple, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS); } private void configureLEDsTrigger() { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 93ece311..a5ee8a7e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -88,9 +88,9 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - void breathe(Color color, int breathingLEDs) { + void breathe(Color color, int breathingLEDs, double cycleTimeSeconds) { clearLedColors(); - double moveLEDTimeSeconds = IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS / numberOfLEDs; + double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; double currentTime = Timer.getFPGATimestamp(); if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { lastBreatheMovementTime = currentTime; @@ -106,6 +106,20 @@ void breathe(Color color, int breathingLEDs) { } } + void singleBreathe(Color color, int breathingLEDs, double timeSeconds) { + clearLedColors(); + double moveLEDTimeSeconds = timeSeconds / numberOfLEDs; + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { + lastBreatheMovementTime = currentTime; + lastBreatheLED++; + } + for (int i = 0; i < breathingLEDs; i++) { + if (lastBreatheLED - i <= indexOffset + numberOfLEDs) + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i, color); + } + } + void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { final int ledsPerSection = (int) Math.floor(numberOfLEDs / 3.0); setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index c8866fb3..4ac46205 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -35,9 +35,16 @@ public static Command getRainbowCommand(LEDStrip... ledStrips) { ).ignoringDisable(true); } - public static Command getBreathingCommand(Color color, int breathingLEDs, LEDStrip... ledStrips) { + public static Command getBreathingCommand(Color color, int breathingLEDs, double cycleTimeSeconds, LEDStrip... ledStrips) { return new RunCommand( - () -> runForLEDs((ledStrip) -> ledStrip.breathe(color, breathingLEDs), ledStrips), + () -> runForLEDs((ledStrip) -> ledStrip.breathe(color, breathingLEDs, cycleTimeSeconds), ledStrips), + ledStrips + ).ignoringDisable(true); + } + + public static Command getSingleBreatheCommand(Color color, int breathingLEDs, double timeSeconds, LEDStrip... ledStrips) { + return new RunCommand( + () -> runForLEDs((ledStrip) -> ledStrip.singleBreathe(color, breathingLEDs, timeSeconds)), ledStrips ).ignoringDisable(true); } From c4e44410602eef24be466e3eed526bef18b4a2e8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Wed, 16 Oct 2024 14:09:06 +0300 Subject: [PATCH 162/261] I'll check my changes laterrrrrr --- .../commands/factories/GeneralCommands.java | 4 ++ .../commands/factories/ShootingCommands.java | 3 +- .../robot/constants/CameraConstants.java | 4 +- .../robot/constants/FieldConstants.java | 4 +- .../robot/constants/ShootingConstants.java | 2 +- .../robot/subsystems/intake/Intake.java | 2 +- .../subsystems/intake/IntakeConstants.java | 2 +- .../robot/subsystems/ledstrip/LEDStrip.java | 37 +++++++++---------- .../subsystems/ledstrip/LEDStripCommands.java | 14 ++----- .../subsystems/shooter/ShooterConstants.java | 4 +- 10 files changed, 36 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index be9e60f5..29228b13 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -121,6 +121,10 @@ public static Command runWhen(Command command, BooleanSupplier condition) { return new WaitUntilCommand(condition).andThen(command); } + public static Command runWhen(Command command,BooleanSupplier condition, double debounceTimeSeconds) { + return new WaitUntilCommand(condition).andThen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition).repeatedly())); + } + public static Command duplicate(Command command) { return new FunctionalCommand( command::initialize, diff --git a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java index 31411379..e08927a4 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java @@ -91,7 +91,8 @@ private static Command getFeedNoteForShootingCommand() { .alongWith(GeneralCommands.getVisualizeNoteShootingCommand()), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch() && - RobotContainer.SWERVE.atAngle(SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle()) + RobotContainer.SWERVE.atAngle(SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle()), + 0.2 ); } diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 6d7e5e72..59658a80 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -10,7 +10,7 @@ public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.0003, + TRANSLATIONS_STD_EXPONENT = 0.0004, THETA_STD_EXPONENT = 0.01; private static final Transform3d @@ -20,7 +20,7 @@ public class CameraConstants { ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), - new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33.5), Math.PI + Units.degreesToRadians(0)) + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-34.1), Math.PI + Units.degreesToRadians(0)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index f00343b1..c6146c48 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; +import org.littletonrobotics.junction.Logger; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; @@ -19,7 +20,6 @@ public class FieldConstants { try { APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); } catch (IOException e) { - System.out.println("test"); throw new RuntimeException(e); } } @@ -32,7 +32,7 @@ public class FieldConstants { SPEAKER_TAG_ID = 7, AMP_TAG_ID = 6; private static final Translation3d - SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.82), + SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.80), AMP_TAG_TO_AMP = new Translation3d(0, 0.03, -0.32); public static final MirrorableTranslation3d SPEAKER_TRANSLATION = new MirrorableTranslation3d(TAG_ID_TO_POSE.get(SPEAKER_TAG_ID).getTranslation().plus(SPEAKER_TAG_TO_SPEAKER), true), diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 800586de..81a3e5e6 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -8,7 +8,7 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double - SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, + SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 50, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index a60b02a3..5e7239a7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -95,7 +95,7 @@ private Command getCollectionIndicationLEDsCommand() { } private Command getFeedingIndicationLEDsCommand() { - return LEDStripCommands.getSingleBreatheCommand(Color.kPurple, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS); + return LEDStripCommands.getBreatheCommand(Color.kPurple, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, false, LEDStrip.LED_STRIPS); } private void configureLEDsTrigger() { diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index b392f84f..2dead57c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -74,7 +74,7 @@ public class IntakeConstants { static final double COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; - public static final double FEEDING_INDICATION_BREATHING_TIME_SECONDS = 0.6; + public static final double FEEDING_INDICATION_BREATHING_TIME_SECONDS = 0.3; static { configureMasterMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index a5ee8a7e..ecbe38ae 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.factories.GeneralCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -15,9 +16,9 @@ public class LEDStrip extends SubsystemBase { private final boolean inverted; private final int numberOfLEDs; private int lastBreatheLED; + private double lastBreatheMovementTime = 0; private double rainbowFirstPixelHue = 0; private boolean areLEDsOnForBlinking = false; - private double lastBreatheMovementTime = 0; private double lastBlinkTime = 0; static { @@ -37,7 +38,7 @@ public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { this.inverted = inverted; this.numberOfLEDs = numberOfLEDs; - lastBreatheLED = indexOffset; + resetBreatheSettings(0); addLEDStripToLEDStripsArray(this); } @@ -88,7 +89,13 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - void breathe(Color color, int breathingLEDs, double cycleTimeSeconds) { + void resetBreatheSettings(int breathingLEDs) { + lastBreatheLED = indexOffset + breathingLEDs; + lastBreatheMovementTime = Timer.getFPGATimestamp(); + System.out.println(lastBreatheLED); + } + + void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop) { clearLedColors(); double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; double currentTime = Timer.getFPGATimestamp(); @@ -96,27 +103,17 @@ void breathe(Color color, int breathingLEDs, double cycleTimeSeconds) { lastBreatheMovementTime = currentTime; lastBreatheLED++; } - if (lastBreatheLED >= numberOfLEDs + indexOffset) - lastBreatheLED = indexOffset; - for (int i = 0; i < breathingLEDs; i++) { - if (lastBreatheLED - i >= indexOffset) - LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i, color); + if (lastBreatheLED >= numberOfLEDs + indexOffset) { + if (shouldLoop) + lastBreatheLED = indexOffset; else - LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i + numberOfLEDs, color); - } - } - - void singleBreathe(Color color, int breathingLEDs, double timeSeconds) { - clearLedColors(); - double moveLEDTimeSeconds = timeSeconds / numberOfLEDs; - double currentTime = Timer.getFPGATimestamp(); - if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { - lastBreatheMovementTime = currentTime; - lastBreatheLED++; + setDefaultCommandForAllLEDS(CommandConstants.DEFAULT_LEDS_COMMAND); } for (int i = 0; i < breathingLEDs; i++) { - if (lastBreatheLED - i <= indexOffset + numberOfLEDs) + if (lastBreatheLED - i >= indexOffset && lastBreatheLED - i < indexOffset + numberOfLEDs) LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i, color); + else if (lastBreatheLED - i < indexOffset + numberOfLEDs) + LEDStripConstants.LED_BUFFER.setLED(lastBreatheLED - i + numberOfLEDs, color); } } diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 4ac46205..c0a378cd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -35,16 +35,10 @@ public static Command getRainbowCommand(LEDStrip... ledStrips) { ).ignoringDisable(true); } - public static Command getBreathingCommand(Color color, int breathingLEDs, double cycleTimeSeconds, LEDStrip... ledStrips) { - return new RunCommand( - () -> runForLEDs((ledStrip) -> ledStrip.breathe(color, breathingLEDs, cycleTimeSeconds), ledStrips), - ledStrips - ).ignoringDisable(true); - } - - public static Command getSingleBreatheCommand(Color color, int breathingLEDs, double timeSeconds, LEDStrip... ledStrips) { - return new RunCommand( - () -> runForLEDs((ledStrip) -> ledStrip.singleBreathe(color, breathingLEDs, timeSeconds)), + public static Command getBreatheCommand(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, LEDStrip... ledStrips) { + return new InitExecuteCommand( + () -> runForLEDs((LEDStrip) -> LEDStrip.resetBreatheSettings(breathingLEDs)), + () -> runForLEDs((ledStrip) -> ledStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), ledStrips ).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 39169da5..f777569d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -66,8 +66,8 @@ public class ShooterConstants { LEFT_MECHANISM = new SpeedMechanism2d("LeftShooterMechanism", MAX_DISPLAYABLE_VELOCITY); public static final double WHEEL_DIAMETER_METERS = edu.wpi.first.math.util.Units.inchesToMeters(4); - public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1.2; - static final double VELOCITY_TOLERANCE = 1; + public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1.1; + static final double VELOCITY_TOLERANCE = 0.4; static { configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_INVERTED_VALUE, RIGHT_SIMULATION, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_KS, RIGHT_KV, RIGHT_KA); From 75c6ee3b5d87728998e86e4ec6a7708e29ef60fc Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Thu, 17 Oct 2024 21:12:43 +0300 Subject: [PATCH 163/261] camera and shooting --- .../trigon/robot/constants/FieldConstants.java | 2 +- .../trigon/robot/constants/ShootingConstants.java | 9 +++++---- .../trigon/robot/misc/ShootingCalculations.java | 2 +- .../poseestimator/PoseEstimator.java | 15 ++++++++------- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index c6146c48..46ecd47d 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -32,7 +32,7 @@ public class FieldConstants { SPEAKER_TAG_ID = 7, AMP_TAG_ID = 6; private static final Translation3d - SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.80), + SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.79), AMP_TAG_TO_AMP = new Translation3d(0, 0.03, -0.32); public static final MirrorableTranslation3d SPEAKER_TRANSLATION = new MirrorableTranslation3d(TAG_ID_TO_POSE.get(SPEAKER_TAG_ID).getTranslation().plus(SPEAKER_TAG_TO_SPEAKER), true), diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 81a3e5e6..1ba1a8a6 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -10,7 +10,8 @@ public class ShootingConstants { public static final double SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 50, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, - FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; + FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 3; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, @@ -23,9 +24,9 @@ public class ShootingConstants { PREPARE_AMP_PITCH = Rotation2d.fromDegrees(50), SHOOT_AMP_PITCH = Rotation2d.fromDegrees(44), HIGH_EJECT_PITCH = Rotation2d.fromDegrees(45), - MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(13), - EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13), - CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(13); + MANUAL_LOW_DELIVERY_PITCH = Rotation2d.fromDegrees(20), + EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(20), + CLOSE_EJECT_FROM_SHOOTER_PITCH = Rotation2d.fromDegrees(20); public static final Pose3d ROBOT_RELATIVE_PITCHER_PIVOT_POINT = new Pose3d(0.2521, 0, 0.15545, new Rotation3d(0, 0, Math.PI)); public static final Transform3d PITCHER_PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.4209, 0, -0.0165, new Rotation3d()); diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index 50a4e46d..48fb0ef6 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -137,7 +137,7 @@ private TargetShootingState calculateTargetShootingState(TargetShootingState sta private TargetShootingState calculateTargetShootingState(Translation3d shootingVector) { final MirrorableRotation2d targetRobotAngle = new MirrorableRotation2d(getYaw(shootingVector), false); final Rotation2d targetPitch = getPitch(shootingVector); - final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()); + final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()) + ShootingConstants.SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND; return new TargetShootingState(targetRobotAngle, targetPitch, targetVelocity); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index d41532f3..26bc7732 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -84,18 +84,19 @@ public void setGyroHeadingToBestSolvePNPHeading() { if (aprilTagCameras.length == 0) return; int closestCameraToTag = 0; + boolean seesTag = false; for (int i = 0; i < aprilTagCameras.length; i++) { - if (aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters() == 0) { - closestCameraToTag++; - if (closestCameraToTag > aprilTagCameras.length - 1) - return; - } + if (aprilTagCameras[i].getDistanceToBestTagMeters() == 0) + continue; if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) closestCameraToTag = i; + seesTag = true; } - final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); - resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + if (seesTag) { + final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + } } private void updateFromVision() { From ca3debb286d1d9310443df9fef58154ca443ceeb Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 17 Oct 2024 21:51:15 +0300 Subject: [PATCH 164/261] The LED stuff works! --- .../frc/trigon/robot/subsystems/intake/Intake.java | 8 +++++++- .../trigon/robot/subsystems/ledstrip/LEDStrip.java | 14 +++++--------- .../subsystems/ledstrip/LEDStripCommands.java | 4 ++-- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 5e7239a7..e59770e4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -71,8 +71,10 @@ void sendStaticBrakeRequest() { void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; - if (targetState == IntakeConstants.IntakeState.FEED_SHOOTING || targetState == IntakeConstants.IntakeState.FEED_AMP || targetState == IntakeConstants.IntakeState.EJECT) + if (targetState == IntakeConstants.IntakeState.FEED_SHOOTING || targetState == IntakeConstants.IntakeState.FEED_AMP) getFeedingIndicationLEDsCommand().schedule(); + if (targetState == IntakeConstants.IntakeState.EJECT) + getEjectingIndicationLEDsCommand().schedule(); setTargetVoltage(targetState.voltage); } @@ -98,6 +100,10 @@ private Command getFeedingIndicationLEDsCommand() { return LEDStripCommands.getBreatheCommand(Color.kPurple, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, false, LEDStrip.LED_STRIPS); } + private Command getEjectingIndicationLEDsCommand() { + return LEDStripCommands.getBreatheCommand(Color.kBlue, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, false, LEDStrip.LED_STRIPS); + } + private void configureLEDsTrigger() { Trigger trigger = new Trigger(this::hasNote); trigger.onTrue(new InstantCommand(() -> LEDStrip.changeDefaultCommandForAllLEDs(LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS)))); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index ecbe38ae..01d47c1c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.factories.GeneralCommands; -import frc.trigon.robot.subsystems.intake.IntakeConstants; public class LEDStrip extends SubsystemBase { public static LEDStrip[] LED_STRIPS = new LEDStrip[0]; @@ -38,7 +37,7 @@ public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { this.inverted = inverted; this.numberOfLEDs = numberOfLEDs; - resetBreatheSettings(0); + resetBreatheSettings(); addLEDStripToLEDStripsArray(this); } @@ -89,10 +88,9 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - void resetBreatheSettings(int breathingLEDs) { - lastBreatheLED = indexOffset + breathingLEDs; + void resetBreatheSettings() { + lastBreatheLED = indexOffset; lastBreatheMovementTime = Timer.getFPGATimestamp(); - System.out.println(lastBreatheLED); } void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop) { @@ -104,10 +102,8 @@ void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean sh lastBreatheLED++; } if (lastBreatheLED >= numberOfLEDs + indexOffset) { - if (shouldLoop) - lastBreatheLED = indexOffset; - else - setDefaultCommandForAllLEDS(CommandConstants.DEFAULT_LEDS_COMMAND); + if (!shouldLoop) + CommandConstants.DEFAULT_LEDS_COMMAND.schedule(); } for (int i = 0; i < breathingLEDs; i++) { if (lastBreatheLED - i >= indexOffset && lastBreatheLED - i < indexOffset + numberOfLEDs) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index c0a378cd..87fccb7d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -36,9 +36,9 @@ public static Command getRainbowCommand(LEDStrip... ledStrips) { } public static Command getBreatheCommand(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, LEDStrip... ledStrips) { - return new InitExecuteCommand( - () -> runForLEDs((LEDStrip) -> LEDStrip.resetBreatheSettings(breathingLEDs)), + return new ExecuteEndCommand( () -> runForLEDs((ledStrip) -> ledStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), + () -> runForLEDs(LEDStrip::resetBreatheSettings, ledStrips), ledStrips ).ignoringDisable(true); } From 12f4b32bce0d9801d2414aa70ce5ed5eb1001101 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 17 Oct 2024 21:56:06 +0300 Subject: [PATCH 165/261] Climber single movement commands --- src/main/java/frc/trigon/robot/RobotContainer.java | 4 ++++ .../java/frc/trigon/robot/commands/CommandConstants.java | 4 ++++ .../frc/trigon/robot/constants/OperatorConstants.java | 8 ++++++-- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 129d63b6..fff4856a 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -88,6 +88,10 @@ private void bindControllerCommands() { OperatorConstants.CLIMB_TRIGGER.whileTrue(GeneralCommands.getClimbCommand()); OperatorConstants.MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_DOWN_MANUALLY_COMMAND); OperatorConstants.MOVE_CLIMBER_UP_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_UP_MANUALLY_COMMAND); + OperatorConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY.whileTrue(CommandConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY_COMMAND); + OperatorConstants.MOVE_RIGHT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY_COMMAND); + OperatorConstants.MOVE_LEFT_CLIMBER_DOWN_MANUALLY.whileTrue(CommandConstants.MOVE_LEFT_CLIMBER_DOWN_MANUALLY_COMMAND); + OperatorConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY_COMMAND); OperatorConstants.OVERRIDE_IS_CLIMBING_TRIGGER.onTrue(CommandConstants.OVERRIDE_IS_CLIMBING_COMMAND); OperatorConstants.SPEAKER_SHOT_TRIGGER.whileTrue(CommandConstants.SHOOT_AT_SPEAKER_COMMAND); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 0cafd238..2896b180 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -90,6 +90,10 @@ public class CommandConstants { DEFAULT_CLIMBER_COMMAND = ClimberCommands.getStopCommand(), MOVE_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), MOVE_CLIMBER_UP_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_UP_VOLTAGE).alongWith(new InstantCommand(() -> RobotContainer.CLIMBER.setIsClimbing(true))), + MOVE_RIGHT_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE, 0), + MOVE_RIGHT_CLIMBER_UP_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_UP_VOLTAGE, 0), + MOVE_LEFT_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(0, ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), + MOVE_LEFT_CLIMBER_UP_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(0, ClimberConstants.MOVE_CLIMBER_UP_VOLTAGE), OVERRIDE_IS_CLIMBING_COMMAND = new InstantCommand(() -> { RobotContainer.CLIMBER.setIsClimbing(false); Logger.recordOutput("IsClimbing", false); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 82cdc001..cc310b15 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -35,6 +35,10 @@ public class OperatorConstants { OVERRIDE_IS_CLIMBING_TRIGGER = OPERATOR_CONTROLLER.i(), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), MOVE_CLIMBER_UP_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f2(), + MOVE_RIGHT_CLIMBER_DOWN_MANUALLY = OPERATOR_CONTROLLER.f3(), + MOVE_RIGHT_CLIMBER_UP_MANUALLY = OPERATOR_CONTROLLER.f4(), + MOVE_LEFT_CLIMBER_DOWN_MANUALLY = OPERATOR_CONTROLLER.f5(), + MOVE_LEFT_CLIMBER_UP_MANUALLY = OPERATOR_CONTROLLER.f6(), CONTINUE_TRIGGER = DRIVER_CONTROLLER.leftBumper().or(OPERATOR_CONTROLLER.k()), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), @@ -51,6 +55,6 @@ public class OperatorConstants { AMP_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.x()), AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), - //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), - RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); + //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), + RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file From 71e420b89b352ce60e451e7162a1e73976b11e21 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 17 Oct 2024 22:23:46 +0300 Subject: [PATCH 166/261] Possibly perfected intake stuff --- .../java/frc/trigon/robot/subsystems/intake/IntakeCommands.java | 2 +- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 0df2162c..651fafd3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -59,6 +59,6 @@ private static Command getStopIntakeCommand() { private static Command getCorrectNotePositionCommand() { - return getSetTargetStateCommand(IntakeConstants.IntakeState.CORRECT_NOTE_POSITION).withTimeout(IntakeConstants.CORRECT_NOTE_POSITION_TIMEOUT_SECONDS); + return getSetTargetStateCommand(IntakeConstants.IntakeState.CORRECT_NOTE_POSITION).onlyWhile(RobotContainer.INTAKE::hasNote); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 2dead57c..580fe264 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -135,7 +135,7 @@ public enum IntakeState { STOP(0), FEED_SHOOTING(8), FEED_AMP(5), - CORRECT_NOTE_POSITION(-3); + CORRECT_NOTE_POSITION(-1); public final double voltage; From 94fbc71cee520694291457d687689219a7b3f070 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 17 Oct 2024 23:04:32 +0300 Subject: [PATCH 167/261] Fixed stupid things --- .../trigon/robot/subsystems/pitcher/PitcherConstants.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 52157ad5..45238e97 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -146,7 +146,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.setPhysicsSimulation(SIMULATION); MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 1000); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); @@ -177,10 +177,6 @@ public static void configureEncoder() { config.MagnetSensor.AbsoluteSensorRange = ENCODER_ABSOLUTE_SENSOR_RANGE_VALUE; ENCODER.applyConfiguration(config); - - ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); - ENCODER.registerSignal(CANcoderSignal.POSITION, 100); - ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); ENCODER.registerSignal(CANcoderSignal.POSITION, 100); From 6d4874fac0f6f7545d54d0124f13e43b83e8123d Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 00:00:41 +0300 Subject: [PATCH 168/261] added lower amp trigger --- src/main/java/frc/trigon/robot/RobotContainer.java | 5 ++++- .../frc/trigon/robot/constants/OperatorConstants.java | 1 + .../subsystems/ampaligner/AmpAlignerCommands.java | 10 ++++++++++ .../subsystems/ampaligner/AmpAlignerConstants.java | 1 + 4 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 129d63b6..f871061e 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -18,6 +18,8 @@ import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ampaligner.AmpAligner; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerCommands; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerConstants; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; @@ -99,6 +101,7 @@ private void bindControllerCommands() { OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(true)); OperatorConstants.AMP_WITHOUT_ALIGN_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(false)); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); + OperatorConstants.LOWER_AMP_ALIGNER_TRIGGER.whileTrue(AmpAlignerCommands.getSetTargetVoltageCommand(AmpAlignerConstants.LOWER_AMP_ALIGNER_VOLTAGE)); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); } @@ -110,7 +113,7 @@ private void configureSysIdBindings(MotorSubsystem subsystem) { OperatorConstants.BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER.whileTrue(subsystem.getDynamicCharacterizationCommand(SysIdRoutine.Direction.kReverse)); subsystem.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(subsystem)); } - + private void buildAutoChooser() { autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser("FrontRow4NoteAuto")); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 82cdc001..1400ab55 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -51,6 +51,7 @@ public class OperatorConstants { AMP_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.x()), AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), + LOWER_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java index 48c40bd6..733babd2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java @@ -1,7 +1,10 @@ package frc.trigon.robot.subsystems.ampaligner; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.NetworkTablesCommand; @@ -30,4 +33,11 @@ public static Command getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerSta RobotContainer.AMP_ALIGNER ); } + + public static Command getSetTargetVoltageCommand(double voltage) { + return new StartEndCommand( + () -> RobotContainer.AMP_ALIGNER.drive(Units.Volts.of(voltage)), + RobotContainer.AMP_ALIGNER::stop + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java index e139927f..29996579 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java @@ -65,6 +65,7 @@ public class AmpAlignerConstants { public static final Transform3d PITCHER_TO_AMP_ALIGNER = new Transform3d(-0.4838, 0, 0.1472, new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(6.3), 0)); + public static final double LOWER_AMP_ALIGNER_VOLTAGE = -1; static final Rotation2d READY_FOR_DEFAULT_PITCHER_MOVEMENT_ANGLE = Rotation2d.fromDegrees(80); static final Rotation2d LIMIT_SWITCH_PRESSED_ANGLE = Rotation2d.fromDegrees(173.7); static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.01; From 33b560a7a9b0d12269e40c9085411daed539c7f3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 18 Oct 2024 00:09:37 +0300 Subject: [PATCH 169/261] added blocking command --- src/main/java/frc/trigon/robot/RobotContainer.java | 3 ++- .../frc/trigon/robot/commands/factories/AmpCommands.java | 8 ++++++++ .../frc/trigon/robot/constants/OperatorConstants.java | 1 + .../trigon/robot/subsystems/pitcher/PitcherConstants.java | 1 + 4 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 48bc9b3d..3568d58c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -106,6 +106,7 @@ private void bindControllerCommands() { OperatorConstants.AMP_WITHOUT_ALIGN_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(false)); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.LOWER_AMP_ALIGNER_TRIGGER.whileTrue(AmpAlignerCommands.getSetTargetVoltageCommand(AmpAlignerConstants.LOWER_AMP_ALIGNER_VOLTAGE)); + OperatorConstants.BLOCK_TRIGGER.whileTrue(AmpCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); } @@ -117,7 +118,7 @@ private void configureSysIdBindings(MotorSubsystem subsystem) { OperatorConstants.BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER.whileTrue(subsystem.getDynamicCharacterizationCommand(SysIdRoutine.Direction.kReverse)); subsystem.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(subsystem)); } - + private void buildAutoChooser() { autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser("FrontRow4NoteAuto")); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 5387cee7..fae9f96f 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -13,6 +13,7 @@ import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; +import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -35,6 +36,13 @@ public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { )); } + public static Command getBlockCommand() { + return new ParallelCommandGroup( + AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.CLOSE), + PitcherCommands.getSetTargetPitchCommand(PitcherConstants.BLOCK_PITCH) + ); + } + /** * Creates a command that prepares to score in the amp for the autonomous score in amp command. * This command waits until we're within a certain distance from the amp to ensure that the robot doesn't hit the stage. diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 2b2bdf5e..48b6c134 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -56,6 +56,7 @@ public class OperatorConstants { AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), LOWER_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), + BLOCK_TRIGGER = OPERATOR_CONTROLLER.one(), //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 45238e97..15ec8982 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -102,6 +102,7 @@ public class PitcherConstants { public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(13); static final Rotation2d PITCH_TOLERANCE = Rotation2d.fromDegrees(1); public static final Transform3d VISUALIZATION_PITCHER_PIVOT_POINT_TO_HELD_NOTE = new Transform3d(0.24, 0, 0.02, new Rotation3d()); + public static final Rotation2d BLOCK_PITCH = Rotation2d.fromDegrees(70); static { configureMasterMotor(); From 94e4cc2a440590e26be28abb448fb0819959425f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 18 Oct 2024 01:07:10 +0300 Subject: [PATCH 170/261] Fixed block command --- .../java/frc/trigon/robot/commands/factories/AmpCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index fae9f96f..ee0c531f 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -38,7 +38,7 @@ public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { public static Command getBlockCommand() { return new ParallelCommandGroup( - AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.CLOSE), + AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN), PitcherCommands.getSetTargetPitchCommand(PitcherConstants.BLOCK_PITCH) ); } From fd8d679363b64320de30ca1439859efbe4baaf37 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 18 Oct 2024 01:08:33 +0300 Subject: [PATCH 171/261] Fixed climber move stuffy --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 3568d58c..28be28fa 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -91,7 +91,7 @@ private void bindControllerCommands() { OperatorConstants.MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_DOWN_MANUALLY_COMMAND); OperatorConstants.MOVE_CLIMBER_UP_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_UP_MANUALLY_COMMAND); OperatorConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY.whileTrue(CommandConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY_COMMAND); - OperatorConstants.MOVE_RIGHT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_RIGHT_CLIMBER_DOWN_MANUALLY_COMMAND); + OperatorConstants.MOVE_RIGHT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_RIGHT_CLIMBER_UP_MANUALLY_COMMAND); OperatorConstants.MOVE_LEFT_CLIMBER_DOWN_MANUALLY.whileTrue(CommandConstants.MOVE_LEFT_CLIMBER_DOWN_MANUALLY_COMMAND); OperatorConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY.whileTrue(CommandConstants.MOVE_LEFT_CLIMBER_UP_MANUALLY_COMMAND); OperatorConstants.OVERRIDE_IS_CLIMBING_TRIGGER.onTrue(CommandConstants.OVERRIDE_IS_CLIMBING_COMMAND); From eb8fd4a9cdac75bf695e9f4dc93ae8df41889cf8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 01:19:22 +0300 Subject: [PATCH 172/261] STOP PUTTING EVERYTHING ON 1!!! --- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 48b6c134..1a8695a2 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -56,7 +56,7 @@ public class OperatorConstants { AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), LOWER_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), - BLOCK_TRIGGER = OPERATOR_CONTROLLER.one(), - //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), + BLOCK_TRIGGER = OPERATOR_CONTROLLER.b(), + //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConsta CDRFQA nts.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file From 931c3989622562c34b127129143f57fa2e5b13a9 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 01:40:42 +0300 Subject: [PATCH 173/261] New Swerve offset --- src/main/java/frc/trigon/robot/constants/OperatorConstants.java | 2 +- .../frc/trigon/robot/subsystems/swerve/SwerveConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 1a8695a2..c23dbfd0 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -57,6 +57,6 @@ public class OperatorConstants { AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), LOWER_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), BLOCK_TRIGGER = OPERATOR_CONTROLLER.b(), - //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConsta CDRFQA nts.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), + //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 4ea026a6..677aff78 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -28,7 +28,7 @@ public class SwerveConstants { private static final double FRONT_LEFT_STEER_ENCODER_OFFSET = -1.561, FRONT_RIGHT_STEER_ENCODER_OFFSET = -3.431, - REAR_LEFT_STEER_ENCODER_OFFSET = -1.239, + REAR_LEFT_STEER_ENCODER_OFFSET = -0.971 + 0.437, REAR_RIGHT_STEER_ENCODER_OFFSET = -2.008; private static final int FRONT_LEFT_ID = 1, From ebb4102eec67e500bc019b2dfbfba866da4d268f Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 02:25:40 +0300 Subject: [PATCH 174/261] Pretty sure calibrated --- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 59658a80..f5aacf3b 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -16,7 +16,7 @@ public class CameraConstants { private static final Transform3d FRONT_CENTER_TO_CAMERA = new Transform3d( new Translation3d(0.0465, 0.325, 0.192), - new Rotation3d(0, Units.degreesToRadians(-32.65), 0) + new Rotation3d(0, Units.degreesToRadians(-31), 0) ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), From 60a147bc2f645dd7d68e765a4e791a19f5e6a28c Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 03:08:49 +0300 Subject: [PATCH 175/261] Some stuff --- .../autos/3NoteAutoThroughAmp.auto | 14 ++--- .../autos/3NoteAutoThroughAmpLeft.auto | 8 +-- .../paths/3NoteAutoThroughAmp1.path | 52 ------------------- .../paths/3NoteAutoThroughAmp2.path | 14 ++--- .../robot/commands/AlignToNoteCommand.java | 4 +- .../factories/AutonomousCommands.java | 2 +- .../robot/constants/CameraConstants.java | 2 +- .../robot/constants/ShootingConstants.java | 4 +- .../apriltagcamera/AprilTagCamera.java | 4 ++ .../poseestimator/PoseEstimator.java | 27 +++++----- .../subsystems/swerve/SwerveConstants.java | 4 +- 11 files changed, 37 insertions(+), 98 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index 467360b6..4172969d 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -46,16 +46,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp1" - } - }, { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.5 } }, { @@ -132,7 +126,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -228,7 +222,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -324,7 +318,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index 970f0680..0134a136 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -52,16 +52,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "3NoteAutoThroughAmp1" - } - }, { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path deleted file mode 100644 index 4ccf0cbd..00000000 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp1.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.6849446477990359, - "y": 6.688334136763285 - }, - "prevControl": null, - "nextControl": { - "x": 1.6849446477990346, - "y": 6.688334136763285 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8159166009543433, - "y": 6.932078092184688 - }, - "prevControl": { - "x": 1.6424807611249173, - "y": 6.9176251055322355 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "3NoteAutoThroughAmpFirstFeed" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 38.0, - "rotateFast": false - }, - "reversed": false, - "folder": "3NoteAutoThroughAmp", - "previewStartingState": { - "rotation": 59.74356283647073, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index ddf561b5..8da3d8b1 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.8159166009543433, - "y": 6.932078092184688 + "x": 0.6924444618120005, + "y": 6.677362186633205 }, "prevControl": null, "nextControl": { - "x": 2.446567628177991, - "y": 6.443479097784407 + "x": 1.4993411183403562, + "y": 6.595503105536126 }, "isLocked": false, "linkedName": "3NoteAutoThroughAmpFirstFeed" @@ -20,7 +20,7 @@ "y": 6.995245539786855 }, "prevControl": { - "x": 2.6570624081419103, + "x": 2.329626083753592, "y": 6.794303731057604 }, "nextControl": null, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": 28.47, + "rotation": 24.32091962058401, "rotateFast": false }, "reversed": false, "folder": "3NoteAutoThroughAmp", "previewStartingState": { - "rotation": 38.0, + "rotation": 59.30027744918559, "velocity": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index c70da222..c02e3334 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -26,7 +26,7 @@ public class AlignToNoteCommand extends ParallelCommandGroup { public AlignToNoteCommand() { addCommands( - getSetCurrentLEDColorCommand().asProxy(), + getSetCurrentLEDColorCommand(), GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::shouldAlignToNote).asProxy(), new RunCommand(CAMERA::trackObject) ); @@ -37,7 +37,7 @@ private Command getSetCurrentLEDColorCommand() { LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS), LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), CAMERA::hasTargets - ); + ).asProxy(); } private Command getDriveWhileAligningToNoteCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 2134c8ff..6f4acafb 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -44,7 +44,7 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( - LEDStripCommands.getStaticColorCommand(Color.kOrange, LEDStrip.LED_STRIPS).asProxy(), + LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy(), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) ).onlyIf(() -> !RobotContainer.INTAKE.hasNote()); diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index f5aacf3b..38fbca9a 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -20,7 +20,7 @@ public class CameraConstants { ), REAR_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.325 + 0.00975, 0, 0.095), - new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-34.1), Math.PI + Units.degreesToRadians(0)) + new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33), Math.PI + Units.degreesToRadians(0)) ); public static final AprilTagCamera diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 1ba1a8a6..c4709c77 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -8,10 +8,10 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double - SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 50, + SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 55, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, - SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 3; + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 4; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 5db07225..531ab084 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -63,6 +63,10 @@ public boolean hasNewResult() { return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp(); } + public boolean hasResult() { + return inputs.hasResult; + } + public Pose2d getEstimatedRobotPose() { return robotPose; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 26bc7732..3b419ad0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -81,22 +81,21 @@ public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPos } public void setGyroHeadingToBestSolvePNPHeading() { - if (aprilTagCameras.length == 0) - return; + double firstCameraTagDistance = 10; + double secondCameraTagDistance = 10; int closestCameraToTag = 0; - boolean seesTag = false; - for (int i = 0; i < aprilTagCameras.length; i++) { - if (aprilTagCameras[i].getDistanceToBestTagMeters() == 0) - continue; - if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) - closestCameraToTag = i; - seesTag = true; - } - if (seesTag) { - final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); - resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); - } + if (aprilTagCameras[0].hasResult()) + firstCameraTagDistance = aprilTagCameras[0].getDistanceToBestTagMeters(); + if (aprilTagCameras[1].hasResult()) + secondCameraTagDistance = aprilTagCameras[1].getDistanceToBestTagMeters(); + if (firstCameraTagDistance > secondCameraTagDistance) + closestCameraToTag = 1; + else if (secondCameraTagDistance > firstCameraTagDistance) + closestCameraToTag = 0; + + final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); } private void updateFromVision() { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 677aff78..00b67a2e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -80,10 +80,10 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0.1) : - new PIDConstants(5, 0, 0), + new PIDConstants(1, 0, 0), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(2.5, 0, 0.2) : - new PIDConstants(3, 0, 0); + new PIDConstants(2, 0, 0); private static final double MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 720, MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 720; From 0b658affa50a97b85745a9ceee8de92865730e0a Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 05:13:33 +0300 Subject: [PATCH 176/261] Finished shooting stuff --- .../deploy/pathplanner/paths/FrontRow4NoteAuto1.path | 10 +++++----- .../deploy/pathplanner/paths/FrontRow4NoteAuto2.path | 10 +++++----- .../deploy/pathplanner/paths/FrontRow4NoteAuto3.path | 6 +++--- .../deploy/pathplanner/paths/FrontRow4NoteAuto4.path | 6 +++--- .../deploy/pathplanner/paths/FrontRow4NoteAuto5.path | 2 +- .../deploy/pathplanner/paths/FrontRow4NoteAuto6.path | 2 +- .../robot/commands/factories/AutonomousCommands.java | 4 ++-- .../frc/trigon/robot/constants/CameraConstants.java | 2 +- .../frc/trigon/robot/constants/ShootingConstants.java | 6 +++--- .../robot/subsystems/intake/IntakeConstants.java | 2 +- .../robot/subsystems/shooter/ShooterConstants.java | 2 +- .../robot/subsystems/swerve/SwerveConstants.java | 2 +- 12 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index c7539083..8037ce56 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.895296810271618, - "y": 6.990384785778517 + "x": 2.809086415893629, + "y": 6.9346335843668845 }, "prevControl": { - "x": 2.166907896761163, - "y": 6.5518375217273 + "x": 2.080697502383174, + "y": 6.496086320315668 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 2.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index 24bcb8f2..e6ab2f17 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.895296810271618, - "y": 6.990384785778517 + "x": 2.809086415893629, + "y": 6.9346335843668845 }, "prevControl": null, "nextControl": { - "x": 2.164048427036783, - "y": 6.78789880547959 + "x": 2.077838032658794, + "y": 6.732147604067958 }, "isLocked": false, "linkedName": "FrontRow4NoteAutoFirstCollection" @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 2.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index e27190d1..13ca662c 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.895296810271618, + "x": 2.7389214892389893, "y": 5.565873600441298 }, "prevControl": { - "x": 2.795296810271618, + "x": 2.638921489238989, "y": 5.565873600441298 }, "nextControl": null, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 2.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index ca892025..9c4b85cc 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.895296810271618, + "x": 2.7389214892389893, "y": 5.565873600441298 }, "prevControl": null, "nextControl": { - "x": 2.396113343463435, + "x": 2.2397380224308066, "y": 5.555053936962998 }, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 2.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index 35a620d7..99bed00e 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 2.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index fd058163..b14fe81b 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 2.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 6f4acafb..1f6dfb20 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -47,14 +47,14 @@ public static Command getNoteCollectionCommand() { LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy(), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) - ).onlyIf(() -> !RobotContainer.INTAKE.hasNote()); + ); } public static Command getFeedNoteCommand() { return new ParallelCommandGroup( IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_SHOOTING), GeneralCommands.getVisualizeNoteShootingCommand() - ).withTimeout(0.5); + ).withTimeout(0.7); } public static Command getAlignToNoteCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 38fbca9a..a6b31c03 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -10,7 +10,7 @@ public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.0004, + TRANSLATIONS_STD_EXPONENT = 0.0002, THETA_STD_EXPONENT = 0.01; private static final Transform3d diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index c4709c77..cab6b75c 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -8,15 +8,15 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double - SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 55, + SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, - SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 4; + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 0.5; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 37, - MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 10, + MANUAL_LOW_DELIVERY_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND = 45, EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 10, CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = 5; public static final Rotation2d diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 580fe264..d8ed4e74 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -135,7 +135,7 @@ public enum IntakeState { STOP(0), FEED_SHOOTING(8), FEED_AMP(5), - CORRECT_NOTE_POSITION(-1); + CORRECT_NOTE_POSITION(-1.2); public final double voltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index f777569d..5891b54d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -66,7 +66,7 @@ public class ShooterConstants { LEFT_MECHANISM = new SpeedMechanism2d("LeftShooterMechanism", MAX_DISPLAYABLE_VELOCITY); public static final double WHEEL_DIAMETER_METERS = edu.wpi.first.math.util.Units.inchesToMeters(4); - public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1.1; + public static final double RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO = 1.5; static final double VELOCITY_TOLERANCE = 0.4; static { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 00b67a2e..43c2f072 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -80,7 +80,7 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0.1) : - new PIDConstants(1, 0, 0), + new PIDConstants(0.1, 0, 0), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(2.5, 0, 0.2) : new PIDConstants(2, 0, 0); From 20813ee66a4a03304b2c59c111a955c255754517 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 07:55:55 +0300 Subject: [PATCH 177/261] auto stuff --- .pathplanner/settings.json | 2 +- .../autos/Copy of FrontRow4NoteAuto.auto | 365 ++++++++++++++++++ .../pathplanner/autos/FrontRow4NoteAuto.auto | 8 +- .../deploy/pathplanner/autos/New Auto.auto | 109 ++++++ .../paths/3NoteAutoThroughAmp2.path | 2 +- .../paths/3NoteAutoThroughAmp3.path | 4 +- .../paths/3NoteAutoThroughAmp4.path | 4 +- .../paths/3NoteAutoThroughAmp5.path | 4 +- .../paths/3NoteAutoThroughAmp6.path | 4 +- .../paths/3NoteAutoThroughAmpLeft3.path | 4 +- .../paths/3NoteAutoThroughAmpLeft4.path | 4 +- .../paths/3NoteAutoThroughAmpLeft5.path | 4 +- .../paths/5NoteAutoAroundStage1.path | 4 +- .../paths/5NoteAutoAroundStage2.path | 4 +- .../paths/5NoteAutoAroundStage3.path | 4 +- .../paths/5NoteAutoAroundStage4.path | 4 +- .../paths/5NoteAutoAroundStage5.path | 4 +- .../paths/5NoteAutoAroundStage6.path | 4 +- .../paths/5NoteAutoAroundStage7.path | 4 +- .../paths/5NoteAutoAroundStage8.path | 4 +- .../deploy/pathplanner/paths/7NoteAuto1.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto10.path | 4 +- .../deploy/pathplanner/paths/7NoteAuto2.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto3.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto4.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto5.path | 4 +- .../deploy/pathplanner/paths/7NoteAuto6.path | 4 +- .../deploy/pathplanner/paths/7NoteAuto7.path | 4 +- .../deploy/pathplanner/paths/7NoteAuto8.path | 4 +- .../deploy/pathplanner/paths/7NoteAuto9.path | 4 +- .../pathplanner/paths/FrontRow4NoteAuto1.path | 12 +- .../pathplanner/paths/FrontRow4NoteAuto2.path | 10 +- .../pathplanner/paths/FrontRow4NoteAuto3.path | 6 +- .../pathplanner/paths/FrontRow4NoteAuto4.path | 6 +- .../pathplanner/paths/FrontRow4NoteAuto5.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto6.path | 2 +- .../paths/MiddleRush3NoteAuto1.path | 2 +- .../paths/MiddleRush3NoteAuto2.path | 4 +- .../paths/MiddleRush3NoteAuto3.path | 4 +- .../paths/MiddleRush3NoteAuto4.path | 4 +- .../paths/MiddleRush3NoteAuto5.path | 4 +- .../paths/MiddleRush3NoteAuto6.path | 4 +- .../paths/MiddleRush3NoteAuto7.path | 4 +- .../paths/MiddleRush3NoteAuto8.path | 4 +- .../paths/MiddleRush3NoteAuto9.path | 4 +- .../pathplanner/paths/New New Path.path | 52 +++ .../deploy/pathplanner/paths/New Path.path | 52 +++ .../deploy/pathplanner/paths/PIDAuto0.path | 2 +- .../deploy/pathplanner/paths/PIDAuto1.path | 2 +- .../paths/SourceSide3NoteAuto1.path | 2 +- .../paths/SourceSide3NoteAuto2.path | 4 +- .../paths/SourceSide3NoteAuto3.path | 4 +- .../paths/SourceSide3NoteAuto4.path | 4 +- .../paths/SourceSide3NoteAuto5.path | 4 +- .../paths/SourceSide3NoteAutoRight2.path | 4 +- .../paths/SourceSide3NoteAutoRight3.path | 4 +- .../paths/SourceSide3NoteAutoRight4.path | 4 +- .../paths/SourceSide3NoteAutoRight5.path | 4 +- .../paths/SpeakerSide3NoteAuto1.path | 2 +- .../paths/SpeakerSide3NoteAuto2.path | 4 +- .../paths/SpeakerSide3NoteAuto3.path | 4 +- .../paths/SpeakerSide3NoteAuto4.path | 4 +- .../paths/SpeakerSide3NoteAuto5.path | 4 +- .../paths/SpeakerSide3NoteAuto6.path | 4 +- .../paths/SpeakerSide3NoteAuto7.path | 4 +- .../pathplanner/paths/Zayde3NoteAuto1.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto2.path | 4 +- .../pathplanner/paths/Zayde3NoteAuto3.path | 4 +- .../pathplanner/paths/Zayde3NoteAuto4.path | 4 +- .../pathplanner/paths/Zayde3NoteAuto5.path | 4 +- .../pathplanner/paths/Zayde3NoteAuto6.path | 4 +- .../pathplanner/paths/Zayde3NoteAuto7.path | 4 +- src/main/deploy/pathplanner/paths/front1.path | 52 +++ src/main/deploy/pathplanner/paths/front2.path | 52 +++ src/main/deploy/pathplanner/paths/front3.path | 52 +++ src/main/deploy/pathplanner/paths/front4.path | 52 +++ .../robot/constants/ShootingConstants.java | 2 +- .../subsystems/swerve/SwerveConstants.java | 2 +- 78 files changed, 921 insertions(+), 135 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Copy of FrontRow4NoteAuto.auto create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/New New Path.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/pathplanner/paths/front1.path create mode 100644 src/main/deploy/pathplanner/paths/front2.path create mode 100644 src/main/deploy/pathplanner/paths/front3.path create mode 100644 src/main/deploy/pathplanner/paths/front4.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index cfa85e7b..e4902a92 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -23,7 +23,7 @@ "OtherAutos", "SourceSide3NoteAutoVarients" ], - "defaultMaxVel": 1.0, + "defaultMaxVel": 4.0, "defaultMaxAccel": 2.0, "defaultMaxAngVel": 400.0, "defaultMaxAngAccel": 600.0, diff --git a/src/main/deploy/pathplanner/autos/Copy of FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/Copy of FrontRow4NoteAuto.auto new file mode 100644 index 00000000..9b41a4cb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of FrontRow4NoteAuto.auto @@ -0,0 +1,365 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3973572133190786, + "y": 5.565873600441298 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontRow4NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "front1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "front2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "front3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "front4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "Backup", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index fcf866ff..87608a29 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -74,7 +74,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.5 } } ] @@ -176,7 +176,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.5 } } ] @@ -189,7 +189,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.1 } }, { @@ -278,7 +278,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.5 } } ] diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 00000000..c25e2873 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,109 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3239288017037572, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New Path" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "New New Path" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index 8da3d8b1..ce1b17f4 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index 28ed9e5b..e3f8ec4f 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } } ] @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path index 852c566f..ace3d246 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index a54a2de4..288f68bb 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path index 9b071f33..40c901f4 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index 08eb674b..926616d7 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path index a5b0b09e..6ccd7e55 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index e606c426..2d0befe2 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index 52d26200..6f16bcf2 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -68,7 +68,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -83,7 +83,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path index e767ad0f..2be0348a 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index d145d7b4..5711988b 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path index da2da89c..4077fcdd 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index 0364112f..476edc68 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path index 4c8711e6..c2c50b62 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index 3cfef17d..fbabfd4b 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path index 6b7e8345..cb3c7e4e 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto1.path b/src/main/deploy/pathplanner/paths/7NoteAuto1.path index 86f97df5..457e69db 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto1.path @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto10.path b/src/main/deploy/pathplanner/paths/7NoteAuto10.path index ff273dc0..a443919d 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto10.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto10.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto2.path b/src/main/deploy/pathplanner/paths/7NoteAuto2.path index d8fdb0b5..a9d0b31c 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto3.path b/src/main/deploy/pathplanner/paths/7NoteAuto3.path index 38278595..b6db83e6 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto3.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto4.path b/src/main/deploy/pathplanner/paths/7NoteAuto4.path index e71f1c0e..6c73cecf 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto4.path @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index 9a570e5d..0f731588 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto6.path b/src/main/deploy/pathplanner/paths/7NoteAuto6.path index b318e94f..2fb89acc 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto6.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index 2b094baa..c4235230 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto8.path b/src/main/deploy/pathplanner/paths/7NoteAuto8.path index 8f5b9e9a..3fc12471 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto8.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index ae292d15..3737d39f 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index 8037ce56..d4424e88 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.809086415893629, - "y": 6.9346335843668845 + "x": 2.8441688792209487, + "y": 6.969716047694203 }, "prevControl": { - "x": 2.080697502383174, - "y": 6.496086320315668 + "x": 2.1157799657104936, + "y": 6.531168783642987 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 3.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, "goalEndState": { "velocity": 0, - "rotation": 29.139259553040947, + "rotation": 27.77000000000001, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index e6ab2f17..347e9733 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.809086415893629, - "y": 6.9346335843668845 + "x": 2.8441688792209487, + "y": 6.969716047694203 }, "prevControl": null, "nextControl": { - "x": 2.077838032658794, - "y": 6.732147604067958 + "x": 2.112920495986114, + "y": 6.7672300673952765 }, "isLocked": false, "linkedName": "FrontRow4NoteAutoFirstCollection" @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 3.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index 13ca662c..db7b0122 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.7389214892389893, + "x": 2.820780570336069, "y": 5.565873600441298 }, "prevControl": { - "x": 2.638921489238989, + "x": 2.720780570336069, "y": 5.565873600441298 }, "nextControl": null, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 2.5, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index 9c4b85cc..966f4fb4 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.7389214892389893, + "x": 2.820780570336069, "y": 5.565873600441298 }, "prevControl": null, "nextControl": { - "x": 2.2397380224308066, + "x": 2.3215971035278864, "y": 5.555053936962998 }, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 3.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index 99bed00e..6e94dfb1 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 3.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index b14fe81b..9e7f1fd2 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 3.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path index 637850ba..b5de6793 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index 5e5ccf0b..253096ed 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path index 6b610c68..c49e7575 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index 9806879a..d7c310f7 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path index 65e5857c..92d5e94a 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 423b05c1..494fdcd0 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path index 6687179a..df076b12 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index 11b1bf1d..ea04faa0 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path index ef2a2bf0..ce9739ef 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 00000000..60d4177a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.8261782620431335, + "y": 5.531335051274091 + }, + "prevControl": null, + "nextControl": { + "x": 5.416882856557735, + "y": 6.0692661556263285 + }, + "isLocked": false, + "linkedName": "1" + }, + { + "anchor": { + "x": 4.235773257871302, + "y": 6.537032333323926 + }, + "prevControl": { + "x": 5.0660582232845375, + "y": 6.221290163378048 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 13.139999999999986, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 00000000..8990df2f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3239288017037572, + "y": 5.531335051274091 + }, + "prevControl": null, + "nextControl": { + "x": 2.3062377748687117, + "y": 6.303149244475128 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8261782620431335, + "y": 5.531335051274091 + }, + "prevControl": { + "x": 4.60998620002938, + "y": 6.478561561111727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.897271030947643, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path index a3d42343..e55937bf 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path index 5f6e0df8..07b9ef18 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto1.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 47c8dc18..73e7eeae 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path index 64db4624..d32c55b2 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path index bed0e747..d0993214 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path index a38a4a90..f976295c 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path index c094b6cd..d62e4b35 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index 45dda295..26a9255a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 68e19968..4e3d4065 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index 3ed98bfe..8bb0b034 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 5aaa07df..04bc26d5 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path index ce49abb5..6cb73ad6 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index c14dbdcc..d25362c7 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path index 221f7f67..27d6c328 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index 81849e43..16c7723a 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path index c6350208..662cf162 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index bfc9c11d..23b90c92 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path index cb1da68f..390a55c8 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path index f1b07914..f56f14ab 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index 3abdb3c2..bc1afd4c 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path index feeceb32..a3755aaf 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index 3f7707c2..694556d2 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -47,7 +47,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path index 16dd173b..a00d39d5 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index 8c3ace25..61abcdcb 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "AlignToNote" + "name": null } }, { @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path index f2ff78ee..463d6abc 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "StopAligningToNote" + "name": null } } ] @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 4.0, "maxAcceleration": 2.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 diff --git a/src/main/deploy/pathplanner/paths/front1.path b/src/main/deploy/pathplanner/paths/front1.path new file mode 100644 index 00000000..740c4465 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/front1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8441688792209487, + "y": 6.969716047694203 + }, + "prevControl": null, + "nextControl": { + "x": 2.2126845393291923, + "y": 6.747527113287845 + }, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoFirstCollection" + }, + { + "anchor": { + "x": 2.820780570336069, + "y": 5.565873600441298 + }, + "prevControl": { + "x": 1.1134340217398382, + "y": 6.186207700050727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoSecondCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -18.823282103833993, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAuto", + "previewStartingState": { + "rotation": 27.819999999999993, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/front2.path b/src/main/deploy/pathplanner/paths/front2.path new file mode 100644 index 00000000..5fd2c328 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/front2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.820780570336069, + "y": 5.565873600441298 + }, + "prevControl": null, + "nextControl": { + "x": 2.547148467218551, + "y": 5.586611458258547 + }, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoSecondCollection" + }, + { + "anchor": { + "x": 2.0791600728094584, + "y": 5.615860732909115 + }, + "prevControl": { + "x": 2.371652819315141, + "y": 5.606110974692258 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "nne" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.3322198538695922, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAuto", + "previewStartingState": { + "rotation": -19.82440273715712, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/front3.path b/src/main/deploy/pathplanner/paths/front3.path new file mode 100644 index 00000000..fb556ce6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/front3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0791600728094584, + "y": 5.615860732909115 + }, + "prevControl": null, + "nextControl": { + "x": 1.6111716784003651, + "y": 4.777381526259489 + }, + "isLocked": false, + "linkedName": "nne" + }, + { + "anchor": { + "x": 2.7337543047179125, + "y": 4.097305368134887 + }, + "prevControl": { + "x": 1.757418051653207, + "y": 4.260644340766117 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoThirdCollection" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -0.4057415406159514, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAuto", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/front4.path b/src/main/deploy/pathplanner/paths/front4.path new file mode 100644 index 00000000..f9513c4b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/front4.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.7337543047179125, + "y": 4.097305368134887 + }, + "prevControl": null, + "nextControl": { + "x": 2.488649917917414, + "y": 4.065649176428995 + }, + "isLocked": false, + "linkedName": "FrontRow4NoteAutoThirdCollection" + }, + { + "anchor": { + "x": 2.030411281725178, + "y": 4.097305368134887 + }, + "prevControl": { + "x": 2.2839049953634354, + "y": 4.1046482092964185 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -38.64999999999998, + "rotateFast": false + }, + "reversed": false, + "folder": "FrontRow4NoteAuto", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index cab6b75c..1b5620fd 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -11,7 +11,7 @@ public class ShootingConstants { SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, - SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 0.5; + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 3; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 43c2f072..2b1d04d6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -80,7 +80,7 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0.1) : - new PIDConstants(0.1, 0, 0), + new PIDConstants(0, 0, 0.1), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(2.5, 0, 0.2) : new PIDConstants(2, 0, 0); From 09bfbb207999b3a93b95c675eb1766be0270d246 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 18 Oct 2024 15:23:42 +0300 Subject: [PATCH 178/261] Maybe upgraded intake and added LEDAutoSetup --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 ++ .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- .../frc/trigon/robot/subsystems/intake/IntakeCommands.java | 6 +++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 28be28fa..abdbb785 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.LEDAutoSetupCommand; import frc.trigon.robot.commands.factories.AmpCommands; import frc.trigon.robot.commands.factories.AutonomousCommands; import frc.trigon.robot.commands.factories.GeneralCommands; @@ -86,6 +87,7 @@ private void bindControllerCommands() { OperatorConstants.COLLECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getNoteCollectionCommand()); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); + OperatorConstants.LED_AUTO_SETUP_TRIGGER.whileTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); OperatorConstants.CLIMB_TRIGGER.whileTrue(GeneralCommands.getClimbCommand()); OperatorConstants.MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_DOWN_MANUALLY_COMMAND); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index c23dbfd0..e648f716 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -31,6 +31,7 @@ public class OperatorConstants { ALIGN_TO_MIDDLE_STAGE = OPERATOR_CONTROLLER.u(), TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER = OPERATOR_CONTROLLER.o(), TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER = OPERATOR_CONTROLLER.p(), + LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), CLIMB_TRIGGER = OPERATOR_CONTROLLER.c(), OVERRIDE_IS_CLIMBING_TRIGGER = OPERATOR_CONTROLLER.i(), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), @@ -57,6 +58,5 @@ public class OperatorConstants { AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), LOWER_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), BLOCK_TRIGGER = OPERATOR_CONTROLLER.b(), - //ALIGN_TO_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(OperatorConstants.AMP_TRIGGER.or(OperatorConstants.AUTONOMOUS_AMP_TRIGGER).negate()), - RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); + RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 651fafd3..7d63a8da 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -40,13 +40,13 @@ private static Command getCollectionCommand() { (interrupted) -> { if (!interrupted) { RobotContainer.INTAKE.indicateCollection(); - RobotContainer.INTAKE.sendStaticBrakeRequest(); +// RobotContainer.INTAKE.sendStaticBrakeRequest() + getCorrectNotePositionCommand().andThen(getStopIntakeCommand()); } }, RobotContainer.INTAKE::hasNote, RobotContainer.INTAKE - ), - getCorrectNotePositionCommand().andThen(getStopIntakeCommand()) + ) ); } From a4e2137dcd1ff5fca1b88efa9c862bc3688db3ef Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 18 Oct 2024 15:41:33 +0300 Subject: [PATCH 179/261] Possibly improved AlignToNoteCommand (BE READY TO REVERT THIS) --- .../robot/commands/AlignToNoteCommand.java | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index c02e3334..1537bbd6 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.trigon.robot.RobotContainer; @@ -15,7 +16,6 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.trigon.hardware.RobotHardwareStats; -import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.MirrorableRotation2d; public class AlignToNoteCommand extends ParallelCommandGroup { @@ -23,11 +23,12 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : new PIDController(0.0002, 0, 0); + private MirrorableRotation2d targetAngle = null; public AlignToNoteCommand() { addCommands( getSetCurrentLEDColorCommand(), - GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::shouldAlignToNote).asProxy(), + new InstantCommand(this::resetTargetAngle).andThen(GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND).until(this::shouldAlignToNote).andThen(getDriveWhileAligningToNoteCommand())).asProxy(), new RunCommand(CAMERA::trackObject) ); } @@ -37,31 +38,28 @@ private Command getSetCurrentLEDColorCommand() { LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS), LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), CAMERA::hasTargets - ).asProxy(); + ); } private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(getScaledJoystickValue()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), - this::getTargetAngle + () -> targetAngle ); } - private MirrorableRotation2d getTargetAngle() { + private void calculateTargetAngle() { final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); + targetAngle = new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); } private boolean shouldAlignToNote() { - return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); + calculateTargetAngle(); + return /*CAMERA.hasTargets()*/targetAngle.get() != null && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } - private double getScaledJoystickValue() { - final XboxController controller = OperatorConstants.DRIVER_CONTROLLER; - final Rotation2d robotRelativeToFieldRelativeRotation = Rotation2d.fromDegrees(90); - final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().plus(robotRelativeToFieldRelativeRotation); - - return controller.getLeftX() * -robotHeading.getCos() - controller.getLeftY() * -robotHeading.getSin(); + private void resetTargetAngle() { + targetAngle = null; } } \ No newline at end of file From 6d281ce65fcfb814b4f8d638c84387575c95d57b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 18 Oct 2024 16:01:26 +0300 Subject: [PATCH 180/261] Fixed final LED stuff and cleaned `LEDStripCommands` --- .../subsystems/ledstrip/LEDStripCommands.java | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 87fccb7d..a0e9939d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -2,8 +2,6 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.InitExecuteCommand; @@ -12,33 +10,36 @@ public class LEDStripCommands { public static Command getStaticColorCommand(Color color, LEDStrip... ledStrips) { - return new StartEndCommand( - () -> { - }, - () -> runForLEDs((ledStrip -> ledStrip.staticColor(color)), ledStrips), + return new ExecuteEndCommand( + () -> runForLEDs((LEDStrip -> LEDStrip.staticColor(color)), ledStrips), + () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), ledStrips ).ignoringDisable(true); } public static Command getBlinkingCommand(Color color, double blinkingIntervalSeconds, LEDStrip... ledStrips) { return new ExecuteEndCommand( - () -> runForLEDs((ledStrip -> ledStrip.blink(color, blinkingIntervalSeconds)), ledStrips), - () -> runForLEDs((LEDStrip::clearLedColors)), + () -> runForLEDs((LEDStrip -> LEDStrip.blink(color, blinkingIntervalSeconds)), ledStrips), + () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), ledStrips ).ignoringDisable(true); } public static Command getRainbowCommand(LEDStrip... ledStrips) { - return new RunCommand( + return new ExecuteEndCommand( () -> runForLEDs((LEDStrip::rainbow), ledStrips), + () -> runForLEDs(LEDStrip::rainbow, ledStrips), ledStrips ).ignoringDisable(true); } public static Command getBreatheCommand(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, LEDStrip... ledStrips) { return new ExecuteEndCommand( - () -> runForLEDs((ledStrip) -> ledStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), - () -> runForLEDs(LEDStrip::resetBreatheSettings, ledStrips), + () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), + () -> { + runForLEDs(LEDStrip::resetBreatheSettings, ledStrips); + runForLEDs(LEDStrip::clearLedColors, ledStrips); + }, ledStrips ).ignoringDisable(true); } @@ -46,13 +47,13 @@ public static Command getBreatheCommand(Color color, int breathingLEDs, double c public static Command getThreeSectionColorCommand(Supplier firstSectionColor, Supplier secondSectionColor, Supplier thirdSectionColor, LEDStrip... ledStrips) { return new InitExecuteCommand( () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), - () -> runForLEDs((ledStrip) -> ledStrip.threeSectionColor(firstSectionColor.get(), secondSectionColor.get(), thirdSectionColor.get()), ledStrips), + () -> runForLEDs((LEDStrip) -> LEDStrip.threeSectionColor(firstSectionColor.get(), secondSectionColor.get(), thirdSectionColor.get()), ledStrips), ledStrips ).ignoringDisable(true); } public static void runForLEDs(Consumer action, LEDStrip... ledStrips) { - for (LEDStrip ledStrip : ledStrips) - action.accept(ledStrip); + for (LEDStrip LEDStrip : ledStrips) + action.accept(LEDStrip); } } \ No newline at end of file From 52cc1112d202d3ea7a8a4187ebfb6902de32b2cb Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 16:43:54 +0300 Subject: [PATCH 181/261] added optimized auto and fixed LEDCommands Errror --- .pathplanner/settings.json | 1 + ...to.auto => FrontRow4NoteAutoFarShoot.auto} | 0 .../FrontRow4NoteAutoFarShootVarient.auto | 359 ++++++++++++++++++ .../autos/{New Auto.auto => test.auto} | 6 +- .../deploy/pathplanner/paths/7NoteAuto1.path | 30 +- .../deploy/pathplanner/paths/7NoteAuto10.path | 20 +- .../deploy/pathplanner/paths/7NoteAuto3.path | 20 +- .../deploy/pathplanner/paths/7NoteAuto4.path | 20 +- .../deploy/pathplanner/paths/7NoteAuto5.path | 6 - .../paths/{New Path.path => test1.path} | 2 +- .../paths/{New New Path.path => test2.path} | 2 +- .../subsystems/ledstrip/LEDStripCommands.java | 2 +- 12 files changed, 375 insertions(+), 93 deletions(-) rename src/main/deploy/pathplanner/autos/{Copy of FrontRow4NoteAuto.auto => FrontRow4NoteAutoFarShoot.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto rename src/main/deploy/pathplanner/autos/{New Auto.auto => test.auto} (95%) rename src/main/deploy/pathplanner/paths/{New Path.path => test1.path} (98%) rename src/main/deploy/pathplanner/paths/{New New Path.path => test2.path} (98%) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index e4902a92..47d4114f 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -9,6 +9,7 @@ "7NoteAuto", "FrontRow4NoteAuto", "MiddleRush3NoteAuto", + "test", "PIDAuto", "SourceSide3NoteAuto", "SourceSide3NoteAutoRight", diff --git a/src/main/deploy/pathplanner/autos/Copy of FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Copy of FrontRow4NoteAuto.auto rename to src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto new file mode 100644 index 00000000..34cd5057 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto @@ -0,0 +1,359 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6751948895821795, + "y": 4.387391197585246 + }, + "rotation": -59.53445508054011 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "Backup", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/test.auto similarity index 95% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/test.auto index c25e2873..8c0a7b35 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -36,7 +36,7 @@ { "type": "path", "data": { - "pathName": "New Path" + "pathName": "test1" } }, { @@ -74,7 +74,7 @@ { "type": "path", "data": { - "pathName": "New New Path" + "pathName": "test2" } }, { @@ -104,6 +104,6 @@ ] } }, - "folder": null, + "folder": "misc", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto1.path b/src/main/deploy/pathplanner/paths/7NoteAuto1.path index 457e69db..eefc9940 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.172432558641841, - "y": 4.20214579146498 + "x": 0.6751948895821795, + "y": 4.387391197585246 }, "prevControl": null, "nextControl": { - "x": 2.09865958924317, - "y": 4.1046482092964185 + "x": 1.6014219201835096, + "y": 4.289893615416685 }, "isLocked": false, "linkedName": null @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, @@ -69,7 +51,7 @@ "reversed": false, "folder": "7NoteAuto", "previewStartingState": { - "rotation": -49.101008357498635, + "rotation": -59.03624346792648, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto10.path b/src/main/deploy/pathplanner/paths/7NoteAuto10.path index a443919d..43f82415 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto10.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto10.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.10000000000000009, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto3.path b/src/main/deploy/pathplanner/paths/7NoteAuto3.path index b6db83e6..a787423d 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.8, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto4.path b/src/main/deploy/pathplanner/paths/7NoteAuto4.path index 6c73cecf..594e3384 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto4.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 1.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index 0f731588..76d83af7 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -38,12 +38,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/test1.path similarity index 98% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/test1.path index 8990df2f..2e2b91e2 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/test1.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "test", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/test2.path similarity index 98% rename from src/main/deploy/pathplanner/paths/New New Path.path rename to src/main/deploy/pathplanner/paths/test2.path index 60d4177a..df6d4e9d 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/test2.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "test", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index a0e9939d..275908f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -28,7 +28,7 @@ public static Command getBlinkingCommand(Color color, double blinkingIntervalSec public static Command getRainbowCommand(LEDStrip... ledStrips) { return new ExecuteEndCommand( () -> runForLEDs((LEDStrip::rainbow), ledStrips), - () -> runForLEDs(LEDStrip::rainbow, ledStrips), + () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), ledStrips ).ignoringDisable(true); } From 58c2bce1903c1fd27bb5ea4339c398a0dc2e1dc3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 16:55:51 +0300 Subject: [PATCH 182/261] updated 3Triumph and bb --- .../autos/3NoteAutoThroughAmp.auto | 4 ++-- .../autos/SourceSide3NoteAutoRight.auto | 14 ++++-------- .../paths/3NoteAutoThroughAmp3.path | 20 +---------------- .../paths/3NoteAutoThroughAmp4.path | 20 +---------------- .../paths/3NoteAutoThroughAmp5.path | 20 +---------------- .../paths/3NoteAutoThroughAmp6.path | 20 +---------------- .../paths/SourceSide3NoteAutoRight2.path | 22 ++----------------- .../paths/SourceSide3NoteAutoRight3.path | 20 +---------------- .../paths/SourceSide3NoteAutoRight4.path | 20 +---------------- .../paths/SourceSide3NoteAutoRight5.path | 20 +---------------- 10 files changed, 15 insertions(+), 165 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index 4172969d..682ed39e 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -100,7 +100,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.4 } } ] @@ -273,7 +273,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.4 } } ] diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index 0316028d..fbb9bb65 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -33,12 +33,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.05 - } - }, { "type": "named", "data": { @@ -55,7 +49,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.4 } }, { @@ -106,7 +100,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.4 } } ] @@ -151,7 +145,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -202,7 +196,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.4 } } ] diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index e3f8ec4f..e1473f9f 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "AlignToNOte", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path index ace3d246..a91436fc 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index 288f68bb..e75cace1 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "AlignTOnOte", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path index 40c901f4..f73eae54 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index 26a9255a..70c21f7c 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "AlignToNote", - "waypointRelativePos": 0.75, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, @@ -63,7 +45,7 @@ "reversed": false, "folder": "SourceSide3NoteAutoRight", "previewStartingState": { - "rotation": -45.210000000000036, + "rotation": -57.42594286542754, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 4e3d4065..e3022f5a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index 8bb0b034..74572668 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "AlignToNote", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 04bc26d5..d77f0a87 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, From 84264f31aa13fe5870db114078bf75a7060ff00a Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 17:19:35 +0300 Subject: [PATCH 183/261] correted my auto --- .../paths/SourceSide3NoteAuto2.path | 26 +------------------ .../paths/SourceSide3NoteAuto3.path | 20 +------------- .../paths/SourceSide3NoteAuto4.path | 26 +------------------ .../paths/SourceSide3NoteAuto5.path | 20 +------------- 4 files changed, 4 insertions(+), 88 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path index d32c55b2..d48e03d7 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path @@ -36,31 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path index d0993214..df23583e 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path index f976295c..36b4ba51 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path @@ -36,31 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path index d62e4b35..9a52036f 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, From 58a62702cff5c03e45a0013904e8c33c2509413a Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Fri, 18 Oct 2024 17:19:50 +0300 Subject: [PATCH 184/261] ahhhhhhhhhhhhhhhhhhhhh --- .../autos/3NoteAutoThroughAmpLeft.auto | 10 +++++----- .../paths/3NoteAutoThroughAmpLeft3.path | 20 +------------------ .../paths/3NoteAutoThroughAmpLeft4.path | 20 +------------------ .../paths/3NoteAutoThroughAmpLeft5.path | 20 +------------------ 4 files changed, 8 insertions(+), 62 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index 0134a136..973ec414 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -106,7 +106,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.4 } } ] @@ -183,7 +183,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.4 } } ] @@ -228,7 +228,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { @@ -279,7 +279,7 @@ { "type": "wait", "data": { - "waitTime": 0.05 + "waitTime": 0.4 } } ] @@ -324,7 +324,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.3 } }, { diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index 926616d7..20f462ed 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "AlignToNOte", - "waypointRelativePos": 0.8, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path index 6ccd7e55..11d25d91 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.15, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index 2d0befe2..510b601b 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "AlignToNote", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 2.0, From 8c0833a18d5e65d12a50d18f45d55db7ac6f77f3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 05:31:05 +0300 Subject: [PATCH 185/261] ahhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh --- .pathplanner/settings.json | 2 +- .../paths/3NoteAutoThroughAmp2.path | 2 +- .../paths/3NoteAutoThroughAmp3.path | 2 +- .../paths/3NoteAutoThroughAmp4.path | 2 +- .../paths/3NoteAutoThroughAmp5.path | 2 +- .../paths/3NoteAutoThroughAmp6.path | 2 +- .../paths/3NoteAutoThroughAmpLeft3.path | 2 +- .../paths/3NoteAutoThroughAmpLeft4.path | 2 +- .../paths/3NoteAutoThroughAmpLeft5.path | 2 +- .../paths/5NoteAutoAroundStage1.path | 2 +- .../paths/5NoteAutoAroundStage2.path | 2 +- .../paths/5NoteAutoAroundStage3.path | 2 +- .../paths/5NoteAutoAroundStage4.path | 2 +- .../paths/5NoteAutoAroundStage5.path | 2 +- .../paths/5NoteAutoAroundStage6.path | 2 +- .../paths/5NoteAutoAroundStage7.path | 2 +- .../paths/5NoteAutoAroundStage8.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto1.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto10.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto2.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto3.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto4.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto5.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto6.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto7.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto8.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto9.path | 2 +- .../paths/MiddleRush3NoteAuto1.path | 2 +- .../paths/MiddleRush3NoteAuto2.path | 2 +- .../paths/MiddleRush3NoteAuto3.path | 2 +- .../paths/MiddleRush3NoteAuto4.path | 2 +- .../paths/MiddleRush3NoteAuto5.path | 2 +- .../paths/MiddleRush3NoteAuto6.path | 2 +- .../paths/MiddleRush3NoteAuto7.path | 2 +- .../paths/MiddleRush3NoteAuto8.path | 2 +- .../paths/MiddleRush3NoteAuto9.path | 2 +- .../deploy/pathplanner/paths/PIDAuto0.path | 2 +- .../deploy/pathplanner/paths/PIDAuto1.path | 2 +- .../paths/SourceSide3NoteAuto1.path | 2 +- .../paths/SourceSide3NoteAuto2.path | 2 +- .../paths/SourceSide3NoteAuto3.path | 2 +- .../paths/SourceSide3NoteAuto4.path | 2 +- .../paths/SourceSide3NoteAuto5.path | 2 +- .../paths/SourceSide3NoteAutoRight2.path | 2 +- .../paths/SourceSide3NoteAutoRight3.path | 2 +- .../paths/SourceSide3NoteAutoRight4.path | 2 +- .../paths/SourceSide3NoteAutoRight5.path | 2 +- .../paths/SpeakerSide3NoteAuto1.path | 2 +- .../paths/SpeakerSide3NoteAuto2.path | 2 +- .../paths/SpeakerSide3NoteAuto3.path | 2 +- .../paths/SpeakerSide3NoteAuto4.path | 2 +- .../paths/SpeakerSide3NoteAuto5.path | 2 +- .../paths/SpeakerSide3NoteAuto6.path | 2 +- .../paths/SpeakerSide3NoteAuto7.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto1.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto2.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto3.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto4.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto5.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto6.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto7.path | 2 +- src/main/deploy/pathplanner/paths/test1.path | 2 +- src/main/deploy/pathplanner/paths/test2.path | 2 +- .../java/frc/trigon/robot/RobotContainer.java | 6 ++--- .../robot/commands/AlignToNoteCommand.java | 25 +++++++++++-------- .../factories/AutonomousCommands.java | 2 +- .../commands/factories/GeneralCommands.java | 2 +- .../commands/factories/ShootingCommands.java | 3 +-- .../robot/constants/FieldConstants.java | 21 ++++++++-------- .../robot/constants/OperatorConstants.java | 6 ++--- .../robot/constants/ShootingConstants.java | 2 +- .../subsystems/ampaligner/AmpAligner.java | 6 ++++- .../ampaligner/AmpAlignerCommands.java | 7 ++++++ .../ampaligner/AmpAlignerConstants.java | 2 +- .../subsystems/intake/IntakeCommands.java | 4 +-- .../subsystems/intake/IntakeConstants.java | 2 +- .../subsystems/swerve/SwerveConstants.java | 6 ++--- 77 files changed, 117 insertions(+), 103 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 47d4114f..89a4dc01 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -25,7 +25,7 @@ "SourceSide3NoteAutoVarients" ], "defaultMaxVel": 4.0, - "defaultMaxAccel": 2.0, + "defaultMaxAccel": 4.0, "defaultMaxAngVel": 400.0, "defaultMaxAngAccel": 600.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index ce1b17f4..8d6e5d20 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index e1473f9f..05f6df8d 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path index a91436fc..2e0db7ba 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index e75cace1..2a2c3a02 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path index f73eae54..8a9660c8 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index 20f462ed..4e84bad7 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path index 11d25d91..2d93555e 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index 510b601b..4ce49aa0 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index 6f16bcf2..fd2fc0a2 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -84,7 +84,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path index 2be0348a..5f0503ab 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index 5711988b..566cd4e1 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path index 4077fcdd..22a6d8e0 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index 476edc68..09eb0e98 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path index c2c50b62..07aa0bbf 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index fbabfd4b..65a3ca65 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path index cb3c7e4e..d42f82a9 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto1.path b/src/main/deploy/pathplanner/paths/7NoteAuto1.path index eefc9940..45c46ca1 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto1.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto10.path b/src/main/deploy/pathplanner/paths/7NoteAuto10.path index 43f82415..1943a94c 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto10.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto10.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto2.path b/src/main/deploy/pathplanner/paths/7NoteAuto2.path index a9d0b31c..3e9c03e8 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto2.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto3.path b/src/main/deploy/pathplanner/paths/7NoteAuto3.path index a787423d..da386fd7 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto3.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto4.path b/src/main/deploy/pathplanner/paths/7NoteAuto4.path index 594e3384..8406beee 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto4.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index 76d83af7..5ce0f718 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto6.path b/src/main/deploy/pathplanner/paths/7NoteAuto6.path index 2fb89acc..9993d4d8 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto6.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index c4235230..e78fad75 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto8.path b/src/main/deploy/pathplanner/paths/7NoteAuto8.path index 3fc12471..60089835 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto8.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index 3737d39f..b0b43940 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path index b5de6793..57ee08a9 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index 253096ed..3a0e2d0f 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path index c49e7575..1dc258af 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index d7c310f7..6e0e7a04 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path index 92d5e94a..346c462c 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 494fdcd0..9659b6b7 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path index df076b12..d38814e1 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index ea04faa0..1efb93e8 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path index ce9739ef..28d4129f 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path index e55937bf..f6ea1247 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path index 07b9ef18..39463d71 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto1.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 73e7eeae..03ff90be 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path index d48e03d7..0eec812b 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path index df23583e..8eca5b53 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path index 36b4ba51..78f47219 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path @@ -39,7 +39,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path index 9a52036f..2ead400f 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index 70c21f7c..03f482fe 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index e3022f5a..252e0233 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index 74572668..d891d5fe 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index d77f0a87..8fb931a8 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path index 6cb73ad6..1d925fff 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index d25362c7..3885e2e6 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path index 27d6c328..dc3edb94 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index 16c7723a..5213f2ed 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path index 662cf162..6a5d1442 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index 23b90c92..e3a502e0 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path index 390a55c8..ff3435a8 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path index f56f14ab..f42b13ad 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index bc1afd4c..f5b5042a 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path index a3755aaf..aa0378bd 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index 694556d2..a56d138a 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -63,7 +63,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path index a00d39d5..9262147a 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index 61abcdcb..69995291 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path index 463d6abc..e46d4fc0 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/test1.path b/src/main/deploy/pathplanner/paths/test1.path index 2e2b91e2..b8d8c2c6 100644 --- a/src/main/deploy/pathplanner/paths/test1.path +++ b/src/main/deploy/pathplanner/paths/test1.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/deploy/pathplanner/paths/test2.path b/src/main/deploy/pathplanner/paths/test2.path index df6d4e9d..6815b66d 100644 --- a/src/main/deploy/pathplanner/paths/test2.path +++ b/src/main/deploy/pathplanner/paths/test2.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index abdbb785..587b0de5 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; +import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.Pitcher; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; @@ -76,8 +77,6 @@ private void bindControllerCommands() { OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); -// OperatorConstants.ALIGN_TO_AMP_TRIGGER.whileTrue(CommandConstants.FACE_AMP_COMMAND); - OperatorConstants.ALIGN_TO_SPEAKER_TRIGGER.whileTrue(CommandConstants.FACE_SPEAKER_COMMAND); OperatorConstants.ALIGN_TO_RIGHT_STAGE.whileTrue(CommandConstants.ALIGN_TO_RIGHT_STAGE_COMMAND); OperatorConstants.ALIGN_TO_LEFT_STAGE.whileTrue(CommandConstants.ALIGN_TO_LEFT_STAGE_COMMAND); OperatorConstants.ALIGN_TO_MIDDLE_STAGE.whileTrue(CommandConstants.ALIGN_TO_MIDDLE_STAGE_COMMAND); @@ -87,6 +86,7 @@ private void bindControllerCommands() { OperatorConstants.COLLECT_NOTE_TRIGGER.whileTrue(GeneralCommands.getNoteCollectionCommand()); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); + OperatorConstants.LED_AUTO_SETUP_TRIGGER.whileTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); OperatorConstants.CLIMB_TRIGGER.whileTrue(GeneralCommands.getClimbCommand()); @@ -107,7 +107,7 @@ private void bindControllerCommands() { OperatorConstants.AMP_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(true)); OperatorConstants.AMP_WITHOUT_ALIGN_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(false)); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); - OperatorConstants.LOWER_AMP_ALIGNER_TRIGGER.whileTrue(AmpAlignerCommands.getSetTargetVoltageCommand(AmpAlignerConstants.LOWER_AMP_ALIGNER_VOLTAGE)); + OperatorConstants.REALIGN_AMP_ALIGNER_TRIGGER.whileTrue(AmpAlignerCommands.getSetPositionCommand(AmpAlignerConstants.REALIGN_AMP_ALIGNER_ANGLE)); OperatorConstants.BLOCK_TRIGGER.whileTrue(AmpCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 1537bbd6..5c9091e3 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -16,6 +16,7 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.trigon.hardware.RobotHardwareStats; +import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.MirrorableRotation2d; public class AlignToNoteCommand extends ParallelCommandGroup { @@ -23,12 +24,11 @@ public class AlignToNoteCommand extends ParallelCommandGroup { private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.0075, 0, 0) : new PIDController(0.0002, 0, 0); - private MirrorableRotation2d targetAngle = null; public AlignToNoteCommand() { addCommands( getSetCurrentLEDColorCommand(), - new InstantCommand(this::resetTargetAngle).andThen(GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND).until(this::shouldAlignToNote).andThen(getDriveWhileAligningToNoteCommand())).asProxy(), + GeneralCommands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), GeneralCommands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::shouldAlignToNote).asProxy(), new RunCommand(CAMERA::trackObject) ); } @@ -38,28 +38,31 @@ private Command getSetCurrentLEDColorCommand() { LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS), LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), CAMERA::hasTargets - ); + ).asProxy(); } private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> CommandConstants.calculateDriveStickAxisValue(getScaledJoystickValue()), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), - () -> targetAngle + this::getTargetAngle ); } - private void calculateTargetAngle() { + private MirrorableRotation2d getTargetAngle() { final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - targetAngle = new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); + return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); } private boolean shouldAlignToNote() { - calculateTargetAngle(); - return /*CAMERA.hasTargets()*/targetAngle.get() != null && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); + return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } - private void resetTargetAngle() { - targetAngle = null; + private double getScaledJoystickValue() { + final XboxController controller = OperatorConstants.DRIVER_CONTROLLER; + final Rotation2d robotRelativeToFieldRelativeRotation = Rotation2d.fromDegrees(90); + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().plus(robotRelativeToFieldRelativeRotation); + + return controller.getLeftX() * -robotHeading.getCos() - controller.getLeftY() * -robotHeading.getSin(); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 1f6dfb20..a72c3e5d 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -46,7 +46,7 @@ public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy(), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) + ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).until(() -> !RobotContainer.INTAKE.hasNote()) ); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 29228b13..8d3f49d3 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -43,7 +43,7 @@ public static Command getNoteCollectionCommand() { new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - ShooterCommands.getSendStaticBreakRequestCommand().until(RobotContainer.INTAKE::hasNote).andThen(ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).withTimeout(0.1)) + ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).onlyIf(() -> !RobotContainer.INTAKE.hasNote()) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java index e08927a4..31411379 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/ShootingCommands.java @@ -91,8 +91,7 @@ private static Command getFeedNoteForShootingCommand() { .alongWith(GeneralCommands.getVisualizeNoteShootingCommand()), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch() && - RobotContainer.SWERVE.atAngle(SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle()), - 0.2 + RobotContainer.SWERVE.atAngle(SHOOTING_CALCULATIONS.getTargetShootingState().targetRobotAngle()) ); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 46ecd47d..88b1cbd5 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -2,6 +2,7 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -13,16 +14,16 @@ import java.util.HashMap; public class FieldConstants { - // public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; - - static { - try { - APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); - } catch (IOException e) { - throw new RuntimeException(e); - } - } + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); +// public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; +// +// static { +// try { +// APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); +// } catch (IOException e) { +// throw new RuntimeException(e); +// } +// } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index e648f716..9de54764 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -21,11 +21,10 @@ public class OperatorConstants { public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), - SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r().or(DRIVER_CONTROLLER.a()), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER = DRIVER_CONTROLLER.b(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), - ALIGN_TO_SPEAKER_TRIGGER = DRIVER_CONTROLLER.a(), ALIGN_TO_RIGHT_STAGE = OPERATOR_CONTROLLER.j(), ALIGN_TO_LEFT_STAGE = OPERATOR_CONTROLLER.h(), ALIGN_TO_MIDDLE_STAGE = OPERATOR_CONTROLLER.u(), @@ -56,7 +55,8 @@ public class OperatorConstants { AMP_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.x()), AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), - LOWER_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), BLOCK_TRIGGER = OPERATOR_CONTROLLER.b(), + REALIGN_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), + RESET_AMP_ALIGNER_POSITION_TRIGGER = OPERATOR_CONTROLLER.two(), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 1b5620fd..20322615 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -11,7 +11,7 @@ public class ShootingConstants { SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, - SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 3; + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 0.8; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java index b2bfccc5..2f205135 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java @@ -69,6 +69,10 @@ public void updateMechanism() { PitcherConstants.PITCHER_AND_AMP_ALIGNER_MECHANISM.setSecondJointCurrentAngle(getCurrentAngle()); } + public void setPosition(Rotation2d position) { + motor.setPosition(position.getRotations()); + } + public AmpAlignerConstants.AmpAlignerState getTargetState() { return targetState; } @@ -109,7 +113,7 @@ void setTargetAngle(Rotation2d targetAngle) { } private void configurePositionResettingTrigger() { - final Trigger hitLimitSwitchTrigger = new Trigger(this::hasHitForwardLimit).debounce(AmpAlignerConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS); + final Trigger hitLimitSwitchTrigger = new Trigger(this::hasHitForwardLimit).debounce(AmpAlignerConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS).or(OperatorConstants.RESET_AMP_ALIGNER_POSITION_TRIGGER); hitLimitSwitchTrigger.onTrue(new InstantCommand(() -> motor.setPosition(AmpAlignerConstants.LIMIT_SWITCH_PRESSED_ANGLE.getRotations())).ignoringDisable(true)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java index 733babd2..7205e9a5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerCommands.java @@ -4,6 +4,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; import org.trigon.commands.ExecuteEndCommand; @@ -40,4 +41,10 @@ public static Command getSetTargetVoltageCommand(double voltage) { RobotContainer.AMP_ALIGNER::stop ); } + + public static Command getSetPositionCommand(Rotation2d position) { + return new InstantCommand( + () -> RobotContainer.AMP_ALIGNER.setPosition(position) + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java index 29996579..6709371f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAlignerConstants.java @@ -65,7 +65,7 @@ public class AmpAlignerConstants { public static final Transform3d PITCHER_TO_AMP_ALIGNER = new Transform3d(-0.4838, 0, 0.1472, new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(6.3), 0)); - public static final double LOWER_AMP_ALIGNER_VOLTAGE = -1; + public static final Rotation2d REALIGN_AMP_ALIGNER_ANGLE = Rotation2d.fromDegrees(5); static final Rotation2d READY_FOR_DEFAULT_PITCHER_MOVEMENT_ANGLE = Rotation2d.fromDegrees(80); static final Rotation2d LIMIT_SWITCH_PRESSED_ANGLE = Rotation2d.fromDegrees(173.7); static final double LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS = 0.01; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 7d63a8da..3961c6f7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -41,7 +41,7 @@ private static Command getCollectionCommand() { if (!interrupted) { RobotContainer.INTAKE.indicateCollection(); // RobotContainer.INTAKE.sendStaticBrakeRequest() - getCorrectNotePositionCommand().andThen(getStopIntakeCommand()); + getCorrectNotePositionCommand().schedule(); } }, RobotContainer.INTAKE::hasNote, @@ -59,6 +59,6 @@ private static Command getStopIntakeCommand() { private static Command getCorrectNotePositionCommand() { - return getSetTargetStateCommand(IntakeConstants.IntakeState.CORRECT_NOTE_POSITION).onlyWhile(RobotContainer.INTAKE::hasNote); + return getSetTargetStateCommand(IntakeConstants.IntakeState.CORRECT_NOTE_POSITION).onlyWhile(RobotContainer.INTAKE::hasNote).andThen(getStopIntakeCommand()); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index d8ed4e74..dc3c7227 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -135,7 +135,7 @@ public enum IntakeState { STOP(0), FEED_SHOOTING(8), FEED_AMP(5), - CORRECT_NOTE_POSITION(-1.2); + CORRECT_NOTE_POSITION(-1.1); public final double voltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 2b1d04d6..2853f44e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -22,9 +22,9 @@ public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_YAW = 268.093872, - GYRO_MOUNT_POSITION_PITCH = 0.263672, - GYRO_MOUNT_POSITION_ROLL = 0.219727; + GYRO_MOUNT_POSITION_YAW = 86.152039 , + GYRO_MOUNT_POSITION_PITCH = 0.087891 , + GYRO_MOUNT_POSITION_ROLL = -0.351562 ; private static final double FRONT_LEFT_STEER_ENCODER_OFFSET = -1.561, FRONT_RIGHT_STEER_ENCODER_OFFSET = -3.431, From 927d4899b5eaa52602dc1852986d61fde4b46319 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 06:14:56 +0300 Subject: [PATCH 186/261] comp day --- .../java/frc/trigon/robot/commands/AlignToNoteCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 5c9091e3..e5f9606a 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -60,8 +60,8 @@ private boolean shouldAlignToNote() { private double getScaledJoystickValue() { final XboxController controller = OperatorConstants.DRIVER_CONTROLLER; - final Rotation2d robotRelativeToFieldRelativeRotation = Rotation2d.fromDegrees(90); - final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().plus(robotRelativeToFieldRelativeRotation); + final var x = Rotation2d.fromDegrees(90); + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().plus(x); return controller.getLeftX() * -robotHeading.getCos() - controller.getLeftY() * -robotHeading.getSin(); } From 865e4971a3ab5193ab43fbaa339c899ddf317fd2 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 06:24:46 +0300 Subject: [PATCH 187/261] ahhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh --- .pathplanner/settings.json | 6 +++--- src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto | 2 +- .../pathplanner/autos/FrontRow4NoteAutoFarShoot.auto | 2 +- .../autos/FrontRow4NoteAutoFarShootVarient.auto | 2 +- .../java/frc/trigon/robot/subsystems/intake/Intake.java | 7 ------- 5 files changed, 6 insertions(+), 13 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 89a4dc01..2607e05f 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -18,11 +18,11 @@ ], "autoFolders": [ "3NoteAutoThroughAmpVarients", - "Backup", + "frontRow", "FarNotesAroundStageVarients", - "misc", "OtherAutos", - "SourceSide3NoteAutoVarients" + "SourceSide3NoteAutoVarients", + "misc" ], "defaultMaxVel": 4.0, "defaultMaxAccel": 4.0, diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index 87608a29..9d7cfa37 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -378,6 +378,6 @@ ] } }, - "folder": "Backup", + "folder": "frontRow", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto index 9b41a4cb..e1f69ef9 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto @@ -360,6 +360,6 @@ ] } }, - "folder": "Backup", + "folder": "frontRow", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto index 34cd5057..ac715aa8 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto @@ -354,6 +354,6 @@ ] } }, - "folder": "Backup", + "folder": "frontRow", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index e59770e4..0910a0c6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -23,7 +23,6 @@ public class Intake extends MotorSubsystem { public Intake() { setName("Intake"); - configureLEDsTrigger(); } @Override @@ -103,10 +102,4 @@ private Command getFeedingIndicationLEDsCommand() { private Command getEjectingIndicationLEDsCommand() { return LEDStripCommands.getBreatheCommand(Color.kBlue, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, false, LEDStrip.LED_STRIPS); } - - private void configureLEDsTrigger() { - Trigger trigger = new Trigger(this::hasNote); - trigger.onTrue(new InstantCommand(() -> LEDStrip.changeDefaultCommandForAllLEDs(LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS)))); - trigger.onFalse(new InstantCommand(() -> LEDStrip.changeDefaultCommandForAllLEDs(LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS)))); - } } \ No newline at end of file From f5a89be4b5bd0b287e0d1ad19b505da45ceb462e Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 09:43:41 +0300 Subject: [PATCH 188/261] Yohay just broke a cable!!!!!!!!!!!!!! --- .../trigon/robot/commands/factories/AutonomousCommands.java | 3 ++- .../frc/trigon/robot/constants/AutonomousConstants.java | 1 + .../java/frc/trigon/robot/constants/CameraConstants.java | 2 +- .../java/frc/trigon/robot/constants/FieldConstants.java | 2 +- .../java/frc/trigon/robot/constants/ShootingConstants.java | 2 +- .../trigon/robot/subsystems/climber/ClimberConstants.java | 6 +++--- .../java/frc/trigon/robot/subsystems/intake/Intake.java | 2 ++ 7 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index a72c3e5d..a0a3a77f 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; @@ -54,7 +55,7 @@ public static Command getFeedNoteCommand() { return new ParallelCommandGroup( IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_SHOOTING), GeneralCommands.getVisualizeNoteShootingCommand() - ).withTimeout(0.7); + ).withTimeout(AutonomousConstants.AUTONOMOUS_FEEDING_TIME_SECONDS); } public static Command getAlignToNoteCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index eeeda3eb..4da84fcd 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -7,6 +7,7 @@ public class AutonomousConstants { public static final PathConstraints REAL_TIME_CONSTRAINTS = new PathConstraints(2.5, 2.5, 4, 4); + public static final double AUTONOMOUS_FEEDING_TIME_SECONDS = 0.7; static { registerCommands(); diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index a6b31c03..814dac99 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -10,7 +10,7 @@ public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.0002, + TRANSLATIONS_STD_EXPONENT = 0.001, THETA_STD_EXPONENT = 0.01; private static final Transform3d diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 88b1cbd5..1546624b 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -33,7 +33,7 @@ public class FieldConstants { SPEAKER_TAG_ID = 7, AMP_TAG_ID = 6; private static final Translation3d - SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.79), + SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.82), AMP_TAG_TO_AMP = new Translation3d(0, 0.03, -0.32); public static final MirrorableTranslation3d SPEAKER_TRANSLATION = new MirrorableTranslation3d(TAG_ID_TO_POSE.get(SPEAKER_TAG_ID).getTranslation().plus(SPEAKER_TAG_TO_SPEAKER), true), diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index 20322615..cab6b75c 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -11,7 +11,7 @@ public class ShootingConstants { SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, - SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 0.8; + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 0.5; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index 35f53c4e..f08b9125 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -33,21 +33,21 @@ public class ClimberConstants { LEFT_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; private static final NeutralModeValue NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; static final boolean ENABLE_FOC = true; - private static final double //TODO: calibrate + private static final double LEFT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 50, LEFT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, LEFT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.078964, LEFT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9056, LEFT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.18439; - private static final double //TODO: calibrate + private static final double RIGHT_GROUNDED_P = RobotHardwareStats.isSimulation() ? 800 : 50, RIGHT_GROUNDED_I = RobotHardwareStats.isSimulation() ? 0 : 0, RIGHT_GROUNDED_D = RobotHardwareStats.isSimulation() ? 0 : 0, RIGHT_GROUNDED_KS = RobotHardwareStats.isSimulation() ? 0.0045028 : 0.079947, RIGHT_GROUNDED_KV = RobotHardwareStats.isSimulation() ? 8.792 : 7.9986, RIGHT_GROUNDED_KA = RobotHardwareStats.isSimulation() ? 0.17809 : 0.21705; - private static final double //TODO: calibrate + private static final double ON_CHAIN_P = RobotHardwareStats.isSimulation() ? RIGHT_GROUNDED_P : 1, ON_CHAIN_I = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_I : 0, ON_CHAIN_D = RobotHardwareStats.isSimulation() ? LEFT_GROUNDED_D : 0, diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 0910a0c6..b347937a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; @@ -29,6 +30,7 @@ public Intake() { public void updatePeriodically() { masterMotor.update(); IntakeConstants.DISTANCE_SENSOR.updateSensor(); + Logger.recordOutput("ShouldAlignToNote", CommandConstants.SHOULD_ALIGN_TO_NOTE); Logger.recordOutput("HasNote", hasNote()); Logger.recordOutput("distanceSensorScaledValue", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); } From 4183255ee128670c57426e628046330d33b6a05d Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 13:16:59 +0300 Subject: [PATCH 189/261] frsda --- src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto | 2 +- .../deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto | 2 +- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path | 6 +++--- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path | 6 +++--- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path | 6 +++--- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path | 6 +++--- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path | 6 +++--- src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path | 6 +++--- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 2 +- .../robot/subsystems/swerve/SwerveModuleConstants.java | 4 ++-- 11 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto index 9d7cfa37..ff4fe261 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAuto.auto @@ -42,7 +42,7 @@ { "type": "wait", "data": { - "waitTime": 0.5 + "waitTime": 0.6 } }, { diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index fbb9bb65..1c290542 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -49,7 +49,7 @@ { "type": "wait", "data": { - "waitTime": 0.4 + "waitTime": 0.8 } }, { diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index d4424e88..040a2af1 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index 347e9733..4617c352 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index db7b0122..d93c519e 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index 966f4fb4..e68cebd2 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index 6e94dfb1..e1ae87c6 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index 9e7f1fd2..2d3216d7 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 9de54764..50a528b2 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -56,7 +56,7 @@ public class OperatorConstants { AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), BLOCK_TRIGGER = OPERATOR_CONTROLLER.b(), - REALIGN_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.one(), - RESET_AMP_ALIGNER_POSITION_TRIGGER = OPERATOR_CONTROLLER.two(), + REALIGN_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.equals(), + RESET_AMP_ALIGNER_POSITION_TRIGGER = OPERATOR_CONTROLLER.minus(), RESET_POSE_TO_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index c4ffcdde..3135f5db 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -29,7 +29,7 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); - if (inputs.hasResult) + if (inputs.hasResult && latestResult.getBestTarget().poseAmbiguity < 0.4) updateHasResultInputs(inputs, latestResult); else updateNoResultInputs(inputs); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index d5c728c9..204be9ad 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -25,8 +25,8 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 100, - STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 50; + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 60, + STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 30; private static final double STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, STEER_MOTOR_I = 0, From cad175392e3c66aaddc46f825529baf7136237fd Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 15:14:07 +0300 Subject: [PATCH 190/261] 17 --- .../io/AprilTagPhotonCameraIO.java | 28 +++++++++++++------ 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 3135f5db..b5754908 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -14,6 +14,7 @@ import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; import java.util.List; @@ -29,7 +30,7 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); - if (inputs.hasResult && latestResult.getBestTarget().poseAmbiguity < 0.4) + if (inputs.hasResult && getBestTarget(latestResult).poseAmbiguity < 0.4) updateHasResultInputs(inputs, latestResult); else updateNoResultInputs(inputs); @@ -68,7 +69,7 @@ private Point getTagCenter(List tagCorners) { * @return the estimated rotation */ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - final List tagCorners = result.getBestTarget().getDetectedCorners(); + final List tagCorners = getBestTarget(result).getDetectedCorners(); final Point tagCenter = getTagCenter(tagCorners); if (photonCamera.getCameraMatrix().isPresent()) return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); @@ -87,23 +88,32 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()); + final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(getBestTarget(result).getFiducialId()); final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); Logger.recordOutput("TagPose", tagPose); - final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); + final Transform3d targetToCamera = getBestTarget(result).getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } private int[] getVisibleTagIDs(PhotonPipelineResult result) { - final int[] visibleTagIDs = new int[result.getTargets().size()]; +// final int[] visibleTagIDs = new int[result.getTargets().size()]; +// for (int i = 0; i < visibleTagIDs.length; i++) +// visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); +// return visibleTagIDs; + return new int[]{getBestTarget(result).getFiducialId()}; + } - for (int i = 0; i < visibleTagIDs.length; i++) - visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); - return visibleTagIDs; + private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { + var x = result.getBestTarget(); + for (PhotonTrackedTarget target : result.getTargets()) { + if (target.getArea() > x.area) + x = target; + } + return x; } private double getDistanceFromBestTag(PhotonPipelineResult result) { - return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); + return getBestTarget(result).getBestCameraToTarget().getTranslation().getNorm(); } private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { From 093f18a5ba28e9a68320da9110014dbe1ae9834e Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 17:52:05 +0300 Subject: [PATCH 191/261] After camera fix --- build.gradle | 2 +- .../pathplanner/paths/SourceSide3NoteAutoRight2.path | 8 ++++---- .../pathplanner/paths/SourceSide3NoteAutoRight3.path | 10 +++++----- .../pathplanner/paths/SourceSide3NoteAutoRight4.path | 12 ++++++------ .../pathplanner/paths/SourceSide3NoteAutoRight5.path | 8 ++++---- src/main/java/frc/trigon/robot/RobotContainer.java | 3 +++ .../trigon/robot/constants/ShootingConstants.java | 5 +++-- .../frc/trigon/robot/misc/ShootingCalculations.java | 2 +- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 4 ++-- .../poseestimator/PoseEstimatorConstants.java | 2 +- .../frc/trigon/robot/subsystems/pitcher/Pitcher.java | 3 +++ .../robot/subsystems/pitcher/PitcherConstants.java | 2 ++ 12 files changed, 35 insertions(+), 26 deletions(-) diff --git a/build.gradle b/build.gradle index ca4b07cb..7341a1d4 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:v2024.2.12' + implementation 'com.github.Programming-TRIGON:TRIGONLib:debugging-SNAPSHOT' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index 03f482fe..a2c31e45 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.284709527165534, - "y": 0.7804612792324187 + "x": 8.281950694955519, + "y": 0.6899551121039572 }, "prevControl": { - "x": 6.242306831407013, - "y": 1.296717842841861 + "x": 6.239547999196998, + "y": 1.2062116757133996 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 252e0233..a157b23d 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.284709527165534, - "y": 0.7804612792324187 + "x": 8.281950694955519, + "y": 0.6899551121039572 }, "prevControl": null, "nextControl": { - "x": 4.830445717229463, - "y": 2.0523241046482106 + "x": 4.827686885019448, + "y": 1.961817937519749 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightFirstCollection" @@ -45,7 +45,7 @@ "reversed": false, "folder": "SourceSide3NoteAutoRight", "previewStartingState": { - "rotation": -21.0, + "rotation": -29.744881296942257, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index d891d5fe..b38e2e7d 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.0038815570588895, - "y": 1.560922558464836 + "x": 5.042669914399658, + "y": 1.2863569886683932 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightSecondFeed" }, { "anchor": { - "x": 8.270256540513081, - "y": 2.442554744264419 + "x": 8.293644849397962, + "y": 2.385607506257749 }, "prevControl": { - "x": 6.2179324358648715, - "y": 1.5753755451172882 + "x": 6.317332748625612, + "y": 1.217715596219285 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 8fb931a8..872d82f3 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.270256540513081, - "y": 2.442554744264419 + "x": 8.293644849397962, + "y": 2.385607506257749 }, "prevControl": null, "nextControl": { - "x": 6.333556329084489, - "y": 1.474204638550123 + "x": 6.35694463796937, + "y": 1.417257400543453 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightSecondCollection" diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 587b0de5..00dbe81f 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.Pitcher; +import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; @@ -111,6 +112,8 @@ private void bindControllerCommands() { OperatorConstants.BLOCK_TRIGGER.whileTrue(AmpCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); + + OperatorConstants.OPERATOR_CONTROLLER.t().whileTrue(PitcherCommands.getDebuggingCommand()); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index cab6b75c..dcce3377 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -4,14 +4,15 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, - FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, - SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 0.5; + FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; + public static final LoggedDashboardNumber SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = new LoggedDashboardNumber("SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND", 3); public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index 48fb0ef6..d89c9a9e 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -137,7 +137,7 @@ private TargetShootingState calculateTargetShootingState(TargetShootingState sta private TargetShootingState calculateTargetShootingState(Translation3d shootingVector) { final MirrorableRotation2d targetRobotAngle = new MirrorableRotation2d(getYaw(shootingVector), false); final Rotation2d targetPitch = getPitch(shootingVector); - final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()) + ShootingConstants.SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND; + final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()) + ShootingConstants.SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND.get(); return new TargetShootingState(targetRobotAngle, targetPitch, targetVelocity); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index b5754908..674f2a17 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -29,8 +29,8 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); - if (inputs.hasResult && getBestTarget(latestResult).poseAmbiguity < 0.4) + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getBestTarget(latestResult).poseAmbiguity < 0.4; + if (inputs.hasResult) updateHasResultInputs(inputs, latestResult); else updateNoResultInputs(inputs); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index 72d74799..d8967b54 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.numbers.N3; public class PoseEstimatorConstants { - public static final double ODOMETRY_FREQUENCY_HERTZ = 50; + public static final double ODOMETRY_FREQUENCY_HERTZ = 250; /** * The vector represents how ambiguous each value of the odometry is. diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index 58d7693a..2684f43b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -57,6 +57,9 @@ public void stop() { @Override public void updatePeriodically() { masterMotor.update(); + followerMotor.update(); + Logger.recordOutput("MasterPitcherMotorVotlage", masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); + Logger.recordOutput("FollowerPitcherMotorVoltage", followerMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); encoder.update(); Logger.recordOutput("PitcherAngleDegrees", Conversions.rotationsToDegrees(encoder.getSignal(CANcoderSignal.POSITION))); } diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index 15ec8982..4714aaf4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -168,6 +168,8 @@ private static void configureFollowerMotor() { final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); FOLLOWER_MOTOR.setControl(followerRequest); + + FOLLOWER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); } public static void configureEncoder() { From c41c83398f30447db621c635e24c14fb60285cc8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 19:22:38 +0300 Subject: [PATCH 192/261] Switched driver amp button to indicate LEDs --- src/main/java/frc/trigon/robot/RobotContainer.java | 4 ++-- .../java/frc/trigon/robot/constants/OperatorConstants.java | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 00dbe81f..d2fdfe39 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,6 +6,7 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; @@ -89,6 +90,7 @@ private void bindControllerCommands() { OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); OperatorConstants.LED_AUTO_SETUP_TRIGGER.whileTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); + OperatorConstants.LED_INDICATION_TRIGGER.onTrue(LEDStripCommands.getBlinkingCommand(Color.kBlue, 0.25, LEDStrip.LED_STRIPS).withTimeout(1)); OperatorConstants.CLIMB_TRIGGER.whileTrue(GeneralCommands.getClimbCommand()); OperatorConstants.MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_DOWN_MANUALLY_COMMAND); @@ -112,8 +114,6 @@ private void bindControllerCommands() { OperatorConstants.BLOCK_TRIGGER.whileTrue(AmpCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); - - OperatorConstants.OPERATOR_CONTROLLER.t().whileTrue(PitcherCommands.getDebuggingCommand()); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 50a528b2..f2406a59 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -6,8 +6,7 @@ import org.trigon.hardware.misc.XboxController; public class OperatorConstants { - private static final int - DRIVER_CONTROLLER_PORT = 0; + private static final int DRIVER_CONTROLLER_PORT = 0; private static final int DRIVER_CONTROLLER_EXPONENT = 1; private static final double DRIVER_CONTROLLER_DEADBAND = 0; public static final XboxController DRIVER_CONTROLLER = new XboxController( @@ -31,6 +30,7 @@ public class OperatorConstants { TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER = OPERATOR_CONTROLLER.o(), TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER = OPERATOR_CONTROLLER.p(), LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), + LED_INDICATION_TRIGGER = DRIVER_CONTROLLER.x(), CLIMB_TRIGGER = OPERATOR_CONTROLLER.c(), OVERRIDE_IS_CLIMBING_TRIGGER = OPERATOR_CONTROLLER.i(), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), @@ -52,7 +52,7 @@ public class OperatorConstants { WARM_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.w().and(SPEAKER_SHOT_TRIGGER.negate()), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m(), - AMP_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.x()), + AMP_TRIGGER = OPERATOR_CONTROLLER.a(), AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), BLOCK_TRIGGER = OPERATOR_CONTROLLER.b(), From d2adb53e50ac114bef37ca543e6d11bf32c0b924 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Sun, 20 Oct 2024 22:17:58 +0300 Subject: [PATCH 193/261] added improved auto and minor fix for @Strflightmight09 :). FINALS TOMMOROW!!! --- .../SourceSide3NoteAutoRightImproved.auto | 282 ++++++++++++++++++ .../robot/commands/CommandConstants.java | 2 +- 2 files changed, 283 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto new file mode 100644 index 00000000..1f621a71 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto @@ -0,0 +1,282 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6946944060158917, + "y": 4.426390230452671 + }, + "rotation": -60.10109816138545 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShooting" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAutoRight5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } + } + ] + } + }, + "folder": "SourceSide3NoteAutoVarients", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 2896b180..1ab4795c 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -27,7 +27,7 @@ import org.trigon.utilities.mirrorable.MirrorableRotation2d; public class CommandConstants { - public static boolean SHOULD_ALIGN_TO_NOTE = true; + public static boolean SHOULD_ALIGN_TO_NOTE = false; private static final XboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; private static final double MINIMUM_TRANSLATION_SHIFT_POWER = 0.18, From 1dff2ebb793c85952504fb9fff922410ee5ee325 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 21 Oct 2024 07:28:01 +0300 Subject: [PATCH 194/261] FINAL COMP DAY --- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 2 +- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 814dac99..4ed6440e 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -10,7 +10,7 @@ public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.001, + TRANSLATIONS_STD_EXPONENT = 0.002, THETA_STD_EXPONENT = 0.01; private static final Transform3d diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 674f2a17..182eae61 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -29,7 +29,7 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getBestTarget(latestResult).poseAmbiguity < 0.4; + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getBestTarget(latestResult).poseAmbiguity < 0.5; if (inputs.hasResult) updateHasResultInputs(inputs, latestResult); else From b732bf0992054887c25f068daf9f89e9785d1c42 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 21 Oct 2024 12:24:43 +0300 Subject: [PATCH 195/261] fix --- .../autos/SourceSide3NoteAutoRight.auto | 19 ------------------- .../pathplanner/paths/FrontRow4NoteAuto5.path | 4 ++-- .../pathplanner/paths/Zayde3NoteAuto2.path | 6 ------ .../factories/AutonomousCommands.java | 3 +-- .../robot/constants/CameraConstants.java | 2 +- .../robot/constants/RobotConstants.java | 2 +- .../subsystems/intake/IntakeCommands.java | 3 +-- .../subsystems/intake/IntakeConstants.java | 2 +- .../subsystems/swerve/SwerveConstants.java | 2 +- 9 files changed, 8 insertions(+), 35 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index 1c290542..dc0d449f 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -68,25 +68,6 @@ "type": "race", "data": { "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - }, { "type": "sequential", "data": { diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path index e1ae87c6..122b6ac8 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 2.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0 @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index f5b5042a..49011106 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index a0a3a77f..8fc69142 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -45,9 +45,8 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( - LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy(), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).until(() -> !RobotContainer.INTAKE.hasNote()) + ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND) ); } diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 4ed6440e..be2ebac8 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -10,7 +10,7 @@ public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.002, + TRANSLATIONS_STD_EXPONENT = 0.02, THETA_STD_EXPONENT = 0.01; private static final Transform3d diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index dda4e7a0..93fa6a7e 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -7,7 +7,7 @@ public class RobotConstants { private static final boolean IS_SIMULATION = false, - IS_REPLAY = false; + IS_REPLAY = true; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "SwerveCANivore"; public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 3961c6f7..9bdea653 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -15,7 +15,7 @@ public static Command getDebuggingCommand() { public static Command getSetTargetStateCommand(IntakeConstants.IntakeState targetState) { if (targetState == IntakeConstants.IntakeState.COLLECT) - return getCollectionCommand(); + return getCollectionCommand().andThen(getCorrectNotePositionCommand()); return new StartEndCommand( () -> RobotContainer.INTAKE.setTargetState(targetState), RobotContainer.INTAKE::stop, @@ -41,7 +41,6 @@ private static Command getCollectionCommand() { if (!interrupted) { RobotContainer.INTAKE.indicateCollection(); // RobotContainer.INTAKE.sendStaticBrakeRequest() - getCorrectNotePositionCommand().schedule(); } }, RobotContainer.INTAKE::hasNote, diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index dc3c7227..c506dca3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -133,7 +133,7 @@ public enum IntakeState { COLLECT(4.5), EJECT(-2), STOP(0), - FEED_SHOOTING(8), + FEED_SHOOTING(7), FEED_AMP(5), CORRECT_NOTE_POSITION(-1.1); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 2853f44e..3debee58 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -80,7 +80,7 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0.1) : - new PIDConstants(0, 0, 0.1), + new PIDConstants(2, 0, 0.1), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(2.5, 0, 0.2) : new PIDConstants(2, 0, 0); From a8985712e7ecebdcba135df86c93eef185cdb4e8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 21 Oct 2024 12:44:18 +0300 Subject: [PATCH 196/261] Gave me (Ezra) manual delivery button --- .../paths/SourceSide3NoteAutoRight2.path | 16 ++++++++-------- .../paths/SourceSide3NoteAutoRight3.path | 16 ++++++++-------- .../paths/SourceSide3NoteAutoRight4.path | 16 ++++++++-------- .../paths/SourceSide3NoteAutoRight5.path | 16 ++++++++-------- .../java/frc/trigon/robot/RobotContainer.java | 2 +- .../robot/constants/OperatorConstants.java | 4 ++-- 6 files changed, 35 insertions(+), 35 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index a2c31e45..a70afb7a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 4.107109781915527, - "y": 1.6672086550823948 + "x": 4.036972632349823, + "y": 1.239580370898635 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoStartPoint" }, { "anchor": { - "x": 8.281950694955519, - "y": 0.6899551121039572 + "x": 8.293644849397962, + "y": 2.385607506257749 }, "prevControl": { - "x": 6.239547999196998, - "y": 1.2062116757133996 + "x": 5.9080373431402124, + "y": 1.0641680542620358 }, "nextControl": null, "isLocked": false, - "linkedName": "SourceSide3NoteAutoRightFirstCollection" + "linkedName": "SourceSide3NoteAutoRightSecondCollection" } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -21.447736327105403, + "rotation": 33.340707346477025, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index a157b23d..2552d136 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 8.281950694955519, - "y": 0.6899551121039572 + "x": 8.293644849397962, + "y": 2.385607506257749 }, "prevControl": null, "nextControl": { - "x": 4.827686885019448, - "y": 1.961817937519749 + "x": 5.416882856557735, + "y": 0.7835083476434771 }, "isLocked": false, - "linkedName": "SourceSide3NoteAutoRightFirstCollection" + "linkedName": "SourceSide3NoteAutoRightSecondCollection" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 2.71716149066101 }, "prevControl": { - "x": 3.6019418517710275, - "y": 2.5003666908742277 + "x": 4.270855721198622, + "y": 1.777511475250871 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": "SourceSide3NoteAutoRight", "previewStartingState": { - "rotation": -29.744881296942257, + "rotation": 35.706691400603006, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index b38e2e7d..4f69517d 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 5.042669914399658, - "y": 1.2863569886683932 + "x": 5.370106238788276, + "y": 1.7892056296974757 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightSecondFeed" }, { "anchor": { - "x": 8.293644849397962, - "y": 2.385607506257749 + "x": 8.281950694955519, + "y": 0.6899551121039572 }, "prevControl": { - "x": 6.317332748625612, - "y": 1.217715596219285 + "x": 6.492745065262509, + "y": 1.3448277608847596 }, "nextControl": null, "isLocked": false, - "linkedName": "SourceSide3NoteAutoRightSecondCollection" + "linkedName": "SourceSide3NoteAutoRightFirstCollection" } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 22.1663458220825, + "rotation": -22.619864947542787, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 872d82f3..46f4ea6b 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 8.293644849397962, - "y": 2.385607506257749 + "x": 8.281950694955519, + "y": 0.6899551121039572 }, "prevControl": null, "nextControl": { - "x": 6.35694463796937, - "y": 1.417257400543453 + "x": 5.919731497582653, + "y": 1.4383809964201122 }, "isLocked": false, - "linkedName": "SourceSide3NoteAutoRightSecondCollection" + "linkedName": "SourceSide3NoteAutoRightFirstCollection" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 2.746067463965915 }, "prevControl": { - "x": 4.758180783967202, - "y": 1.5753755451172882 + "x": 4.832175134435738, + "y": 1.8125939385781908 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": "SourceSide3NoteAutoRight", "previewStartingState": { - "rotation": 37.87498365109831, + "rotation": -20.376435213836338, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index d2fdfe39..0620b08a 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -90,7 +90,7 @@ private void bindControllerCommands() { OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); OperatorConstants.LED_AUTO_SETUP_TRIGGER.whileTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); - OperatorConstants.LED_INDICATION_TRIGGER.onTrue(LEDStripCommands.getBlinkingCommand(Color.kBlue, 0.25, LEDStrip.LED_STRIPS).withTimeout(1)); +// OperatorConstants.LED_INDICATION_TRIGGER.onTrue(LEDStripCommands.getBlinkingCommand(Color.kBlue, 0.25, LEDStrip.LED_STRIPS).withTimeout(1)); OperatorConstants.CLIMB_TRIGGER.whileTrue(GeneralCommands.getClimbCommand()); OperatorConstants.MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_DOWN_MANUALLY_COMMAND); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f2406a59..3088e16a 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -30,7 +30,7 @@ public class OperatorConstants { TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER = OPERATOR_CONTROLLER.o(), TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER = OPERATOR_CONTROLLER.p(), LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), - LED_INDICATION_TRIGGER = DRIVER_CONTROLLER.x(), +// LED_INDICATION_TRIGGER = DRIVER_CONTROLLER.x(), CLIMB_TRIGGER = OPERATOR_CONTROLLER.c(), OVERRIDE_IS_CLIMBING_TRIGGER = OPERATOR_CONTROLLER.i(), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), @@ -51,7 +51,7 @@ public class OperatorConstants { CLOSE_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.x(), WARM_SPEAKER_SHOT_TRIGGER = OPERATOR_CONTROLLER.w().and(SPEAKER_SHOT_TRIGGER.negate()), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), - MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m(), + MANUAL_LOW_DELIVERY_TRIGGER = OPERATOR_CONTROLLER.m().or(DRIVER_CONTROLLER.x()), AMP_TRIGGER = OPERATOR_CONTROLLER.a(), AMP_WITHOUT_ALIGN_TRIGGER = OPERATOR_CONTROLLER.q(), AUTONOMOUS_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), From df013ee98439b14ea083c49b9ff0bb1f1b9019c3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 21 Oct 2024 13:23:12 +0300 Subject: [PATCH 197/261] first playoff game --- .../pathplanner/paths/SourceSide3NoteAutoRight2.path | 8 ++++---- .../pathplanner/paths/SourceSide3NoteAutoRight3.path | 8 ++++---- .../pathplanner/paths/SourceSide3NoteAutoRight4.path | 8 ++++---- .../pathplanner/paths/SourceSide3NoteAutoRight5.path | 8 ++++---- .../java/frc/trigon/robot/constants/CameraConstants.java | 2 +- .../java/frc/trigon/robot/constants/RobotConstants.java | 2 +- .../apriltagcamera/AprilTagCameraConstants.java | 2 +- 7 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index a70afb7a..2219e8f6 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.293644849397962, - "y": 2.385607506257749 + "x": 8.281950694955519, + "y": 2.4440782784699477 }, "prevControl": { - "x": 5.9080373431402124, - "y": 1.0641680542620358 + "x": 5.896343188697769, + "y": 1.1226388264742346 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 2552d136..6d69b0da 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.293644849397962, - "y": 2.385607506257749 + "x": 8.281950694955519, + "y": 2.4440782784699477 }, "prevControl": null, "nextControl": { - "x": 5.416882856557735, - "y": 0.7835083476434771 + "x": 5.405188702115292, + "y": 0.841979119855676 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightSecondCollection" diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index 4f69517d..26b5a70a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.281950694955519, - "y": 0.6899551121039572 + "x": 8.270256540513081, + "y": 0.7952025020859156 }, "prevControl": { - "x": 6.492745065262509, - "y": 1.3448277608847596 + "x": 6.481050910820072, + "y": 1.4500751508667182 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 46f4ea6b..7a2016d5 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.281950694955519, - "y": 0.6899551121039572 + "x": 8.270256540513081, + "y": 0.7952025020859156 }, "prevControl": null, "nextControl": { - "x": 5.919731497582653, - "y": 1.4383809964201122 + "x": 5.908037343140215, + "y": 1.5436283864020706 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightFirstCollection" diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index be2ebac8..365a8ebf 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -11,7 +11,7 @@ public class CameraConstants { public static final double TRANSLATIONS_STD_EXPONENT = 0.02, - THETA_STD_EXPONENT = 0.01; + THETA_STD_EXPONENT = 0.02; private static final Transform3d FRONT_CENTER_TO_CAMERA = new Transform3d( diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 93fa6a7e..dda4e7a0 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -7,7 +7,7 @@ public class RobotConstants { private static final boolean IS_SIMULATION = false, - IS_REPLAY = true; + IS_REPLAY = false; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "SwerveCANivore"; public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 9cedd24f..04fa7e8f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -10,7 +10,7 @@ public class AprilTagCameraConstants { public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2; + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2.5; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; From ef093851652070e71cc91d70516e645b33b0d7fa Mon Sep 17 00:00:00 2001 From: Strflightmight09 Date: Mon, 21 Oct 2024 19:22:14 +0300 Subject: [PATCH 198/261] Soo closeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee --- .pathplanner/settings.json | 2 +- .../SourceSide3NoteAutoRightImproved.auto | 4 +- .../paths/3NoteAutoThroughAmp2.path | 2 +- .../paths/3NoteAutoThroughAmp3.path | 2 +- .../paths/3NoteAutoThroughAmp4.path | 2 +- .../paths/3NoteAutoThroughAmp5.path | 2 +- .../paths/3NoteAutoThroughAmp6.path | 2 +- .../paths/3NoteAutoThroughAmpLeft3.path | 2 +- .../paths/3NoteAutoThroughAmpLeft4.path | 2 +- .../paths/3NoteAutoThroughAmpLeft5.path | 2 +- .../paths/5NoteAutoAroundStage1.path | 2 +- .../paths/5NoteAutoAroundStage2.path | 2 +- .../paths/5NoteAutoAroundStage3.path | 2 +- .../paths/5NoteAutoAroundStage4.path | 2 +- .../paths/5NoteAutoAroundStage5.path | 2 +- .../paths/5NoteAutoAroundStage6.path | 2 +- .../paths/5NoteAutoAroundStage7.path | 2 +- .../paths/5NoteAutoAroundStage8.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto1.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto10.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto2.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto3.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto4.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto5.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto6.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto7.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto8.path | 2 +- .../deploy/pathplanner/paths/7NoteAuto9.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto1.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto2.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto3.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto4.path | 2 +- .../pathplanner/paths/FrontRow4NoteAuto6.path | 2 +- .../paths/MiddleRush3NoteAuto1.path | 2 +- .../paths/MiddleRush3NoteAuto2.path | 2 +- .../paths/MiddleRush3NoteAuto3.path | 2 +- .../paths/MiddleRush3NoteAuto4.path | 2 +- .../paths/MiddleRush3NoteAuto5.path | 2 +- .../paths/MiddleRush3NoteAuto6.path | 2 +- .../paths/MiddleRush3NoteAuto7.path | 2 +- .../paths/MiddleRush3NoteAuto8.path | 2 +- .../paths/MiddleRush3NoteAuto9.path | 2 +- .../pathplanner/paths/New New Path.path | 52 +++++++++++++++++++ .../deploy/pathplanner/paths/New Path.path | 52 +++++++++++++++++++ .../deploy/pathplanner/paths/PIDAuto0.path | 2 +- .../deploy/pathplanner/paths/PIDAuto1.path | 2 +- .../paths/SourceSide3NoteAuto1.path | 2 +- .../paths/SourceSide3NoteAuto2.path | 2 +- .../paths/SourceSide3NoteAuto3.path | 2 +- .../paths/SourceSide3NoteAuto4.path | 2 +- .../paths/SourceSide3NoteAuto5.path | 2 +- .../paths/SourceSide3NoteAutoRight2.path | 6 +-- .../paths/SourceSide3NoteAutoRight3.path | 24 ++++----- .../paths/SourceSide3NoteAutoRight4.path | 18 +++---- .../paths/SourceSide3NoteAutoRight5.path | 10 ++-- .../paths/SpeakerSide3NoteAuto1.path | 2 +- .../paths/SpeakerSide3NoteAuto2.path | 2 +- .../paths/SpeakerSide3NoteAuto3.path | 2 +- .../paths/SpeakerSide3NoteAuto4.path | 2 +- .../paths/SpeakerSide3NoteAuto5.path | 2 +- .../paths/SpeakerSide3NoteAuto6.path | 2 +- .../paths/SpeakerSide3NoteAuto7.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto1.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto2.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto3.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto4.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto5.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto6.path | 2 +- .../pathplanner/paths/Zayde3NoteAuto7.path | 2 +- src/main/deploy/pathplanner/paths/test1.path | 2 +- src/main/deploy/pathplanner/paths/test2.path | 2 +- .../java/frc/trigon/robot/RobotContainer.java | 2 + .../factories/AutonomousCommands.java | 2 +- .../robot/constants/RobotConstants.java | 2 +- .../poseestimator/PoseEstimator.java | 1 + .../poseestimator/PoseEstimator6328.java | 3 ++ .../subsystems/swerve/SwerveConstants.java | 4 +- .../swerve/SwerveModuleConstants.java | 4 +- 78 files changed, 211 insertions(+), 101 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New New Path.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2607e05f..2ea88ba8 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -27,6 +27,6 @@ "defaultMaxVel": 4.0, "defaultMaxAccel": 4.0, "defaultMaxAngVel": 400.0, - "defaultMaxAngAccel": 600.0, + "defaultMaxAngAccel": 400.0, "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto index 1f621a71..a0ff7791 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto @@ -196,7 +196,7 @@ { "type": "path", "data": { - "pathName": "SourceSide3NoteAutoRight4" + "pathName": "New Path" } }, { @@ -241,7 +241,7 @@ { "type": "path", "data": { - "pathName": "SourceSide3NoteAutoRight5" + "pathName": "New New Path" } }, { diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path index 8d6e5d20..ac8af41f 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp2.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path index 05f6df8d..b9f74d07 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp3.path @@ -41,7 +41,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path index 2e0db7ba..3f34b4d5 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp4.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path index 2a2c3a02..77b1f652 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp5.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path index 8a9660c8..b4b688f9 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmp6.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path index 4e84bad7..ac1c6503 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft3.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path index 2d93555e..2aa81026 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft4.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path index 4ce49aa0..2cebea92 100644 --- a/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path +++ b/src/main/deploy/pathplanner/paths/3NoteAutoThroughAmpLeft5.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index fd2fc0a2..b4215353 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -86,7 +86,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path index 5f0503ab..cf57fdc5 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index 566cd4e1..b6b1cb09 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path index 22a6d8e0..3c2b8581 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index 09eb0e98..ab44e6b6 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path index 07aa0bbf..7288ab90 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index 65a3ca65..f249342b 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -59,7 +59,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path index d42f82a9..8dd1ed2c 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto1.path b/src/main/deploy/pathplanner/paths/7NoteAuto1.path index 45c46ca1..6bc28e07 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto1.path @@ -41,7 +41,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto10.path b/src/main/deploy/pathplanner/paths/7NoteAuto10.path index 1943a94c..549e1d98 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto10.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto10.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto2.path b/src/main/deploy/pathplanner/paths/7NoteAuto2.path index 3e9c03e8..1b8f0264 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto2.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto3.path b/src/main/deploy/pathplanner/paths/7NoteAuto3.path index da386fd7..008b7e87 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto3.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto4.path b/src/main/deploy/pathplanner/paths/7NoteAuto4.path index 8406beee..635bf9c8 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto4.path @@ -41,7 +41,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index 5ce0f718..0dec60c6 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto6.path b/src/main/deploy/pathplanner/paths/7NoteAuto6.path index 9993d4d8..80ec294b 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto6.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index e78fad75..e98d8f9b 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto8.path b/src/main/deploy/pathplanner/paths/7NoteAuto8.path index 60089835..45a0f9fd 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto8.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index b0b43940..039ea4b0 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path index 040a2af1..5f353d9b 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path index 4617c352..ea0e2cdd 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto2.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path index d93c519e..46bf26d1 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto3.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path index e68cebd2..f9fd2991 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto4.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path index 2d3216d7..439da821 100644 --- a/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAuto6.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path index 57ee08a9..82b00e9d 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index 3a0e2d0f..b50df5e7 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path index 1dc258af..515ed226 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index 6e0e7a04..028c2550 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path index 346c462c..fce074dc 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 9659b6b7..104dfe3b 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path index d38814e1..c697788f 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index 1efb93e8..f00bdb63 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -59,7 +59,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path index 28d4129f..f649532f 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 00000000..2753493a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.7155331803541096, + "y": 4.1046482092964185 + }, + "prevControl": null, + "nextControl": { + "x": 2.4348734737355513, + "y": 4.151424827066179 + }, + "isLocked": false, + "linkedName": "eee" + }, + { + "anchor": { + "x": 1.733224207189155, + "y": 5.262369499097973 + }, + "prevControl": { + "x": 2.282849465983831, + "y": 4.8881565569398955 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -10.340000000000032, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 00000000..7956ff73 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.663059280534515, + "y": 4.584108541436456 + }, + "prevControl": null, + "nextControl": { + "x": 1.8852482149408742, + "y": 4.326837143702777 + }, + "isLocked": false, + "linkedName": "SourceSide3NoteAutoRightSecondFeed" + }, + { + "anchor": { + "x": 2.7155331803541096, + "y": 4.1046482092964185 + }, + "prevControl": { + "x": 2.130825458232113, + "y": 4.385307915914978 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "eee" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.591140271194542, + "rotateFast": false + }, + "reversed": false, + "folder": "SourceSide3NoteAutoRight", + "previewStartingState": { + "rotation": -60.25511870305777, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path index f6ea1247..9392fe59 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto0.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path index 39463d71..305a9438 100644 --- a/src/main/deploy/pathplanner/paths/PIDAuto1.path +++ b/src/main/deploy/pathplanner/paths/PIDAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path index 03ff90be..6834105c 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path index 0eec812b..251aa288 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto2.path @@ -41,7 +41,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path index 8eca5b53..b9902f4a 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto3.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path index 78f47219..2415d65e 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto4.path @@ -41,7 +41,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path index 2ead400f..0c583b16 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAuto5.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path index 2219e8f6..b63bab9b 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight2.path @@ -17,11 +17,11 @@ { "anchor": { "x": 8.281950694955519, - "y": 2.4440782784699477 + "y": 2.525937359567027 }, "prevControl": { "x": 5.896343188697769, - "y": 1.1226388264742346 + "y": 1.204497907571314 }, "nextControl": null, "isLocked": false, @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path index 6d69b0da..873f305e 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight3.path @@ -4,24 +4,24 @@ { "anchor": { "x": 8.281950694955519, - "y": 2.4440782784699477 + "y": 2.525937359567027 }, "prevControl": null, "nextControl": { - "x": 5.405188702115292, - "y": 0.841979119855676 + "x": 5.299941312133336, + "y": 0.46776617769759743 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightSecondCollection" }, { "anchor": { - "x": 3.1683522521974625, - "y": 2.71716149066101 + "x": 1.663059280534515, + "y": 4.584108541436456 }, "prevControl": { - "x": 4.270855721198622, - "y": 1.777511475250871 + "x": 2.271155311541392, + "y": 2.315442579603108 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, - "rotation": -43.74000000000001, + "rotation": -31.019999999999982, "rotateFast": false }, "reversed": false, @@ -48,5 +48,5 @@ "rotation": 35.706691400603006, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index 26b5a70a..d12ff718 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.1683522521974625, - "y": 2.71716149066101 + "x": 1.663059280534515, + "y": 4.584108541436456 }, "prevControl": null, "nextControl": { - "x": 5.370106238788276, - "y": 1.7892056296974757 + "x": 3.8648132671253284, + "y": 3.6561526804729216 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightSecondFeed" }, { "anchor": { - "x": 8.270256540513081, - "y": 0.7952025020859156 + "x": 8.281950694955519, + "y": 0.8185908109707958 }, "prevControl": { - "x": 6.481050910820072, - "y": 1.4500751508667182 + "x": 6.492745065262509, + "y": 1.4734634597515983 }, "nextControl": null, "isLocked": false, @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path index 7a2016d5..ca033ed3 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.270256540513081, - "y": 0.7952025020859156 + "x": 8.281950694955519, + "y": 0.8185908109707958 }, "prevControl": null, "nextControl": { - "x": 5.908037343140215, - "y": 1.5436283864020706 + "x": 5.919731497582653, + "y": 1.5670166952869513 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightFirstCollection" @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path index 1d925fff..fc3d2ce1 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index 3885e2e6..4b09d79c 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path index dc3edb94..ee1ec14d 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index 5213f2ed..f18342bf 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path index 6a5d1442..154a6672 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index e3a502e0..a18d5a3a 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -59,7 +59,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path index ff3435a8..8aba8037 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path index f42b13ad..5150ef96 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index 49011106..54b8a38f 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -59,7 +59,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path index aa0378bd..8af7db1f 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index a56d138a..4524d439 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -65,7 +65,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path index 9262147a..b26a6dd2 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index 69995291..4f35b982 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -59,7 +59,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path index e46d4fc0..5306e8e1 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path @@ -53,7 +53,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/test1.path b/src/main/deploy/pathplanner/paths/test1.path index b8d8c2c6..70026f97 100644 --- a/src/main/deploy/pathplanner/paths/test1.path +++ b/src/main/deploy/pathplanner/paths/test1.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/test2.path b/src/main/deploy/pathplanner/paths/test2.path index 6815b66d..b726a7eb 100644 --- a/src/main/deploy/pathplanner/paths/test2.path +++ b/src/main/deploy/pathplanner/paths/test2.path @@ -35,7 +35,7 @@ "maxVelocity": 4.0, "maxAcceleration": 4.0, "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 600.0 + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0620b08a..78cc65d8 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -114,6 +114,8 @@ private void bindControllerCommands() { OperatorConstants.BLOCK_TRIGGER.whileTrue(AmpCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); + + OperatorConstants.OPERATOR_CONTROLLER.t().whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 8fc69142..3feb8ff9 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -38,7 +38,7 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { if (DriverStation.isEnabled()) return; final Pose2d autoStartPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathName.get()); - RobotContainer.POSE_ESTIMATOR.resetPose(new MirrorablePose2d(autoStartPose, true).get()); + RobotContainer.POSE_ESTIMATOR.resetPose(autoStartPose); } ).ignoringDisable(true); } diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index dda4e7a0..93fa6a7e 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -7,7 +7,7 @@ public class RobotConstants { private static final boolean IS_SIMULATION = false, - IS_REPLAY = false; + IS_REPLAY = true; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "SwerveCANivore"; public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 3b419ad0..368739a7 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -38,6 +38,7 @@ public PoseEstimator(AprilTagCamera... aprilTagCameras) { field.getObject("path").setPoses(pose); Logger.recordOutput("Path", pose.toArray(new Pose2d[0])); }); + PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("TargetPPPose", pose)); } @Override diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index e2cb1b5f..93aadf2d 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.AutoLogOutput; @@ -89,6 +90,8 @@ public void addOdometryObservation(OdometryObservation observation) { } public void addVisionObservation(VisionObservation observation) { +// if (DriverStation.isAutonomous() || DriverStation.isDisabled()) +// return; // If measurement is old enough to be outside the pose buffer's timespan, skip. try { if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 3debee58..7d54d3a5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -77,13 +77,13 @@ public class SwerveConstants { new PIDConstants(5, 0, 0), PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(4, 0, 0.05) : - new PIDConstants(5, 0, 0), + new PIDConstants(3, 0, 0), AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0.1) : new PIDConstants(2, 0, 0.1), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(2.5, 0, 0.2) : - new PIDConstants(2, 0, 0); + new PIDConstants(6.5, 0, 0); private static final double MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 720, MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 720; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 204be9ad..3637f916 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -25,7 +25,7 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 60, + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 70, STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 30; private static final double STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, @@ -60,7 +60,7 @@ public class SwerveModuleConstants { Units.Second.of(1000) ); - static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049149 * 2; + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.049274 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; static SimpleMotorSimulation createDriveSimulation() { From ca1a994e644f59cf65978f09d1565d6621ec5a7a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 24 Oct 2024 12:54:01 -0400 Subject: [PATCH 199/261] Tiny cleaning stuff I just found --- src/main/java/frc/trigon/robot/RobotContainer.java | 5 ----- src/main/java/frc/trigon/robot/constants/RobotConstants.java | 2 +- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 78cc65d8..6e8e595a 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,7 +6,6 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; @@ -25,9 +24,7 @@ import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; -import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.Pitcher; -import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; @@ -114,8 +111,6 @@ private void bindControllerCommands() { OperatorConstants.BLOCK_TRIGGER.whileTrue(AmpCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); - - OperatorConstants.OPERATOR_CONTROLLER.t().whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 93fa6a7e..dda4e7a0 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -7,7 +7,7 @@ public class RobotConstants { private static final boolean IS_SIMULATION = false, - IS_REPLAY = true; + IS_REPLAY = false; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "SwerveCANivore"; public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; From 83ff106e23b437cd7ef9fcc6c9d77044b7a91492 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 27 Oct 2024 11:47:52 +0200 Subject: [PATCH 200/261] LOTS of fixes (not enough) --- .../java/frc/trigon/robot/RobotContainer.java | 5 ++-- .../robot/commands/factories/AmpCommands.java | 10 +------- .../factories/AutonomousCommands.java | 11 +++++---- .../commands/factories/GeneralCommands.java | 13 ++++++++-- ...on => 2024-crescendo-home-tag-layout.json} | 0 .../robot/constants/AutonomousConstants.java | 10 ++++---- .../robot/constants/FieldConstants.java | 24 +++++++++---------- .../apriltagcamera/AprilTagCamera.java | 11 +++++++-- .../AprilTagCameraConstants.java | 2 ++ .../io/AprilTagPhotonCameraIO.java | 12 ++++++---- .../poseestimator/PoseEstimator6328.java | 3 --- .../subsystems/ampaligner/AmpAligner.java | 2 +- .../subsystems/climber/ClimberConstants.java | 2 +- .../robot/subsystems/intake/Intake.java | 7 ------ .../subsystems/intake/IntakeCommands.java | 1 - .../subsystems/intake/IntakeConstants.java | 6 ----- .../robot/subsystems/ledstrip/LEDStrip.java | 16 ++++--------- .../subsystems/ledstrip/LEDStripCommands.java | 2 +- .../swerve/SwerveModuleConstants.java | 2 +- 19 files changed, 63 insertions(+), 76 deletions(-) rename src/main/java/frc/trigon/robot/constants/{2024-crescendo.json => 2024-crescendo-home-tag-layout.json} (100%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 6e8e595a..176cc495 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -72,7 +72,7 @@ private void bindDefaultCommands() { private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); - OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.whileTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); + OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(GeneralCommands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); @@ -87,7 +87,6 @@ private void bindControllerCommands() { OperatorConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_COMMAND); OperatorConstants.LED_AUTO_SETUP_TRIGGER.whileTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); -// OperatorConstants.LED_INDICATION_TRIGGER.onTrue(LEDStripCommands.getBlinkingCommand(Color.kBlue, 0.25, LEDStrip.LED_STRIPS).withTimeout(1)); OperatorConstants.CLIMB_TRIGGER.whileTrue(GeneralCommands.getClimbCommand()); OperatorConstants.MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER.whileTrue(CommandConstants.MOVE_CLIMBER_DOWN_MANUALLY_COMMAND); @@ -108,7 +107,7 @@ private void bindControllerCommands() { OperatorConstants.AMP_WITHOUT_ALIGN_TRIGGER.whileTrue(AmpCommands.getScoreInAmpCommand(false)); OperatorConstants.AUTONOMOUS_AMP_TRIGGER.whileTrue(AmpCommands.getAutonomousScoreInAmpCommand()); OperatorConstants.REALIGN_AMP_ALIGNER_TRIGGER.whileTrue(AmpAlignerCommands.getSetPositionCommand(AmpAlignerConstants.REALIGN_AMP_ALIGNER_ANGLE)); - OperatorConstants.BLOCK_TRIGGER.whileTrue(AmpCommands.getBlockCommand()); + OperatorConstants.BLOCK_TRIGGER.whileTrue(GeneralCommands.getBlockCommand()); OperatorConstants.RESET_POSE_TO_AUTO_POSE_TRIGGER.onTrue(AutonomousCommands.getResetPoseToAutoPoseCommand(() -> autoChooser.get().getName())); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index ee0c531f..bef986f1 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -13,7 +13,6 @@ import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; -import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -36,13 +35,6 @@ public static Command getScoreInAmpCommand(boolean shouldAlignToAmp) { )); } - public static Command getBlockCommand() { - return new ParallelCommandGroup( - AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN), - PitcherCommands.getSetTargetPitchCommand(PitcherConstants.BLOCK_PITCH) - ); - } - /** * Creates a command that prepares to score in the amp for the autonomous score in amp command. * This command waits until we're within a certain distance from the amp to ensure that the robot doesn't hit the stage. @@ -57,7 +49,7 @@ private static Command getAutonomousPrepareForAmpCommand() { AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN) .alongWith(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.PREPARE_AMP_PITCH)), () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation().getDistance( - FieldConstants.IN_FRONT_OF_AMP_POSE.get().getTranslation()) < FieldConstants.MINIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS + FieldConstants.IN_FRONT_OF_AMP_POSE.get().getTranslation()) < FieldConstants.MAXIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS ), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND) ); diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 3feb8ff9..51ae7fbc 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -20,6 +20,7 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.ShooterCommands; +import frc.trigon.robot.subsystems.shooter.ShooterConstants; import org.trigon.utilities.mirrorable.MirrorablePose2d; import java.util.Optional; @@ -38,7 +39,7 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { if (DriverStation.isEnabled()) return; final Pose2d autoStartPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathName.get()); - RobotContainer.POSE_ESTIMATOR.resetPose(autoStartPose); + RobotContainer.POSE_ESTIMATOR.resetPose(new MirrorablePose2d(autoStartPose, true).get()); } ).ignoringDisable(true); } @@ -76,7 +77,7 @@ public static Command getStopAligningToNoteCommand() { ); } - public static Command getPreparePitchCommand() { + public static Command getPreparePitchForSpeakerShotCommand() { return new ParallelCommandGroup( ShootingCommands.getUpdateShootingCalculationsCommand(false), PitcherCommands.getReachTargetPitchFromShootingCalculationsCommand() @@ -90,12 +91,12 @@ public static Command getPreparePitchCommand(Rotation2d pitch) { ); } - public static Command getPrepareShootingCommand() { + public static Command getPrepareShooterForSpeakerShotCommand() { return ShooterCommands.getReachTargetShootingVelocityFromShootingCalculationsCommand(); } - public static Command getPrepareShootingCommand(double velocity) { - return ShooterCommands.getSetTargetVelocityCommand(velocity); + public static Command getPrepareShooterCommand(double targetVelocityRotationsPerSecond) { + return ShooterCommands.getSetTargetVelocityCommand(targetVelocityRotationsPerSecond, targetVelocityRotationsPerSecond * ShooterConstants.RIGHT_MOTOR_TO_LEFT_MOTOR_RATIO); } private static Optional calculateRotationOverride() { diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 8d3f49d3..1c36d527 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -9,6 +9,8 @@ import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.ShootingConstants; import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerCommands; +import frc.trigon.robot.subsystems.ampaligner.AmpAlignerConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; @@ -54,6 +56,13 @@ public static Command getHighEjectNoteCommand() { ); } + public static Command getBlockCommand() { + return new ParallelCommandGroup( + AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN), + PitcherCommands.getSetTargetPitchCommand(PitcherConstants.BLOCK_PITCH) + ); + } + public static Command getVisualizeNoteShootingCommand() { return new InstantCommand( () -> new VisualizeNoteShootingCommand() @@ -121,8 +130,8 @@ public static Command runWhen(Command command, BooleanSupplier condition) { return new WaitUntilCommand(condition).andThen(command); } - public static Command runWhen(Command command,BooleanSupplier condition, double debounceTimeSeconds) { - return new WaitUntilCommand(condition).andThen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition).repeatedly())); + public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) { + return new WaitUntilCommand(condition).andThen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition))); } public static Command duplicate(Command command) { diff --git a/src/main/java/frc/trigon/robot/constants/2024-crescendo.json b/src/main/java/frc/trigon/robot/constants/2024-crescendo-home-tag-layout.json similarity index 100% rename from src/main/java/frc/trigon/robot/constants/2024-crescendo.json rename to src/main/java/frc/trigon/robot/constants/2024-crescendo-home-tag-layout.json diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 4da84fcd..1eee0566 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -17,15 +17,15 @@ private static void registerCommands() { NamedCommands.registerCommand("Collect", AutonomousCommands.getNoteCollectionCommand()); NamedCommands.registerCommand("AlignToNote", AutonomousCommands.getAlignToNoteCommand()); NamedCommands.registerCommand("StopAligningToNote", AutonomousCommands.getStopAligningToNoteCommand()); - NamedCommands.registerCommand("PrepareForShooting", AutonomousCommands.getPrepareShootingCommand()); - NamedCommands.registerCommand("PreparePitch", AutonomousCommands.getPreparePitchCommand()); - NamedCommands.registerCommand("PrepareForShootingCloseShot", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PrepareForShooting", AutonomousCommands.getPrepareShooterForSpeakerShotCommand()); + NamedCommands.registerCommand("PreparePitch", AutonomousCommands.getPreparePitchForSpeakerShotCommand()); + NamedCommands.registerCommand("PrepareForShootingCloseShot", AutonomousCommands.getPrepareShooterCommand(ShootingConstants.CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND)); NamedCommands.registerCommand("PreparePitchForCloseShot", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_SHOT_PITCH)); NamedCommands.registerCommand("StopShooting", ShooterCommands.getStopCommand()); NamedCommands.registerCommand("FeedNote", AutonomousCommands.getFeedNoteCommand()); NamedCommands.registerCommand("PreparePitchForEjectFromShooter", AutonomousCommands.getPreparePitchCommand(ShootingConstants.EJECT_FROM_SHOOTER_PITCH)); - NamedCommands.registerCommand("PrepareShootingForEjectFromShooter", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PrepareShootingForEjectFromShooter", AutonomousCommands.getPrepareShooterCommand(ShootingConstants.EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); NamedCommands.registerCommand("PreparePitchForCloseEjectFromShooter", AutonomousCommands.getPreparePitchCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_PITCH)); - NamedCommands.registerCommand("PrepareShootingForCloseEjectFromShooter", AutonomousCommands.getPrepareShootingCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); + NamedCommands.registerCommand("PrepareShootingForCloseEjectFromShooter", AutonomousCommands.getPrepareShooterCommand(ShootingConstants.CLOSE_EJECT_FROM_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND)); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 1546624b..3f62935e 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -2,11 +2,9 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; -import org.littletonrobotics.junction.Logger; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; @@ -14,16 +12,16 @@ import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); -// public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; -// -// static { -// try { -// APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo.json"); -// } catch (IOException e) { -// throw new RuntimeException(e); -// } -// } + // public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; + + static { + try { + APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json"); + } catch (IOException e) { + throw new RuntimeException(e); + } + } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double @@ -41,7 +39,7 @@ public class FieldConstants { TARGET_DELIVERY_POSITION = new MirrorableTranslation3d(2.5, 7, 0, true); public static final MirrorablePose2d IN_FRONT_OF_AMP_POSE = new MirrorablePose2d(1.842, 8.204 - 0.405, Rotation2d.fromDegrees(-90), true); - public static final double MINIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS = 2.5; + public static final double MAXIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS = 2.5; private static HashMap fieldLayoutToTagIdToPoseMap() { final HashMap tagIdToPose = new HashMap<>(); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 531ab084..d9b8a83e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -54,8 +54,6 @@ public void update() { aprilTagCameraIO.updateInputs(inputs); robotPose = calculateBestRobotPose(); - if (inputs.cameraSolvePNPPose != null) - Logger.recordOutput("SolvePNPPose", inputs.cameraSolvePNPPose); logCameraInfo(); } @@ -134,12 +132,21 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading if (bestTagPose == null) return null; + setProperCameraRotation(); + final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(currentHeading); return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); } + private void setProperCameraRotation() { + if (robotCenterToCamera.getRotation().getX() == Math.PI) { + inputs.bestTargetRelativePitchRadians = -inputs.bestTargetRelativePitchRadians; + inputs.bestTargetRelativeYawRadians = -inputs.bestTargetRelativeYawRadians; + } + } + private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneDistanceToTag(tagPose, robotPlaneTargetYawRadians); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 04fa7e8f..bea1b24e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -14,6 +14,8 @@ public class AprilTagCameraConstants { static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; + public static final double MAXIMUM_AMBIGUITY = 0.5; + public enum AprilTagCameraType { PHOTON_CAMERA(AprilTagPhotonCameraIO::new), LIMELIGHT(AprilTagLimelightIO::new); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 182eae61..ee0b54e8 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -10,7 +10,6 @@ import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; -import org.littletonrobotics.junction.Logger; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; @@ -29,7 +28,7 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getBestTarget(latestResult).poseAmbiguity < 0.5; + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; if (inputs.hasResult) updateHasResultInputs(inputs, latestResult); else @@ -41,8 +40,8 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Photon inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitchRadians = -bestTargetRelativeRotation3d.getY(); - inputs.bestTargetRelativeYawRadians = -bestTargetRelativeRotation3d.getZ(); + inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); inputs.visibleTagIDs = getVisibleTagIDs(latestResult); inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); } @@ -52,6 +51,10 @@ private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { inputs.cameraSolvePNPPose = new Pose3d(); } + private double getPoseAmbiguity(PhotonPipelineResult result) { + return getBestTarget(result).getPoseAmbiguity(); + } + private Point getTagCenter(List tagCorners) { double tagCornerSumX = 0; double tagCornerSumY = 0; @@ -90,7 +93,6 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(getBestTarget(result).getFiducialId()); final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); - Logger.recordOutput("TagPose", tagPose); final Transform3d targetToCamera = getBestTarget(result).getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 93aadf2d..e2cb1b5f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -19,7 +19,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.DriverStation; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.AutoLogOutput; @@ -90,8 +89,6 @@ public void addOdometryObservation(OdometryObservation observation) { } public void addVisionObservation(VisionObservation observation) { -// if (DriverStation.isAutonomous() || DriverStation.isDisabled()) -// return; // If measurement is old enough to be outside the pose buffer's timespan, skip. try { if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds diff --git a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java index 2f205135..a8df8d6d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java +++ b/src/main/java/frc/trigon/robot/subsystems/ampaligner/AmpAligner.java @@ -114,7 +114,7 @@ void setTargetAngle(Rotation2d targetAngle) { private void configurePositionResettingTrigger() { final Trigger hitLimitSwitchTrigger = new Trigger(this::hasHitForwardLimit).debounce(AmpAlignerConstants.LIMIT_SWITCH_DEBOUNCE_TIME_SECONDS).or(OperatorConstants.RESET_AMP_ALIGNER_POSITION_TRIGGER); - hitLimitSwitchTrigger.onTrue(new InstantCommand(() -> motor.setPosition(AmpAlignerConstants.LIMIT_SWITCH_PRESSED_ANGLE.getRotations())).ignoringDisable(true)); + hitLimitSwitchTrigger.onTrue(new InstantCommand(() -> setPosition(AmpAlignerConstants.LIMIT_SWITCH_PRESSED_ANGLE)).ignoringDisable(true)); } private boolean hasHitForwardLimit() { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index f08b9125..c422529a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -18,7 +18,7 @@ import org.trigon.hardware.simulation.SimpleMotorSimulation; public class ClimberConstants { - public static final int + private static final int RIGHT_MOTOR_ID = 14, LEFT_MOTOR_ID = 15; private static final String diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index b347937a..133a05ae 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -5,14 +5,10 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; -import org.littletonrobotics.junction.Logger; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -30,9 +26,6 @@ public Intake() { public void updatePeriodically() { masterMotor.update(); IntakeConstants.DISTANCE_SENSOR.updateSensor(); - Logger.recordOutput("ShouldAlignToNote", CommandConstants.SHOULD_ALIGN_TO_NOTE); - Logger.recordOutput("HasNote", hasNote()); - Logger.recordOutput("distanceSensorScaledValue", IntakeConstants.DISTANCE_SENSOR.getScaledValue()); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 9bdea653..a9e3fbf4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -40,7 +40,6 @@ private static Command getCollectionCommand() { (interrupted) -> { if (!interrupted) { RobotContainer.INTAKE.indicateCollection(); -// RobotContainer.INTAKE.sendStaticBrakeRequest() } }, RobotContainer.INTAKE::hasNote, diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index c506dca3..01cf319c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -90,7 +90,6 @@ private static void configureMasterMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; - config.HardwareLimitSwitch.ForwardLimitEnable = false; config.HardwareLimitSwitch.ReverseLimitEnable = false; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -101,8 +100,6 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); } private static void configureFollowerMotor() { @@ -113,12 +110,9 @@ private static void configureFollowerMotor() { config.Audio.BeepOnBoot = false; config.Audio.BeepOnConfig = false; - config.HardwareLimitSwitch.ForwardLimitEnable = false; config.HardwareLimitSwitch.ReverseLimitEnable = false; FOLLOWER_MOTOR.applyConfiguration(config); - FOLLOWER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); - FOLLOWER_MOTOR.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100); final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); FOLLOWER_MOTOR.setControl(followerRequest); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 01d47c1c..8bc4f4f1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -37,7 +37,7 @@ public LEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) { this.inverted = inverted; this.numberOfLEDs = numberOfLEDs; - resetBreatheSettings(); + resetLEDSettings(); addLEDStripToLEDStripsArray(this); } @@ -46,15 +46,6 @@ public static void setDefaultCommandForAllLEDS(Command command) { ledStrip.setDefaultCommand(command); } - public static void changeDefaultCommandForAllLEDs(Command newDefaultCommand) { - for (LEDStrip ledStrip : LED_STRIPS) { - final Command currentDefaultCommand = ledStrip.getDefaultCommand(); - if (currentDefaultCommand != null) - currentDefaultCommand.cancel(); - ledStrip.setDefaultCommand(newDefaultCommand); - } - } - public int getNumberOfLEDS() { return numberOfLEDs; } @@ -88,9 +79,12 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - void resetBreatheSettings() { + void resetLEDSettings() { lastBreatheLED = indexOffset; lastBreatheMovementTime = Timer.getFPGATimestamp(); + rainbowFirstPixelHue = 0; + areLEDsOnForBlinking = false; + lastBlinkTime = 0; } void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop) { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 275908f2..5672c760 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -37,7 +37,7 @@ public static Command getBreatheCommand(Color color, int breathingLEDs, double c return new ExecuteEndCommand( () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), () -> { - runForLEDs(LEDStrip::resetBreatheSettings, ledStrips); + runForLEDs(LEDStrip::resetLEDSettings, ledStrips); runForLEDs(LEDStrip::clearLedColors, ledStrips); }, ledStrips diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 3637f916..cba24647 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -25,7 +25,7 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 70, + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 80, STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 30; private static final double STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, From a67827a461f741049c84b9a97d497f5c7e7d66a2 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 28 Oct 2024 09:03:31 +0200 Subject: [PATCH 201/261] Thank you @Nummun14 <3 --- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 01cf319c..0b7a634c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -100,6 +100,7 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); } private static void configureFollowerMotor() { @@ -114,6 +115,8 @@ private static void configureFollowerMotor() { FOLLOWER_MOTOR.applyConfiguration(config); + FOLLOWER_MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); + final Follower followerRequest = new Follower(MASTER_MOTOR_ID, FOLLOWER_OPPOSES_MASTER); FOLLOWER_MOTOR.setControl(followerRequest); } From 1c072e6c983e25b1232e8e81ecaa2a7aa460eca4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 28 Oct 2024 16:06:13 +0200 Subject: [PATCH 202/261] =?UTF-8?q?Minor=20LED=20fixes=20=F0=9F=A4=B7?= =?UTF-8?q?=E2=80=8D=E2=99=82=EF=B8=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../robot/constants/FieldConstants.java | 22 +++++++++---------- .../subsystems/intake/IntakeConstants.java | 1 - .../robot/subsystems/ledstrip/LEDStrip.java | 5 ++++- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 3f62935e..28483b5e 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -2,26 +2,26 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; -import java.io.IOException; import java.util.HashMap; public class FieldConstants { - // public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; - - static { - try { - APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json"); - } catch (IOException e) { - throw new RuntimeException(e); - } - } + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); +// public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; +// +// static { +// try { +// APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json"); +// } catch (IOException e) { +// throw new RuntimeException(e); +// } +// } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 0b7a634c..08a53dbd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -64,7 +64,6 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); - static final double CORRECT_NOTE_POSITION_TIMEOUT_SECONDS = 0.07; private static final double NOTE_COLLECTION_CURRENT = 50; private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.1; static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 8bc4f4f1..73f32939 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -96,8 +96,11 @@ void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean sh lastBreatheLED++; } if (lastBreatheLED >= numberOfLEDs + indexOffset) { - if (!shouldLoop) + if (!shouldLoop) { CommandConstants.DEFAULT_LEDS_COMMAND.schedule(); + return; + } + lastBreatheLED = indexOffset; } for (int i = 0; i < breathingLEDs; i++) { if (lastBreatheLED - i >= indexOffset && lastBreatheLED - i < indexOffset + numberOfLEDs) From 4f3c07c68e3839ae86443d46a0c0a0f21dfa3809 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 29 Oct 2024 08:50:12 +0200 Subject: [PATCH 203/261] Fixed `AlignToNoteCommand.java` --- .../robot/commands/AlignToNoteCommand.java | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index e5f9606a..3b117eb7 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.trigon.robot.RobotContainer; @@ -16,7 +15,6 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.trigon.hardware.RobotHardwareStats; -import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.MirrorableRotation2d; public class AlignToNoteCommand extends ParallelCommandGroup { @@ -43,12 +41,20 @@ private Command getSetCurrentLEDColorCommand() { private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(getScaledJoystickValue()), + () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftX(), OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); } + private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); + final double xValue = CommandConstants.calculateDriveStickAxisValue(xPower); + final double yValue = CommandConstants.calculateDriveStickAxisValue(yPower); + + return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); + } + private MirrorableRotation2d getTargetAngle() { final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); @@ -57,12 +63,4 @@ private MirrorableRotation2d getTargetAngle() { private boolean shouldAlignToNote() { return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } - - private double getScaledJoystickValue() { - final XboxController controller = OperatorConstants.DRIVER_CONTROLLER; - final var x = Rotation2d.fromDegrees(90); - final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().plus(x); - - return controller.getLeftX() * -robotHeading.getCos() - controller.getLeftY() * -robotHeading.getSin(); - } } \ No newline at end of file From 90f21c0ad600f443ce6a8a770f8e2310823a1a29 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 29 Oct 2024 12:25:24 +0200 Subject: [PATCH 204/261] Not cleaner --- .../robot/constants/FieldConstants.java | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 28483b5e..365e838c 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -9,19 +9,22 @@ import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; +import java.io.IOException; import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); -// public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; -// -// static { -// try { -// APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json"); -// } catch (IOException e) { -// throw new RuntimeException(e); -// } -// } + private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; + + static { + try { + APRIL_TAG_FIELD_LAYOUT = SHOULD_USE_HOME_TAG_LAYOUT ? + AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json") : + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + } catch (IOException e) { + throw new RuntimeException(e); + } + } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double From 2ad0e22d174d430df938fa2b9f7aea932ff22fc3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 29 Oct 2024 12:32:19 +0200 Subject: [PATCH 205/261] I hope this works (cleaned reset to solve pnp pose) --- .../poseestimator/PoseEstimator.java | 21 +++++++------------ 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 368739a7..2d9a9d26 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -82,21 +82,14 @@ public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPos } public void setGyroHeadingToBestSolvePNPHeading() { - double firstCameraTagDistance = 10; - double secondCameraTagDistance = 10; int closestCameraToTag = 0; - - if (aprilTagCameras[0].hasResult()) - firstCameraTagDistance = aprilTagCameras[0].getDistanceToBestTagMeters(); - if (aprilTagCameras[1].hasResult()) - secondCameraTagDistance = aprilTagCameras[1].getDistanceToBestTagMeters(); - if (firstCameraTagDistance > secondCameraTagDistance) - closestCameraToTag = 1; - else if (secondCameraTagDistance > firstCameraTagDistance) - closestCameraToTag = 0; - - final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); - resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + for (int i = 0; i < aprilTagCameras.length; i++) { + if (aprilTagCameras[i].hasResult() && aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) + closestCameraToTag = i; + } + + final Rotation2d bestCameraHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestCameraHeading)); } private void updateFromVision() { From 42813049c431ece851e07c03e8700003a3b0a1ec Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 29 Oct 2024 12:36:22 +0200 Subject: [PATCH 206/261] cleaned up autos --- .pathplanner/settings.json | 7 +- .../autos/FrontRow4NoteAutoFarShoot.auto | 8 +- ...uto => FrontRow4NoteAutoFarShootSide.auto} | 0 src/main/deploy/pathplanner/autos/PID.auto | 121 ------------------ ...ved.auto => SourceSide3NoteAutoFront.auto} | 4 +- ...1.path => FrontRow4NoteAutoFarShoot2.path} | 2 +- ...2.path => FrontRow4NoteAutoFarShoot3.path} | 4 +- ...3.path => FrontRow4NoteAutoFarShoot4.path} | 4 +- ...4.path => FrontRow4NoteAutoFarShoot5.path} | 2 +- .../deploy/pathplanner/paths/PIDAuto0.path | 70 ---------- .../deploy/pathplanner/paths/PIDAuto1.path | 52 -------- ...th.path => SourceSide3NoteAutoFront4.path} | 4 +- ...th.path => SourceSide3NoteAutoFront5.path} | 6 +- .../paths/SourceSide3NoteAutoRight4.path | 10 +- src/main/deploy/pathplanner/paths/test1.path | 2 +- src/main/deploy/pathplanner/paths/test2.path | 2 +- 16 files changed, 28 insertions(+), 270 deletions(-) rename src/main/deploy/pathplanner/autos/{FrontRow4NoteAutoFarShootVarient.auto => FrontRow4NoteAutoFarShootSide.auto} (100%) delete mode 100644 src/main/deploy/pathplanner/autos/PID.auto rename src/main/deploy/pathplanner/autos/{SourceSide3NoteAutoRightImproved.auto => SourceSide3NoteAutoFront.auto} (98%) rename src/main/deploy/pathplanner/paths/{front1.path => FrontRow4NoteAutoFarShoot2.path} (96%) rename src/main/deploy/pathplanner/paths/{front2.path => FrontRow4NoteAutoFarShoot3.path} (91%) rename src/main/deploy/pathplanner/paths/{front3.path => FrontRow4NoteAutoFarShoot4.path} (91%) rename src/main/deploy/pathplanner/paths/{front4.path => FrontRow4NoteAutoFarShoot5.path} (96%) delete mode 100644 src/main/deploy/pathplanner/paths/PIDAuto0.path delete mode 100644 src/main/deploy/pathplanner/paths/PIDAuto1.path rename src/main/deploy/pathplanner/paths/{New Path.path => SourceSide3NoteAutoFront4.path} (91%) rename src/main/deploy/pathplanner/paths/{New New Path.path => SourceSide3NoteAutoFront5.path} (87%) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2ea88ba8..b1993e77 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -9,12 +9,13 @@ "7NoteAuto", "FrontRow4NoteAuto", "MiddleRush3NoteAuto", - "test", - "PIDAuto", + "FrontRow4NoteAutoFarShoot", "SourceSide3NoteAuto", + "SourceSide3NoteAutoFront", "SourceSide3NoteAutoRight", "SpeakerSide3NoteAuto", - "Zayde3NoteAuto" + "Zayde3NoteAuto", + "test" ], "autoFolders": [ "3NoteAutoThroughAmpVarients", diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto index e1f69ef9..3b26902b 100644 --- a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto +++ b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShoot.auto @@ -158,7 +158,7 @@ { "type": "path", "data": { - "pathName": "front1" + "pathName": "FrontRow4NoteAutoFarShoot2" } }, { @@ -222,7 +222,7 @@ { "type": "path", "data": { - "pathName": "front2" + "pathName": "FrontRow4NoteAutoFarShoot3" } }, { @@ -254,7 +254,7 @@ { "type": "path", "data": { - "pathName": "front3" + "pathName": "FrontRow4NoteAutoFarShoot4" } }, { @@ -318,7 +318,7 @@ { "type": "path", "data": { - "pathName": "front4" + "pathName": "FrontRow4NoteAutoFarShoot5" } }, { diff --git a/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto b/src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootSide.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootVarient.auto rename to src/main/deploy/pathplanner/autos/FrontRow4NoteAutoFarShootSide.auto diff --git a/src/main/deploy/pathplanner/autos/PID.auto b/src/main/deploy/pathplanner/autos/PID.auto deleted file mode 100644 index 2dc3ba5a..00000000 --- a/src/main/deploy/pathplanner/autos/PID.auto +++ /dev/null @@ -1,121 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.6306711948340775, - "y": 2.359441488479177 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PreparePitch" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "PIDAuto0" - } - }, - { - "type": "path", - "data": { - "pathName": "PIDAuto1" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "StopShooting" - } - } - ] - } - }, - "folder": "misc", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoFront.auto similarity index 98% rename from src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto rename to src/main/deploy/pathplanner/autos/SourceSide3NoteAutoFront.auto index a0ff7791..9b1977f7 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRightImproved.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoFront.auto @@ -196,7 +196,7 @@ { "type": "path", "data": { - "pathName": "New Path" + "pathName": "SourceSide3NoteAutoFront4" } }, { @@ -241,7 +241,7 @@ { "type": "path", "data": { - "pathName": "New New Path" + "pathName": "SourceSide3NoteAutoFront5" } }, { diff --git a/src/main/deploy/pathplanner/paths/front1.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot2.path similarity index 96% rename from src/main/deploy/pathplanner/paths/front1.path rename to src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot2.path index 740c4465..df221fce 100644 --- a/src/main/deploy/pathplanner/paths/front1.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot2.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FrontRow4NoteAuto", + "folder": "FrontRow4NoteAutoFarShoot", "previewStartingState": { "rotation": 27.819999999999993, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/front2.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot3.path similarity index 91% rename from src/main/deploy/pathplanner/paths/front2.path rename to src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot3.path index 5fd2c328..4073b31f 100644 --- a/src/main/deploy/pathplanner/paths/front2.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot3.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "nne" + "linkedName": "FrontRow4NoteAutoFarShootSecondFeed" } ], "rotationTargets": [], @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FrontRow4NoteAuto", + "folder": "FrontRow4NoteAutoFarShoot", "previewStartingState": { "rotation": -19.82440273715712, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/front3.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot4.path similarity index 91% rename from src/main/deploy/pathplanner/paths/front3.path rename to src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot4.path index fb556ce6..11ab0490 100644 --- a/src/main/deploy/pathplanner/paths/front3.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot4.path @@ -12,7 +12,7 @@ "y": 4.777381526259489 }, "isLocked": false, - "linkedName": "nne" + "linkedName": "FrontRow4NoteAutoFarShootSecondFeed" }, { "anchor": { @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FrontRow4NoteAuto", + "folder": "FrontRow4NoteAutoFarShoot", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/front4.path b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot5.path similarity index 96% rename from src/main/deploy/pathplanner/paths/front4.path rename to src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot5.path index f9513c4b..ebc6628e 100644 --- a/src/main/deploy/pathplanner/paths/front4.path +++ b/src/main/deploy/pathplanner/paths/FrontRow4NoteAutoFarShoot5.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FrontRow4NoteAuto", + "folder": "FrontRow4NoteAutoFarShoot", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/PIDAuto0.path b/src/main/deploy/pathplanner/paths/PIDAuto0.path deleted file mode 100644 index 9392fe59..00000000 --- a/src/main/deploy/pathplanner/paths/PIDAuto0.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.6306711948340775, - "y": 2.359441488479177 - }, - "prevControl": null, - "nextControl": { - "x": 2.3521533028814288, - "y": 1.90120285228694 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.385627673868176, - "y": 1.8622038194195158 - }, - "prevControl": { - "x": 2.8688904883748028, - "y": 1.6867081715161052 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "PIDfirstcollection" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 400.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 44.16968451374187, - "rotateFast": false - }, - "reversed": false, - "folder": "PIDAuto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PIDAuto1.path b/src/main/deploy/pathplanner/paths/PIDAuto1.path deleted file mode 100644 index 305a9438..00000000 --- a/src/main/deploy/pathplanner/paths/PIDAuto1.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 3.385627673868176, - "y": 1.8622038194195158 - }, - "prevControl": null, - "nextControl": { - "x": 3.999862441530111, - "y": 1.8427043029858021 - }, - "isLocked": false, - "linkedName": "PIDfirstcollection" - }, - { - "anchor": { - "x": 4.818842131746022, - "y": 1.40396518322728 - }, - "prevControl": { - "x": 4.311854704469504, - "y": 1.5014627653958412 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 400.0, - "maxAngularAcceleration": 400.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -45.744059202888785, - "rotateFast": false - }, - "reversed": false, - "folder": "PIDAuto", - "previewStartingState": { - "rotation": 24.102234501160947, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront4.path similarity index 91% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront4.path index 7956ff73..335192c9 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront4.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "eee" + "linkedName": "SourceSide3NoteAutoFrontSecondCollection" } ], "rotationTargets": [], @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "SourceSide3NoteAutoRight", + "folder": "SourceSide3NoteAutoFront", "previewStartingState": { "rotation": -60.25511870305777, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront5.path similarity index 87% rename from src/main/deploy/pathplanner/paths/New New Path.path rename to src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront5.path index 2753493a..9f918b1b 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoFront5.path @@ -12,7 +12,7 @@ "y": 4.151424827066179 }, "isLocked": false, - "linkedName": "eee" + "linkedName": "SourceSide3NoteAutoFrontSecondCollection" }, { "anchor": { @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": -10.340000000000032, + "rotation": -10.370000000000005, "rotateFast": false }, "reversed": false, - "folder": "SourceSide3NoteAutoRight", + "folder": "SourceSide3NoteAutoFront", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path index d12ff718..74276a9e 100644 --- a/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path +++ b/src/main/deploy/pathplanner/paths/SourceSide3NoteAutoRight4.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.8648132671253284, - "y": 3.6561526804729216 + "x": 2.835933559191062, + "y": 1.474204638550123 }, "isLocked": false, "linkedName": "SourceSide3NoteAutoRightSecondFeed" @@ -20,8 +20,8 @@ "y": 0.8185908109707958 }, "prevControl": { - "x": 6.492745065262509, - "y": 1.4734634597515983 + "x": 6.304650355779585, + "y": 1.170691918848627 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -22.619864947542787, + "rotation": -8.458072159819006, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/test1.path b/src/main/deploy/pathplanner/paths/test1.path index 70026f97..cc487614 100644 --- a/src/main/deploy/pathplanner/paths/test1.path +++ b/src/main/deploy/pathplanner/paths/test1.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "1" + "linkedName": "testAuto1" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/test2.path b/src/main/deploy/pathplanner/paths/test2.path index b726a7eb..ac373394 100644 --- a/src/main/deploy/pathplanner/paths/test2.path +++ b/src/main/deploy/pathplanner/paths/test2.path @@ -12,7 +12,7 @@ "y": 6.0692661556263285 }, "isLocked": false, - "linkedName": "1" + "linkedName": "testAuto1" }, { "anchor": { From 1d6e501349b14e97454b68099a3c2c86fe9aa603 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 29 Oct 2024 12:46:35 +0200 Subject: [PATCH 207/261] removed empty commands --- .../paths/5NoteAutoAroundStage1.path | 6 ------ .../paths/5NoteAutoAroundStage2.path | 20 +------------------ .../paths/5NoteAutoAroundStage3.path | 6 ------ .../paths/5NoteAutoAroundStage4.path | 20 +------------------ .../paths/5NoteAutoAroundStage5.path | 6 ------ .../paths/5NoteAutoAroundStage6.path | 20 +------------------ .../paths/5NoteAutoAroundStage7.path | 6 ------ .../paths/5NoteAutoAroundStage8.path | 20 +------------------ .../deploy/pathplanner/paths/7NoteAuto6.path | 20 +------------------ .../deploy/pathplanner/paths/7NoteAuto7.path | 6 ------ .../deploy/pathplanner/paths/7NoteAuto8.path | 20 +------------------ .../deploy/pathplanner/paths/7NoteAuto9.path | 6 ------ .../paths/MiddleRush3NoteAuto2.path | 6 ------ .../paths/MiddleRush3NoteAuto3.path | 20 +------------------ .../paths/MiddleRush3NoteAuto4.path | 6 ------ .../paths/MiddleRush3NoteAuto5.path | 20 +------------------ .../paths/MiddleRush3NoteAuto6.path | 6 ------ .../paths/MiddleRush3NoteAuto7.path | 20 +------------------ .../paths/MiddleRush3NoteAuto8.path | 6 ------ .../paths/MiddleRush3NoteAuto9.path | 20 +------------------ .../paths/SpeakerSide3NoteAuto2.path | 6 ------ .../paths/SpeakerSide3NoteAuto3.path | 20 +------------------ .../paths/SpeakerSide3NoteAuto4.path | 6 ------ .../paths/SpeakerSide3NoteAuto5.path | 20 +------------------ .../paths/SpeakerSide3NoteAuto6.path | 6 ------ .../paths/SpeakerSide3NoteAuto7.path | 20 +------------------ .../pathplanner/paths/Zayde3NoteAuto3.path | 20 +------------------ .../pathplanner/paths/Zayde3NoteAuto4.path | 6 ------ .../pathplanner/paths/Zayde3NoteAuto5.path | 20 +------------------ .../pathplanner/paths/Zayde3NoteAuto6.path | 6 ------ .../pathplanner/paths/Zayde3NoteAuto7.path | 20 +------------------ 31 files changed, 16 insertions(+), 394 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index b4215353..f12e79e3 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -65,12 +65,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path index cf57fdc5..004aba22 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage2.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index b6b1cb09..6525e05d 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path index 3c2b8581..3ea93ec7 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage4.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index ab44e6b6..7397ee31 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path index 7288ab90..80bb5fda 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage6.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index f249342b..6a0f735a 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -38,12 +38,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path index 8dd1ed2c..fbeb59a4 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage8.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligningToNote", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto6.path b/src/main/deploy/pathplanner/paths/7NoteAuto6.path index 80ec294b..500dfd0f 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto6.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index e98d8f9b..fe3e05d1 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto8.path b/src/main/deploy/pathplanner/paths/7NoteAuto8.path index 45a0f9fd..82b9c1b9 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto8.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Stopaligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index 039ea4b0..40014b08 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index b50df5e7..d5b0f783 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path index 515ed226..1086a5fd 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index 028c2550..76e4475f 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path index fce074dc..85825320 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 104dfe3b..20ecd5ec 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path index c697788f..7aa818ba 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto7.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index f00bdb63..481619ab 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -38,12 +38,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path index f649532f..4c96d09a 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto9.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index 4b09d79c..31885d55 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path index ee1ec14d..5a791c52 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index f18342bf..85716330 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path index 154a6672..3423b676 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index a18d5a3a..f43ffeb6 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -38,12 +38,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path index 8aba8037..0fd50f8c 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto7.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path index 8af7db1f..74a17c7e 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto3.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index 4524d439..9a65d6e8 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -44,12 +44,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path index b26a6dd2..357406d4 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index 4f35b982..63cc2089 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -38,12 +38,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": null - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path index 5306e8e1..e44da0e0 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto7.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopAligning", - "waypointRelativePos": 0.25, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": null - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, From 72eeddfa902e51677d909bee1503b80fd08e6c80 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 29 Oct 2024 13:00:37 +0200 Subject: [PATCH 208/261] Cleaned vision stuff --- .../robot/constants/FieldConstants.java | 8 +++--- .../apriltagcamera/AprilTagCamera.java | 2 +- .../AprilTagCameraConstants.java | 3 -- .../io/AprilTagPhotonCameraIO.java | 28 +++++++++++-------- 4 files changed, 22 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 365e838c..41219e8a 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -3,17 +3,17 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.*; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; import java.io.IOException; import java.util.HashMap; +import java.util.Objects; public class FieldConstants { private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; + private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; static { @@ -47,7 +47,7 @@ public class FieldConstants { private static HashMap fieldLayoutToTagIdToPoseMap() { final HashMap tagIdToPose = new HashMap<>(); for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - tagIdToPose.put(aprilTag.ID, aprilTag.pose); + Objects.requireNonNull(tagIdToPose.put(aprilTag.ID, aprilTag.pose)).transformBy(TAG_OFFSET); return tagIdToPose; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index d9b8a83e..c33487d1 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -128,7 +128,7 @@ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { } private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading) { - final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); + final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]); if (bestTagPose == null) return null; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index bea1b24e..deef468f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -1,15 +1,12 @@ package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; import java.util.function.Function; public class AprilTagCameraConstants { - public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2.5; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index ee0b54e8..d6b080b9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -91,27 +91,33 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result) { return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(getBestTarget(result).getFiducialId()); - final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(getBestTarget(result).getFiducialId()); final Transform3d targetToCamera = getBestTarget(result).getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } private int[] getVisibleTagIDs(PhotonPipelineResult result) { -// final int[] visibleTagIDs = new int[result.getTargets().size()]; -// for (int i = 0; i < visibleTagIDs.length; i++) -// visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); -// return visibleTagIDs; - return new int[]{getBestTarget(result).getFiducialId()}; + final int[] visibleTagIDs = new int[result.getTargets().size()]; + visibleTagIDs[0] = getBestTarget(result).getFiducialId(); + int idAddition = 1; + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = result.getTargets().get(i).getFiducialId(); + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + visibleTagIDs[i + idAddition] = currentID; + } + return visibleTagIDs; } private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { - var x = result.getBestTarget(); + PhotonTrackedTarget bestTarget = result.getBestTarget(); for (PhotonTrackedTarget target : result.getTargets()) { - if (target.getArea() > x.area) - x = target; + if (target.getArea() > bestTarget.area) + bestTarget = target; } - return x; + return bestTarget; } private double getDistanceFromBestTag(PhotonPipelineResult result) { From 75275fd973b5189b62f3667ccaafa0bac6714c3d Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 29 Oct 2024 13:08:23 +0200 Subject: [PATCH 209/261] cleaned up stuff --- .../frc/trigon/robot/commands/factories/AmpCommands.java | 2 +- .../frc/trigon/robot/constants/OperatorConstants.java | 1 - .../frc/trigon/robot/subsystems/pitcher/Pitcher.java | 2 -- .../frc/trigon/robot/subsystems/shooter/Shooter.java | 7 ------- .../trigon/robot/subsystems/shooter/ShooterCommands.java | 9 --------- 5 files changed, 1 insertion(+), 20 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index bef986f1..32356fa7 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -64,7 +64,7 @@ private static Command getPrepareForAmpCommand() { } private static Command getFeedToAmpCommand() { - return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()).alongWith(new InstantCommand(() -> IS_FEEDING_NOTE = true)), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch());// && RobotContainer.AMP_ALIGNER.atTargetState()); + return GeneralCommands.runWhen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.FEED_AMP).alongWith(GeneralCommands.getVisualizeNoteShootingCommand()).alongWith(new InstantCommand(() -> IS_FEEDING_NOTE = true)), () -> RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.PITCHER.atTargetPitch() && RobotContainer.AMP_ALIGNER.atTargetState()); } private static Command getPathfindToAmpCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 3088e16a..2e7bc27c 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -30,7 +30,6 @@ public class OperatorConstants { TURN_AUTONOMOUS_NOTE_ALIGNING_ON_TRIGGER = OPERATOR_CONTROLLER.o(), TURN_AUTONOMOUS_NOTE_ALIGNING_OFF_TRIGGER = OPERATOR_CONTROLLER.p(), LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), -// LED_INDICATION_TRIGGER = DRIVER_CONTROLLER.x(), CLIMB_TRIGGER = OPERATOR_CONTROLLER.c(), OVERRIDE_IS_CLIMBING_TRIGGER = OPERATOR_CONTROLLER.i(), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index 2684f43b..77d70a65 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -58,8 +58,6 @@ public void stop() { public void updatePeriodically() { masterMotor.update(); followerMotor.update(); - Logger.recordOutput("MasterPitcherMotorVotlage", masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); - Logger.recordOutput("FollowerPitcherMotorVoltage", followerMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); encoder.update(); Logger.recordOutput("PitcherAngleDegrees", Conversions.rotationsToDegrees(encoder.getSignal(CANcoderSignal.POSITION))); } diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java index f48d6dc5..822f3359 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java @@ -1,6 +1,5 @@ package frc.trigon.robot.subsystems.shooter; -import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import edu.wpi.first.units.Measure; @@ -20,7 +19,6 @@ public class Shooter extends MotorSubsystem { leftMotor = ShooterConstants.LEFT_MOTOR; private final VelocityTorqueCurrentFOC velocityRequest = new VelocityTorqueCurrentFOC(0); private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); - private final StaticBrake staticBrakeRequest = new StaticBrake(); private double targetRightVelocityRotationsPerSecond = 0, targetLeftVelocityRotationsPerSecond = 0; @@ -104,11 +102,6 @@ void setTargetVelocity(double targetRightVelocityRotationsPerSecond, double targ setTargetLeftVelocity(targetLeftVelocityRotationsPerSecond); } - void sendStaticBrakeRequest() { - rightMotor.setControl(staticBrakeRequest); - leftMotor.setControl(staticBrakeRequest); - } - private boolean atVelocity(double currentVelocity, double targetVelocity) { return Math.abs(currentVelocity - targetVelocity) < ShooterConstants.VELOCITY_TOLERANCE; } diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java index 528a76a8..291ce691 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterCommands.java @@ -40,15 +40,6 @@ public static Command getSetTargetVelocityCommand(double targetRightVelocityRota ); } - public static Command getSendStaticBreakRequestCommand() { - return new StartEndCommand( - RobotContainer.SHOOTER::sendStaticBrakeRequest, - () -> { - }, - RobotContainer.SHOOTER - ); - } - public static Command getStopCommand() { return new StartEndCommand( RobotContainer.SHOOTER::stop, From 0cb5f475de8afa76976d6792149c797046888d52 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 29 Oct 2024 13:14:54 +0200 Subject: [PATCH 210/261] Simple cleaning --- .../frc/trigon/robot/commands/factories/GeneralCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 1c36d527..5f467fae 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -45,7 +45,7 @@ public static Command getNoteCollectionCommand() { new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).onlyIf(() -> !RobotContainer.INTAKE.hasNote()) + ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).unless(RobotContainer.INTAKE::hasNote) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); } From 7c62b0cd5b71234caa3511fb64e52d8e881c9faa Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 29 Oct 2024 13:42:45 +0200 Subject: [PATCH 211/261] Fixed bug and removed one led for alignment --- src/main/java/frc/trigon/robot/constants/FieldConstants.java | 3 +-- .../trigon/robot/subsystems/ledstrip/LEDStripConstants.java | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 41219e8a..e41dfce8 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -9,7 +9,6 @@ import java.io.IOException; import java.util.HashMap; -import java.util.Objects; public class FieldConstants { private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; @@ -47,7 +46,7 @@ public class FieldConstants { private static HashMap fieldLayoutToTagIdToPoseMap() { final HashMap tagIdToPose = new HashMap<>(); for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - Objects.requireNonNull(tagIdToPose.put(aprilTag.ID, aprilTag.pose)).transformBy(TAG_OFFSET); + tagIdToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); return tagIdToPose; } } diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index dbd2e301..882bf5ae 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -11,7 +11,7 @@ public class LEDStripConstants { private static final int PORT = 9; private static final int RIGHT_CLIMBER_NUMBER_OF_LEDS = 22, - LEFT_CLIMBER_NUMBER_OF_LEDS = 23; + LEFT_CLIMBER_NUMBER_OF_LEDS = 22; private static final boolean RIGHT_CLIMBER_INVERTED = false, LEFT_CLIMBER_INVERTED = false; From 10cd7204c2327394c1e10d832bea62e1dc6d8fe8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 29 Oct 2024 14:03:37 +0200 Subject: [PATCH 212/261] Fixed LED stuff and made better intake LED commands and fixed intake in simulation --- .../robot/commands/AlignToNoteCommand.java | 7 ++++--- .../commands/factories/GeneralCommands.java | 2 +- .../trigon/robot/subsystems/intake/Intake.java | 7 +++++-- .../robot/subsystems/intake/IntakeConstants.java | 16 ++++++++++++++-- 4 files changed, 24 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index 3b117eb7..c53ee413 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -11,6 +11,7 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -33,8 +34,8 @@ public AlignToNoteCommand() { private Command getSetCurrentLEDColorCommand() { return GeneralCommands.getContinuousConditionalCommand( - LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS), - LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), + LEDStripCommands.getBreatheCommand(Color.kGreen, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS), + LEDStripCommands.getBreatheCommand(Color.kRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS), CAMERA::hasTargets ).asProxy(); } @@ -48,7 +49,7 @@ private Command getDriveWhileAligningToNoteCommand() { } private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { - final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().rotateBy(Rotation2d.fromDegrees(90)); final double xValue = CommandConstants.calculateDriveStickAxisValue(xPower); final double yValue = CommandConstants.calculateDriveStickAxisValue(yPower); diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 5f467fae..8128a66d 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -43,7 +43,7 @@ public static Command getClimbCommand() { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), - LEDStripCommands.getStaticColorCommand(Color.kOrangeRed, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + LEDStripCommands.getBreatheCommand(Color.kOrangeRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).unless(RobotContainer.INTAKE::hasNote) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 133a05ae..cef02273 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; @@ -69,6 +70,8 @@ void setTargetState(IntakeConstants.IntakeState targetState) { getFeedingIndicationLEDsCommand().schedule(); if (targetState == IntakeConstants.IntakeState.EJECT) getEjectingIndicationLEDsCommand().schedule(); + if (targetState == IntakeConstants.IntakeState.STOP) + CommandConstants.DEFAULT_LEDS_COMMAND.schedule(); setTargetVoltage(targetState.voltage); } @@ -91,10 +94,10 @@ private Command getCollectionIndicationLEDsCommand() { } private Command getFeedingIndicationLEDsCommand() { - return LEDStripCommands.getBreatheCommand(Color.kPurple, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, false, LEDStrip.LED_STRIPS); + return LEDStripCommands.getBreatheCommand(Color.kPurple, IntakeConstants.FEEDING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.FEEDING_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS); } private Command getEjectingIndicationLEDsCommand() { - return LEDStripCommands.getBreatheCommand(Color.kBlue, 5, IntakeConstants.FEEDING_INDICATION_BREATHING_TIME_SECONDS, false, LEDStrip.LED_STRIPS); + return LEDStripCommands.getBreatheCommand(Color.kBlue, IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 08a53dbd..e9449ed5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.trigon.robot.misc.objectdetectioncamera.SimulationObjectDetectionCameraIO; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -62,7 +63,7 @@ public class IntakeConstants { static final double NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS = 0; static final BooleanEvent HAS_NOTE_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), - () -> DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS + () -> !RobotHardwareStats.isSimulation() && (DISTANCE_SENSOR.getScaledValue() < NOTE_DISTANCE_THRESHOLD_CENTIMETERS) ).debounce(NOTE_DETECTION_CONFIRMATION_DELAY_SECONDS); private static final double NOTE_COLLECTION_CURRENT = 50; private static final double NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.1; @@ -73,7 +74,18 @@ public class IntakeConstants { static final double COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; - public static final double FEEDING_INDICATION_BREATHING_TIME_SECONDS = 0.3; + public static final int + FEEDING_INDICATION_BREATHING_LEDS_AMOUNT = 4, + EJECTING_INDICATION_BREATHING_LEDS_AMOUNT = 6, + INTAKE_INDICATION_BREATHING_LEDS_AMOUNT = 4; + public static final double + FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.3, + EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 1, + INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.7; + public static final boolean + FEEDING_INDICATION_BREATHING_SHOULD_LOOP = false, + EJECTING_INDICATION_BREATHING_SHOULD_LOOP = true, + INTAKE_INDICATION_BREATHING_SHOULD_LOOP = true; static { configureMasterMotor(); From ccbf53c45b04a748caf21d16780aae1bed87e67b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 30 Oct 2024 11:19:05 +0200 Subject: [PATCH 213/261] Fixed `AlignToNote.java` --- .../java/frc/trigon/robot/commands/AlignToNoteCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index c53ee413..c4899368 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -42,14 +42,14 @@ private Command getSetCurrentLEDColorCommand() { private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftX(), OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); } private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { - final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle().rotateBy(Rotation2d.fromDegrees(90)); + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); final double xValue = CommandConstants.calculateDriveStickAxisValue(xPower); final double yValue = CommandConstants.calculateDriveStickAxisValue(yPower); From c9be8c0beb9a81da38a925a228375a0b82152de6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 30 Oct 2024 11:23:43 +0200 Subject: [PATCH 214/261] Made blink more versatile --- .../frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 73f32939..87d6060b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -23,7 +23,7 @@ public class LEDStrip extends SubsystemBase { static { GeneralCommands.getDelayedCommand( 1, - () -> LEDStripConstants.LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getBlinkingCommand(Color.kRed, LEDStripConstants.LOW_BATTERY_BLINKING_INTERVAL_SECONDS, LED_STRIPS).withTimeout(LEDStripConstants.LOW_BATTERY_BLINKING_TIME_SECONDS)) + () -> LEDStripConstants.LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getBlinkingCommand(Color.kRed, Color.kBlack, LEDStripConstants.LOW_BATTERY_BLINKING_INTERVAL_SECONDS, LED_STRIPS).withTimeout(LEDStripConstants.LOW_BATTERY_BLINKING_TIME_SECONDS)) ); } @@ -54,16 +54,16 @@ void clearLedColors() { staticColor(Color.kBlack); } - void blink(Color color, double blinkingIntervalSeconds) { + void blink(Color firstColor, Color secondColor, double blinkingIntervalSeconds) { double currentTime = Timer.getFPGATimestamp(); if (currentTime - lastBlinkTime > blinkingIntervalSeconds) { lastBlinkTime = currentTime; areLEDsOnForBlinking = !areLEDsOnForBlinking; } if (areLEDsOnForBlinking) - staticColor(color); + staticColor(firstColor); else - clearLedColors(); + staticColor(secondColor); } void staticColor(Color color) { From 0f844378509d156dcf5f3e21a0e56aa00dcbf99e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 30 Oct 2024 11:23:58 +0200 Subject: [PATCH 215/261] These didn't go with the previous commit for some reason --- src/main/java/frc/trigon/robot/subsystems/intake/Intake.java | 2 +- .../trigon/robot/subsystems/ledstrip/LEDStripCommands.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index cef02273..e965e84d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -90,7 +90,7 @@ void indicateCollection() { } private Command getCollectionIndicationLEDsCommand() { - return LEDStripCommands.getBlinkingCommand(Color.kOrangeRed, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); + return LEDStripCommands.getBlinkingCommand(Color.kOrangeRed, Color.kBlack, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); } private Command getFeedingIndicationLEDsCommand() { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 5672c760..c1afada8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -17,9 +17,9 @@ public static Command getStaticColorCommand(Color color, LEDStrip... ledStrips) ).ignoringDisable(true); } - public static Command getBlinkingCommand(Color color, double blinkingIntervalSeconds, LEDStrip... ledStrips) { + public static Command getBlinkingCommand(Color firstColor, Color secondColor, double blinkingIntervalSeconds, LEDStrip... ledStrips) { return new ExecuteEndCommand( - () -> runForLEDs((LEDStrip -> LEDStrip.blink(color, blinkingIntervalSeconds)), ledStrips), + () -> runForLEDs((LEDStrip -> LEDStrip.blink(firstColor, secondColor, blinkingIntervalSeconds)), ledStrips), () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), ledStrips ).ignoringDisable(true); From 7c1e11b3a9b5a8c4b00b1bcc7f7bdbe2f9343f94 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 30 Oct 2024 11:24:47 +0200 Subject: [PATCH 216/261] Consistency is key --- .../frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 4 ++-- .../robot/subsystems/ledstrip/LEDStripCommands.java | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 87d6060b..3a2e66bc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -50,7 +50,7 @@ public int getNumberOfLEDS() { return numberOfLEDs; } - void clearLedColors() { + void clearLEDColors() { staticColor(Color.kBlack); } @@ -88,7 +88,7 @@ void resetLEDSettings() { } void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop) { - clearLedColors(); + clearLEDColors(); double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; double currentTime = Timer.getFPGATimestamp(); if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index c1afada8..19d5f1f9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -12,7 +12,7 @@ public class LEDStripCommands { public static Command getStaticColorCommand(Color color, LEDStrip... ledStrips) { return new ExecuteEndCommand( () -> runForLEDs((LEDStrip -> LEDStrip.staticColor(color)), ledStrips), - () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), ledStrips ).ignoringDisable(true); } @@ -20,7 +20,7 @@ public static Command getStaticColorCommand(Color color, LEDStrip... ledStrips) public static Command getBlinkingCommand(Color firstColor, Color secondColor, double blinkingIntervalSeconds, LEDStrip... ledStrips) { return new ExecuteEndCommand( () -> runForLEDs((LEDStrip -> LEDStrip.blink(firstColor, secondColor, blinkingIntervalSeconds)), ledStrips), - () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), ledStrips ).ignoringDisable(true); } @@ -28,7 +28,7 @@ public static Command getBlinkingCommand(Color firstColor, Color secondColor, do public static Command getRainbowCommand(LEDStrip... ledStrips) { return new ExecuteEndCommand( () -> runForLEDs((LEDStrip::rainbow), ledStrips), - () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), ledStrips ).ignoringDisable(true); } @@ -38,7 +38,7 @@ public static Command getBreatheCommand(Color color, int breathingLEDs, double c () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), () -> { runForLEDs(LEDStrip::resetLEDSettings, ledStrips); - runForLEDs(LEDStrip::clearLedColors, ledStrips); + runForLEDs(LEDStrip::clearLEDColors, ledStrips); }, ledStrips ).ignoringDisable(true); @@ -46,7 +46,7 @@ public static Command getBreatheCommand(Color color, int breathingLEDs, double c public static Command getThreeSectionColorCommand(Supplier firstSectionColor, Supplier secondSectionColor, Supplier thirdSectionColor, LEDStrip... ledStrips) { return new InitExecuteCommand( - () -> runForLEDs(LEDStrip::clearLedColors, ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), () -> runForLEDs((LEDStrip) -> LEDStrip.threeSectionColor(firstSectionColor.get(), secondSectionColor.get(), thirdSectionColor.get()), ledStrips), ledStrips ).ignoringDisable(true); From 9e5b4c41aff536a25fac2c6435a9bf9d58a1c643 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 30 Oct 2024 11:26:46 +0200 Subject: [PATCH 217/261] Minor cleaning to LEDs --- .../robot/subsystems/ledstrip/LEDStrip.java | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 3a2e66bc..ca6e8776 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -50,6 +50,14 @@ public int getNumberOfLEDS() { return numberOfLEDs; } + void resetLEDSettings() { + lastBreatheLED = indexOffset; + lastBreatheMovementTime = Timer.getFPGATimestamp(); + rainbowFirstPixelHue = 0; + areLEDsOnForBlinking = false; + lastBlinkTime = 0; + } + void clearLEDColors() { staticColor(Color.kBlack); } @@ -79,14 +87,6 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - void resetLEDSettings() { - lastBreatheLED = indexOffset; - lastBreatheMovementTime = Timer.getFPGATimestamp(); - rainbowFirstPixelHue = 0; - areLEDsOnForBlinking = false; - lastBlinkTime = 0; - } - void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop) { clearLEDColors(); double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; @@ -115,17 +115,17 @@ void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); } + private void setLEDColors(Color color, int startIndex, int endIndex) { + for (int i = 0; i <= endIndex - startIndex; i++) + LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); + } + private void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { setLEDColors(inverted ? thirdSectionColor : firstSectionColor, 0, ledsPerSection); setLEDColors(secondSectionColor, ledsPerSection, ledsPerSection * 2); setLEDColors(inverted ? firstSectionColor : thirdSectionColor, ledsPerSection * 2, numberOfLEDs - 1); } - private void setLEDColors(Color color, int startIndex, int endIndex) { - for (int i = 0; i <= endIndex - startIndex; i++) - LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); - } - private void addLEDStripToLEDStripsArray(LEDStrip ledStrip) { final LEDStrip[] newLEDStrips = new LEDStrip[LED_STRIPS.length + 1]; System.arraycopy(LED_STRIPS, 0, newLEDStrips, 0, LED_STRIPS.length); From 126be5b80accb2cffa15eb5612d7f5c4daa4a3fa Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 30 Oct 2024 22:39:55 +0200 Subject: [PATCH 218/261] Changes to LED stuff --- .../frc/trigon/robot/commands/CommandConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/Intake.java | 2 +- .../robot/subsystems/intake/IntakeConstants.java | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 1ab4795c..ee72064a 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -85,7 +85,7 @@ public class CommandConstants { SHOULD_ALIGN_TO_NOTE = false; Logger.recordOutput("ShouldAlignToNote", false); }).ignoringDisable(true), - DEFAULT_LEDS_COMMAND = LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), + DEFAULT_LEDS_COMMAND = LEDStripCommands.getBreatheCommand(Color.kAqua, 7, 1.5, true, LEDStrip.LED_STRIPS), DEFAULT_INTAKE_COMMAND = IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOP), DEFAULT_CLIMBER_COMMAND = ClimberCommands.getStopCommand(), MOVE_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index e965e84d..35d00fbd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -98,6 +98,6 @@ private Command getFeedingIndicationLEDsCommand() { } private Command getEjectingIndicationLEDsCommand() { - return LEDStripCommands.getBreatheCommand(Color.kBlue, IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS); + return LEDStripCommands.getBreatheCommand(Color.kDarkBlue, IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index e9449ed5..17578056 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -75,13 +75,13 @@ public class IntakeConstants { COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; public static final int - FEEDING_INDICATION_BREATHING_LEDS_AMOUNT = 4, - EJECTING_INDICATION_BREATHING_LEDS_AMOUNT = 6, - INTAKE_INDICATION_BREATHING_LEDS_AMOUNT = 4; + FEEDING_INDICATION_BREATHING_LEDS_AMOUNT = 5, + EJECTING_INDICATION_BREATHING_LEDS_AMOUNT = 9, + INTAKE_INDICATION_BREATHING_LEDS_AMOUNT = 5; public static final double - FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.3, + FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.05, EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 1, - INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.7; + INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.2; public static final boolean FEEDING_INDICATION_BREATHING_SHOULD_LOOP = false, EJECTING_INDICATION_BREATHING_SHOULD_LOOP = true, From 0ef7dfe94f6c7dcd9202e76464c9696a3f4da669 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 31 Oct 2024 00:14:13 +0200 Subject: [PATCH 219/261] Implemented clean breathe transitions --- .../robot/subsystems/ledstrip/LEDStripCommands.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 19d5f1f9..44de3d51 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.InitExecuteCommand; @@ -34,12 +35,14 @@ public static Command getRainbowCommand(LEDStrip... ledStrips) { } public static Command getBreatheCommand(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, LEDStrip... ledStrips) { - return new ExecuteEndCommand( - () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), + return new FunctionalCommand( () -> { - runForLEDs(LEDStrip::resetLEDSettings, ledStrips); - runForLEDs(LEDStrip::clearLEDColors, ledStrips); + if (!shouldLoop) + runForLEDs(LEDStrip::resetLEDSettings, ledStrips); }, + () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), + (interrupted) -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), + () -> false, ledStrips ).ignoringDisable(true); } From 6537ef55a448ec85c5d495d45b99a6da05f09b9f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 31 Oct 2024 00:35:51 +0200 Subject: [PATCH 220/261] Added inverted for breathe LED command --- .../frc/trigon/robot/commands/AlignToNoteCommand.java | 4 ++-- .../frc/trigon/robot/commands/CommandConstants.java | 2 +- .../robot/commands/factories/GeneralCommands.java | 2 +- .../frc/trigon/robot/subsystems/intake/Intake.java | 4 ++-- .../robot/subsystems/intake/IntakeConstants.java | 5 ++++- .../trigon/robot/subsystems/ledstrip/LEDStrip.java | 11 +++++++---- .../robot/subsystems/ledstrip/LEDStripCommands.java | 4 ++-- 7 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index c4899368..db3338c8 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -34,8 +34,8 @@ public AlignToNoteCommand() { private Command getSetCurrentLEDColorCommand() { return GeneralCommands.getContinuousConditionalCommand( - LEDStripCommands.getBreatheCommand(Color.kGreen, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS), - LEDStripCommands.getBreatheCommand(Color.kRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS), + LEDStripCommands.getBreatheCommand(Color.kGreen, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.INTAKE_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS), + LEDStripCommands.getBreatheCommand(Color.kRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.INTAKE_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS), CAMERA::hasTargets ).asProxy(); } diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index ee72064a..37484b5c 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -85,7 +85,7 @@ public class CommandConstants { SHOULD_ALIGN_TO_NOTE = false; Logger.recordOutput("ShouldAlignToNote", false); }).ignoringDisable(true), - DEFAULT_LEDS_COMMAND = LEDStripCommands.getBreatheCommand(Color.kAqua, 7, 1.5, true, LEDStrip.LED_STRIPS), + DEFAULT_LEDS_COMMAND = LEDStripCommands.getBreatheCommand(Color.kLightSeaGreen, 7, 1.5, true, false, LEDStrip.LED_STRIPS), DEFAULT_INTAKE_COMMAND = IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOP), DEFAULT_CLIMBER_COMMAND = ClimberCommands.getStopCommand(), MOVE_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index 8128a66d..c7e49943 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -43,7 +43,7 @@ public static Command getClimbCommand() { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), - LEDStripCommands.getBreatheCommand(Color.kOrangeRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + LEDStripCommands.getBreatheCommand(Color.kOrangeRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.INTAKE_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).unless(RobotContainer.INTAKE::hasNote) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 35d00fbd..2e7e0ddf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -94,10 +94,10 @@ private Command getCollectionIndicationLEDsCommand() { } private Command getFeedingIndicationLEDsCommand() { - return LEDStripCommands.getBreatheCommand(Color.kPurple, IntakeConstants.FEEDING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.FEEDING_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS); + return LEDStripCommands.getBreatheCommand(Color.kPurple, IntakeConstants.FEEDING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.FEEDING_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.FEEDING_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS); } private Command getEjectingIndicationLEDsCommand() { - return LEDStripCommands.getBreatheCommand(Color.kDarkBlue, IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, LEDStrip.LED_STRIPS); + return LEDStripCommands.getBreatheCommand(Color.kDarkBlue, IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.EJECTING_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 17578056..a867f978 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -84,8 +84,11 @@ public class IntakeConstants { INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.2; public static final boolean FEEDING_INDICATION_BREATHING_SHOULD_LOOP = false, + FEEDING_INDICATION_BREATHING_IS_INVERTED = false, EJECTING_INDICATION_BREATHING_SHOULD_LOOP = true, - INTAKE_INDICATION_BREATHING_SHOULD_LOOP = true; + EJECTING_INDICATION_BREATHING_IS_INVERTED = true, + INTAKE_INDICATION_BREATHING_SHOULD_LOOP = true, + INTAKE_INDICATION_BREATHING_IS_INVERTED = false; static { configureMasterMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index ca6e8776..4d24b2d5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -87,20 +87,23 @@ void rainbow() { rainbowFirstPixelHue %= 180; } - void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop) { + void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, boolean inverted) { clearLEDColors(); double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; double currentTime = Timer.getFPGATimestamp(); if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { lastBreatheMovementTime = currentTime; - lastBreatheLED++; + if (inverted) + lastBreatheLED--; + else + lastBreatheLED++; } - if (lastBreatheLED >= numberOfLEDs + indexOffset) { + if (inverted ? (lastBreatheLED < indexOffset) : (lastBreatheLED >= numberOfLEDs + indexOffset)) { if (!shouldLoop) { CommandConstants.DEFAULT_LEDS_COMMAND.schedule(); return; } - lastBreatheLED = indexOffset; + lastBreatheLED = inverted ? indexOffset + numberOfLEDs : indexOffset; } for (int i = 0; i < breathingLEDs; i++) { if (lastBreatheLED - i >= indexOffset && lastBreatheLED - i < indexOffset + numberOfLEDs) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 44de3d51..44d4a614 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -34,13 +34,13 @@ public static Command getRainbowCommand(LEDStrip... ledStrips) { ).ignoringDisable(true); } - public static Command getBreatheCommand(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, LEDStrip... ledStrips) { + public static Command getBreatheCommand(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, boolean inverted, LEDStrip... ledStrips) { return new FunctionalCommand( () -> { if (!shouldLoop) runForLEDs(LEDStrip::resetLEDSettings, ledStrips); }, - () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop), ledStrips), + () -> runForLEDs((LEDStrip) -> LEDStrip.breathe(color, breathingLEDs, cycleTimeSeconds, shouldLoop, inverted), ledStrips), (interrupted) -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), () -> false, ledStrips From d68fb49b7a949d7dc6cde379c89e8a6359edfd64 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 31 Oct 2024 14:03:17 +0200 Subject: [PATCH 221/261] Fixed amp simulation animation --- .../frc/trigon/robot/commands/VisualizeNoteShootingCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java index 627282d5..41f4810f 100644 --- a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java +++ b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java @@ -97,7 +97,7 @@ private void configureNoteInAmpStats() { FieldConstants.AMP_TRANSLATION.get().getY(), notePose.getZ()); initialXYVelocity = new Translation2d(0, 0); - initialZVelocity = 2; + initialZVelocity = 0; } private double getStartingTangentialVelocity() { From f76c74efeeec7987b31f8ce5af61a700be31dff4 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 1 Nov 2024 10:38:09 +0200 Subject: [PATCH 222/261] added sectionColorCommand. Didn't test it yet though. Will do that later --- .../robot/commands/LEDAutoSetupCommand.java | 20 ++++++++------ .../robot/subsystems/ledstrip/LEDStrip.java | 26 ++++++++++++------- .../subsystems/ledstrip/LEDStripCommands.java | 4 +-- 3 files changed, 31 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java index cf270e1b..b5e3b8ad 100644 --- a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java +++ b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java @@ -22,6 +22,12 @@ public class LEDAutoSetupCommand extends SequentialCommandGroup { private static final double TOLERANCE_METERS = 0.1, TOLERANCE_DEGREES = 2; + private static final int AMOUNT_OF_SECTIONS = 3; + private final Supplier[] LEDColors = new Supplier[] { + () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES), + () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS), + () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS) + }; private final Supplier autoName; private Pose2d autoStartPose; @@ -35,16 +41,14 @@ public LEDAutoSetupCommand(Supplier autoName) { addCommands( getUpdateAutoStartPoseCommand(), - LEDStripCommands.getThreeSectionColorCommand( - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS), + LEDStripCommands.getSectionColorCommand( + AMOUNT_OF_SECTIONS, + LEDColors, LEDStripConstants.RIGHT_CLIMBER_LEDS ).alongWith( - LEDStripCommands.getThreeSectionColorCommand( - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS), - () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS), + LEDStripCommands.getSectionColorCommand( + AMOUNT_OF_SECTIONS, + LEDColors, LEDStripConstants.LEFT_CLIMBER_LEDS ) ) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 4d24b2d5..490039a6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -8,6 +8,8 @@ import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.factories.GeneralCommands; +import java.util.function.Supplier; + public class LEDStrip extends SubsystemBase { public static LEDStrip[] LED_STRIPS = new LEDStrip[0]; private static final AddressableLED LED = LEDStripConstants.LED; @@ -113,9 +115,21 @@ else if (lastBreatheLED - i < indexOffset + numberOfLEDs) } } - void threeSectionColor(Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { - final int ledsPerSection = (int) Math.floor(numberOfLEDs / 3.0); - setThreeSectionColor(ledsPerSection, firstSectionColor, secondSectionColor, thirdSectionColor); + void sectionColor(int amountOfSections, Supplier... colors) { + if (amountOfSections != colors.length) + throw new IllegalArgumentException("Amount of sections must be equal to the amount of colors"); + final int LEDSPerSection = (int) Math.floor(numberOfLEDs / amountOfSections); + setSectionColor(amountOfSections, LEDSPerSection, colors); + } + + private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier... colors) { + if (inverted) { + for (int i = amountOfSections - 1; i >= 0; i--) + setLEDColors(colors[i].get(), LEDSPerSection * i, LEDSPerSection * (i + 1) - 1); + return; + } + for (int i = 0; i < amountOfSections - 1; i++) + setLEDColors(colors[i].get(), LEDSPerSection * i, LEDSPerSection * (i + 1) - 1); } private void setLEDColors(Color color, int startIndex, int endIndex) { @@ -123,12 +137,6 @@ private void setLEDColors(Color color, int startIndex, int endIndex) { LEDStripConstants.LED_BUFFER.setLED(startIndex + indexOffset + i, color); } - private void setThreeSectionColor(int ledsPerSection, Color firstSectionColor, Color secondSectionColor, Color thirdSectionColor) { - setLEDColors(inverted ? thirdSectionColor : firstSectionColor, 0, ledsPerSection); - setLEDColors(secondSectionColor, ledsPerSection, ledsPerSection * 2); - setLEDColors(inverted ? firstSectionColor : thirdSectionColor, ledsPerSection * 2, numberOfLEDs - 1); - } - private void addLEDStripToLEDStripsArray(LEDStrip ledStrip) { final LEDStrip[] newLEDStrips = new LEDStrip[LED_STRIPS.length + 1]; System.arraycopy(LED_STRIPS, 0, newLEDStrips, 0, LED_STRIPS.length); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 44d4a614..8608af8b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -47,10 +47,10 @@ public static Command getBreatheCommand(Color color, int breathingLEDs, double c ).ignoringDisable(true); } - public static Command getThreeSectionColorCommand(Supplier firstSectionColor, Supplier secondSectionColor, Supplier thirdSectionColor, LEDStrip... ledStrips) { + public static Command getSectionColorCommand(int amountOfSections, Supplier[] colors, LEDStrip... ledStrips) { return new InitExecuteCommand( () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), - () -> runForLEDs((LEDStrip) -> LEDStrip.threeSectionColor(firstSectionColor.get(), secondSectionColor.get(), thirdSectionColor.get()), ledStrips), + () -> runForLEDs((LEDStrip) -> LEDStrip.sectionColor(amountOfSections, colors), ledStrips), ledStrips ).ignoringDisable(true); } From 45f97a3879aec606ec35776fac0b466f2724d111 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sat, 2 Nov 2024 19:53:56 +0200 Subject: [PATCH 223/261] fixes sectionColor index and inverted --- .../frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 490039a6..25f659d0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -124,12 +124,12 @@ void sectionColor(int amountOfSections, Supplier... colors) { private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier... colors) { if (inverted) { - for (int i = amountOfSections - 1; i >= 0; i--) - setLEDColors(colors[i].get(), LEDSPerSection * i, LEDSPerSection * (i + 1) - 1); + for (int colorIndex = amountOfSections - 1, LEDIndex = 0; colorIndex >= 0; colorIndex--, LEDIndex++) + setLEDColors(colors[colorIndex].get(), LEDSPerSection * LEDIndex, colorIndex == 0 ? numberOfLEDs - 1 : LEDSPerSection * (LEDIndex + 1) - 1); return; } - for (int i = 0; i < amountOfSections - 1; i++) - setLEDColors(colors[i].get(), LEDSPerSection * i, LEDSPerSection * (i + 1) - 1); + for (int i = 0; i < amountOfSections; i++) + setLEDColors(colors[i].get(), LEDSPerSection * i, i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1); } private void setLEDColors(Color color, int startIndex, int endIndex) { From 6d268e0ce1b481bff9f48d492ad975df0fe5213f Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 3 Nov 2024 09:47:07 +0200 Subject: [PATCH 224/261] simplified sectionColor --- .../java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 25f659d0..c2c654fd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -124,8 +124,8 @@ void sectionColor(int amountOfSections, Supplier... colors) { private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier... colors) { if (inverted) { - for (int colorIndex = amountOfSections - 1, LEDIndex = 0; colorIndex >= 0; colorIndex--, LEDIndex++) - setLEDColors(colors[colorIndex].get(), LEDSPerSection * LEDIndex, colorIndex == 0 ? numberOfLEDs - 1 : LEDSPerSection * (LEDIndex + 1) - 1); + for (int i = 0; i < amountOfSections; i++) + setLEDColors(colors[amountOfSections - i - 1].get(), LEDSPerSection * i, i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1); return; } for (int i = 0; i < amountOfSections; i++) From 2cd1d76f2550837b9ffc7a1e53947858dab26e72 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 3 Nov 2024 10:13:33 +0200 Subject: [PATCH 225/261] fixed 7NoteAuto and SourceSide3NoteAuto --- .../deploy/pathplanner/autos/7NoteAuto.auto | 316 +++++++++++++++--- .../autos/SourceSide3NoteAuto.auto | 122 +++++-- .../robot/subsystems/ledstrip/LEDStrip.java | 4 +- 3 files changed, 377 insertions(+), 65 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/7NoteAuto.auto b/src/main/deploy/pathplanner/autos/7NoteAuto.auto index 42b905bb..bebc1a87 100644 --- a/src/main/deploy/pathplanner/autos/7NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/7NoteAuto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.1882930808115142, - "y": 4.148007169253776 + "x": 0.6751948895821795, + "y": 4.387391197585246 }, - "rotation": -45.77083940144205 + "rotation": -59.76613049448032 }, "command": { "type": "sequential", @@ -58,15 +58,48 @@ } }, { - "type": "path", - "data": { - "pathName": "7NoteAuto1" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto2" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { @@ -83,6 +116,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto2" + } + }, { "type": "wait", "data": { @@ -102,9 +141,48 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto3" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { @@ -124,7 +202,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.4 } }, { @@ -140,9 +218,48 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto4" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { @@ -162,7 +279,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.4 } }, { @@ -178,15 +295,48 @@ } }, { - "type": "path", - "data": { - "pathName": "7NoteAuto5" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto6" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { @@ -203,6 +353,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto6" + } + }, { "type": "wait", "data": { @@ -222,15 +378,48 @@ } }, { - "type": "path", - "data": { - "pathName": "7NoteAuto7" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto8" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { @@ -247,6 +436,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto8" + } + }, { "type": "wait", "data": { @@ -266,15 +461,48 @@ } }, { - "type": "path", - "data": { - "pathName": "7NoteAuto9" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "7NoteAuto10" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto9" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] } }, { @@ -291,6 +519,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "7NoteAuto10" + } + }, { "type": "wait", "data": { diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index ac5546ad..d727d0d6 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -25,12 +25,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, { "type": "race", "data": { @@ -45,6 +39,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto1" + } + }, { "type": "wait", "data": { @@ -64,15 +64,48 @@ } }, { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto2" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "SourceSide3NoteAuto3" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -89,6 +122,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto3" + } + }, { "type": "wait", "data": { @@ -108,15 +147,48 @@ } }, { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto4" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "SourceSide3NoteAuto5" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -133,6 +205,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceSide3NoteAuto5" + } + }, { "type": "wait", "data": { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index c2c654fd..bf2308b7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -115,14 +115,14 @@ else if (lastBreatheLED - i < indexOffset + numberOfLEDs) } } - void sectionColor(int amountOfSections, Supplier... colors) { + void sectionColor(int amountOfSections, Supplier[] colors) { if (amountOfSections != colors.length) throw new IllegalArgumentException("Amount of sections must be equal to the amount of colors"); final int LEDSPerSection = (int) Math.floor(numberOfLEDs / amountOfSections); setSectionColor(amountOfSections, LEDSPerSection, colors); } - private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier... colors) { + private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier[] colors) { if (inverted) { for (int i = 0; i < amountOfSections; i++) setLEDColors(colors[amountOfSections - i - 1].get(), LEDSPerSection * i, i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1); From 39c3ccdd7f3d082f2d4d38dd35de50859dc23746 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 3 Nov 2024 10:21:48 +0200 Subject: [PATCH 226/261] fixed speakerSide3NoteAuto --- .../autos/SpeakerSide3NoteAuto.auto | 214 +++++++++++++++--- 1 file changed, 188 insertions(+), 26 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto index 6971b900..1d0e6412 100644 --- a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto @@ -25,12 +25,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "SpeakerSide3NoteAuto1" - } - }, { "type": "race", "data": { @@ -45,6 +39,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto1" + } + }, { "type": "wait", "data": { @@ -84,15 +84,93 @@ "data": { "commands": [ { - "type": "path", + "type": "race", "data": { - "pathName": "SpeakerSide3NoteAuto2" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { - "type": "path", + "type": "race", "data": { - "pathName": "SpeakerSide3NoteAuto3" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -109,6 +187,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto3" + } + }, { "type": "wait", "data": { @@ -128,15 +212,48 @@ } }, { - "type": "path", - "data": { - "pathName": "SpeakerSide3NoteAuto4" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "SpeakerSide3NoteAuto5" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -153,6 +270,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto5" + } + }, { "type": "wait", "data": { @@ -172,15 +295,48 @@ } }, { - "type": "path", - "data": { - "pathName": "SpeakerSide3NoteAuto6" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "SpeakerSide3NoteAuto7" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -197,6 +353,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerSide3NoteAuto7" + } + }, { "type": "wait", "data": { From 2bab78bd17557d555a6a07e68a219849de743594 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 5 Nov 2024 12:23:12 +0200 Subject: [PATCH 227/261] fixed middleRush auto --- .../autos/MiddleRush3NoteAuto.auto | 309 +++++++++++------- .../autos/SourceSide3NoteAuto.auto | 70 ++-- .../autos/SourceSide3NoteAutoRight.auto | 71 ++-- .../paths/MiddleRush3NoteAuto2.path | 20 +- .../paths/MiddleRush3NoteAuto4.path | 20 +- .../paths/MiddleRush3NoteAuto6.path | 20 +- .../paths/MiddleRush3NoteAuto8.path | 20 +- 7 files changed, 270 insertions(+), 260 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index 3299f7aa..f32737e8 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -22,42 +22,35 @@ } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto1" + "name": "PreparePitchForCloseEjectFromShooter" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto1" + } + }, + { + "type": "wait", "data": { - "name": "PrepareShootingForCloseEjectFromShooter" + "waitTime": 0.05 } }, { - "type": "sequential", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.05 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "name": "FeedNote" } } ] @@ -80,48 +73,67 @@ } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto2" + "name": "Collect" } }, { - "type": "path", + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto3" + "name": "PrepareShootingForCloseEjectFromShooter" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto3" + } + }, + { + "type": "wait", "data": { - "name": "PrepareShootingForEjectFromShooter" + "waitTime": 0.05 } }, { - "type": "sequential", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.1 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "name": "FeedNote" } } ] @@ -144,136 +156,207 @@ } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto4" + "name": "Collect" } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "MiddleRush3NoteAuto5" + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "PrepareForShooting" + "pathName": "MiddleRush3NoteAuto5" } }, { - "type": "sequential", + "type": "wait", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" } } ] } - }, + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto6" + "name": "Collect" } }, { - "type": "path", + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto7" + "name": "PreparePitch" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "PrepareForShooting" + "pathName": "MiddleRush3NoteAuto7" } }, { - "type": "sequential", + "type": "wait", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" } } ] } - }, + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto8" + "name": "Collect" } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "MiddleRush3NoteAuto9" + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto8" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "PrepareForShooting" + "pathName": "MiddleRush3NoteAuto9" } }, { - "type": "sequential", + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "name": "FeedNote" } } ] diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto index d727d0d6..e4913ae4 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAuto.auto @@ -18,7 +18,13 @@ { "type": "named", "data": { - "name": "PreparePitch" + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" } }, { @@ -26,43 +32,37 @@ "data": { "commands": [ { - "type": "race", + "type": "wait", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SourceSide3NoteAuto1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] + "waitTime": 0.8 } }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ { "type": "race", "data": { diff --git a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto index dc0d449f..28036156 100644 --- a/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/SourceSide3NoteAutoRight.auto @@ -18,7 +18,13 @@ { "type": "named", "data": { - "name": "PreparePitch" + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" } }, { @@ -26,44 +32,37 @@ "data": { "commands": [ { - "type": "race", + "type": "wait", "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.8 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] + "waitTime": 0.8 } }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ { "type": "race", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path index d5b0f783..33b5742d 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto2.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path index 76e4475f..d10eaa4b 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto4.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path index 20ecd5ec..464bdc93 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto6.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "ReCollectNote", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path index 481619ab..7dff55e2 100644 --- a/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path +++ b/src/main/deploy/pathplanner/paths/MiddleRush3NoteAuto8.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "RecollectNote", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, From 15173c867de899d4ef942df4d4cc48aec21aad73 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 5 Nov 2024 12:29:46 +0200 Subject: [PATCH 228/261] Update Zayde3NoteAuto.auto --- .../pathplanner/autos/Zayde3NoteAuto.auto | 238 +++++++++++------- 1 file changed, 154 insertions(+), 84 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto index f189e90b..0152c5ec 100644 --- a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto @@ -21,6 +21,12 @@ "name": "PreparePitchForCloseEjectFromShooter" } }, + { + "type": "named", + "data": { + "name": "PrepareShootingForCloseEjectFromShooter" + } + }, { "type": "sequential", "data": { @@ -32,22 +38,15 @@ } }, { - "type": "race", + "type": "wait", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.05 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" } } ] @@ -67,136 +66,207 @@ } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Zayde3NoteAuto2" + "name": "Collect" } }, { - "type": "path", + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", "data": { - "pathName": "Zayde3NoteAuto3" + "name": "PrepareForShooting" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "PrepareForShooting" + "pathName": "Zayde3NoteAuto3" } }, { - "type": "sequential", + "type": "wait", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" } } ] } - }, + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Zayde3NoteAuto4" + "name": "Collect" } }, { - "type": "path", + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", "data": { - "pathName": "Zayde3NoteAuto5" + "name": "PrepareForShooting" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto5" + } + }, + { + "type": "wait", "data": { - "name": "PrepareForShooting" + "waitTime": 0.4 } }, { - "type": "sequential", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "name": "FeedNote" } } ] } - }, + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Zayde3NoteAuto6" + "name": "Collect" } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "Zayde3NoteAuto7" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareForShooting" } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto7" + } + }, + { + "type": "wait", "data": { - "name": "PrepareForShooting" + "waitTime": 0.4 } }, { - "type": "sequential", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.2 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] + "name": "FeedNote" } } ] From b56a12a92c519a55ac25d6e2b4f39fc52c7251ec Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 5 Nov 2024 16:07:46 +0200 Subject: [PATCH 229/261] fixed around stage autos --- .../autos/3NoteAutoThroughAmp.auto | 71 ++++--- .../autos/3NoteAutoThroughAmpLeft.auto | 38 ++++ .../autos/4NoteAutoAroundStage.auto | 160 +++++++++++---- .../autos/5NoteAutoAroundStage.auto | 192 ++++++++++++++---- .../autos/SpeakerSide3NoteAuto.auto | 54 ++--- .../paths/5NoteAutoAroundStage1.path | 30 +-- .../paths/5NoteAutoAroundStage3.path | 20 +- .../paths/5NoteAutoAroundStage5.path | 20 +- .../paths/5NoteAutoAroundStage7.path | 20 +- 9 files changed, 378 insertions(+), 227 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto index 682ed39e..76667308 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmp.auto @@ -18,7 +18,13 @@ { "type": "named", "data": { - "name": "PreparePitch" + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" } }, { @@ -26,44 +32,37 @@ "data": { "commands": [ { - "type": "race", + "type": "wait", "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] + "waitTime": 0.7 } }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitch" + } + }, + { + "type": "sequential", + "data": { + "commands": [ { "type": "race", "data": { diff --git a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto index 973ec414..e9269e1a 100644 --- a/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto +++ b/src/main/deploy/pathplanner/autos/3NoteAutoThroughAmpLeft.auto @@ -11,6 +11,44 @@ "type": "sequential", "data": { "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, { "type": "race", "data": { diff --git a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto index 6bd7b7bc..19ccb576 100644 --- a/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/4NoteAutoAroundStage.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.2699301408104018, - "y": 6.932078092184688 + "x": 0.7402504945854966, + "y": 6.605014900170646 }, "rotation": 52.489999999999995 }, @@ -11,6 +11,44 @@ "type": "sequential", "data": { "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, { "type": "race", "data": { @@ -32,7 +70,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "Collect" } }, { @@ -40,15 +78,15 @@ "data": { "commands": [ { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.5 + "pathName": "5NoteAutoAroundStage1" } }, { - "type": "named", + "type": "wait", "data": { - "name": "FeedNote" + "waitTime": 0.4 } } ] @@ -57,18 +95,6 @@ ] } }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage1" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage2" - } - }, { "type": "race", "data": { @@ -89,6 +115,12 @@ "waitTime": 0.2 } }, + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage2" + } + }, { "type": "named", "data": { @@ -102,15 +134,35 @@ } }, { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage3" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage4" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -133,6 +185,12 @@ "waitTime": 0.2 } }, + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage4" + } + }, { "type": "named", "data": { @@ -146,15 +204,35 @@ } }, { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage5" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage6" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -177,6 +255,12 @@ "waitTime": 0.2 } }, + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage6" + } + }, { "type": "named", "data": { @@ -194,6 +278,12 @@ } ] } + }, + { + "type": "named", + "data": { + "name": "StopShooting" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto index 6ffa7e45..e3b7463e 100644 --- a/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto +++ b/src/main/deploy/pathplanner/autos/5NoteAutoAroundStage.auto @@ -11,6 +11,44 @@ "type": "sequential", "data": { "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PreparePitchForCloseShot" + } + }, + { + "type": "named", + "data": { + "name": "PrepareForShootingCloseShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] + } + } + ] + } + }, { "type": "race", "data": { @@ -32,7 +70,7 @@ { "type": "named", "data": { - "name": "PrepareForShooting" + "name": "Collect" } }, { @@ -40,15 +78,15 @@ "data": { "commands": [ { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.5 + "pathName": "5NoteAutoAroundStage1" } }, { - "type": "named", + "type": "wait", "data": { - "name": "FeedNote" + "waitTime": 0.4 } } ] @@ -57,18 +95,6 @@ ] } }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage1" - } - }, - { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage2" - } - }, { "type": "race", "data": { @@ -83,6 +109,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage2" + } + }, { "type": "wait", "data": { @@ -102,15 +134,35 @@ } }, { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage3" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage4" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -127,6 +179,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage4" + } + }, { "type": "wait", "data": { @@ -146,15 +204,35 @@ } }, { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage5" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage6" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -171,6 +249,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage6" + } + }, { "type": "wait", "data": { @@ -190,15 +274,35 @@ } }, { - "type": "path", - "data": { - "pathName": "5NoteAutoAroundStage7" - } - }, - { - "type": "path", + "type": "race", "data": { - "pathName": "5NoteAutoAroundStage8" + "commands": [ + { + "type": "named", + "data": { + "name": "Collect" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] + } + } + ] } }, { @@ -215,6 +319,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "5NoteAutoAroundStage8" + } + }, { "type": "wait", "data": { diff --git a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto index 1d0e6412..8a4a7a79 100644 --- a/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/SpeakerSide3NoteAuto.auto @@ -21,46 +21,32 @@ "name": "PreparePitchForCloseEjectFromShooter" } }, + { + "type": "named", + "data": { + "name": "PrepareShootingForCloseEjectFromShooter" + } + }, { "type": "sequential", "data": { "commands": [ { - "type": "race", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareShootingForCloseEjectFromShooter" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SpeakerSide3NoteAuto1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.05 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" - } - } - ] - } - } - ] + "pathName": "SpeakerSide3NoteAuto1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.05 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" } } ] diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path index f12e79e3..b618eda3 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.2699301408104018, - "y": 6.932078092184688 + "x": 0.7113445212805922, + "y": 6.648373860128002 }, "prevControl": null, "nextControl": { - "x": 1.6306711948340775, - "y": 7.780307057051169 + "x": 1.0720855753042686, + "y": 7.496602824994483 }, "isLocked": false, "linkedName": null @@ -57,25 +57,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 1.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, @@ -90,7 +72,7 @@ "reversed": false, "folder": "5NoteAutoAroundStage", "previewStartingState": { - "rotation": 55.06, + "rotation": 58.240519915187164, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path index 6525e05d..114ad791 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage3.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path index 7397ee31..a9cf3d4a 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage5.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path index 6a0f735a..121ffadd 100644 --- a/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path +++ b/src/main/deploy/pathplanner/paths/5NoteAutoAroundStage7.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.8, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, From 57d04e6ee16688b439f50f6ae36d670d8ff3993a Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 5 Nov 2024 16:45:00 +0200 Subject: [PATCH 230/261] fixed breath inverted and removed unnecacery auto commands --- .../deploy/pathplanner/paths/7NoteAuto5.path | 20 +------------------ .../deploy/pathplanner/paths/7NoteAuto7.path | 20 +------------------ .../deploy/pathplanner/paths/7NoteAuto9.path | 20 +------------------ .../paths/SpeakerSide3NoteAuto2.path | 20 +------------------ .../paths/SpeakerSide3NoteAuto4.path | 20 +------------------ .../paths/SpeakerSide3NoteAuto6.path | 20 +------------------ .../pathplanner/paths/Zayde3NoteAuto2.path | 20 +------------------ .../pathplanner/paths/Zayde3NoteAuto4.path | 20 +------------------ .../pathplanner/paths/Zayde3NoteAuto6.path | 20 +------------------ .../robot/subsystems/ledstrip/LEDStrip.java | 1 + .../ledstrip/LEDStripConstants.java | 2 +- 11 files changed, 11 insertions(+), 172 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto5.path b/src/main/deploy/pathplanner/paths/7NoteAuto5.path index 0dec60c6..4a2b2fe6 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto5.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto5.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.75, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto7.path b/src/main/deploy/pathplanner/paths/7NoteAuto7.path index fe3e05d1..fdd92c63 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto7.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto7.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/7NoteAuto9.path b/src/main/deploy/pathplanner/paths/7NoteAuto9.path index 40014b08..03d94dc1 100644 --- a/src/main/deploy/pathplanner/paths/7NoteAuto9.path +++ b/src/main/deploy/pathplanner/paths/7NoteAuto9.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path index 31885d55..073c7dff 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto2.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path index 85716330..4fcab2a8 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto4.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path index f43ffeb6..ed0dac52 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/SpeakerSide3NoteAuto6.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "RecollectNote", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path index 54b8a38f..afc99303 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto2.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path index 9a65d6e8..755b6489 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto4.path @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path index 63cc2089..dea0fc5b 100644 --- a/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path +++ b/src/main/deploy/pathplanner/paths/Zayde3NoteAuto6.path @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "Collect", - "waypointRelativePos": 0.95, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 4.0, diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index bf2308b7..721261bb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -91,6 +91,7 @@ void rainbow() { void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean shouldLoop, boolean inverted) { clearLEDColors(); + inverted = this.inverted != inverted; double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; double currentTime = Timer.getFPGATimestamp(); if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index 882bf5ae..6b89d1dd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -13,7 +13,7 @@ public class LEDStripConstants { RIGHT_CLIMBER_NUMBER_OF_LEDS = 22, LEFT_CLIMBER_NUMBER_OF_LEDS = 22; private static final boolean - RIGHT_CLIMBER_INVERTED = false, + RIGHT_CLIMBER_INVERTED = true, LEFT_CLIMBER_INVERTED = false; static final AddressableLEDBuffer LED_BUFFER = new AddressableLEDBuffer(RIGHT_CLIMBER_NUMBER_OF_LEDS + LEFT_CLIMBER_NUMBER_OF_LEDS); static final AddressableLED LED = new AddressableLED(PORT); From e365f0d550acad19d12bf42478bc2b0062eed8d9 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 5 Nov 2024 16:59:06 +0200 Subject: [PATCH 231/261] fixed --- .../autos/MiddleRush3NoteAuto.auto | 326 +++++++++--------- .../pathplanner/autos/Zayde3NoteAuto.auto | 247 ++++++------- 2 files changed, 297 insertions(+), 276 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index f32737e8..d6653587 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -73,67 +73,74 @@ } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", - "data": { - "name": "Collect" - } - }, - { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto2" + "name": "Collect" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareShootingForCloseEjectFromShooter" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "MiddleRush3NoteAuto3" - } - }, - { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.05 + "name": "PrepareShootingForCloseEjectFromShooter" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] @@ -156,207 +163,214 @@ } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", - "data": { - "name": "Collect" - } - }, - { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto4" + "name": "Collect" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PreparePitch" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto5" + "name": "PrepareForShooting" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto6" + "name": "Collect" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PreparePitch" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "MiddleRush3NoteAuto7" - } - }, - { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.4 + "name": "PrepareForShooting" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "MiddleRush3NoteAuto8" + "name": "Collect" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto8" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PreparePitch" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "MiddleRush3NoteAuto9" - } - }, - { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.4 + "name": "PrepareForShooting" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "MiddleRush3NoteAuto9" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto index 0152c5ec..497eccc0 100644 --- a/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/Zayde3NoteAuto.auto @@ -66,207 +66,214 @@ } }, { - "type": "race", + "type": "sequential", "data": { "commands": [ { - "type": "named", - "data": { - "name": "Collect" - } - }, - { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Zayde3NoteAuto2" + "name": "Collect" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "Zayde3NoteAuto3" - } - }, - { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.4 + "name": "PrepareForShooting" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Zayde3NoteAuto4" + "name": "Collect" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Zayde3NoteAuto5" + "name": "PrepareForShooting" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 - } - }, - { - "type": "named", - "data": { - "name": "FeedNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto5" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Collect" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Zayde3NoteAuto6" + "name": "Collect" } }, { - "type": "wait", + "type": "sequential", "data": { - "waitTime": 0.4 + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto6" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + } + ] } } ] } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PrepareForShooting" - } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "Zayde3NoteAuto7" - } - }, - { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.4 + "name": "PrepareForShooting" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "FeedNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Zayde3NoteAuto7" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + } + ] } } ] From 38257b534fc7040afe0c9233b7ee728b496f7db8 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 5 Nov 2024 17:01:00 +0200 Subject: [PATCH 232/261] fixed stupid error --- src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto index d6653587..4feb902e 100644 --- a/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto +++ b/src/main/deploy/pathplanner/autos/MiddleRush3NoteAuto.auto @@ -28,7 +28,7 @@ { "type": "named", "data": { - "name": "PreparePitchForCloseEjectFromShooter" + "name": "PrepareShootingForCloseEjectFromShooter" } }, { From 02b910d1a4ba1bdd5ca378c46b80762aba4e6651 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 5 Nov 2024 17:01:26 +0200 Subject: [PATCH 233/261] accidently commited this --- .../frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index 6b89d1dd..882bf5ae 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -13,7 +13,7 @@ public class LEDStripConstants { RIGHT_CLIMBER_NUMBER_OF_LEDS = 22, LEFT_CLIMBER_NUMBER_OF_LEDS = 22; private static final boolean - RIGHT_CLIMBER_INVERTED = true, + RIGHT_CLIMBER_INVERTED = false, LEFT_CLIMBER_INVERTED = false; static final AddressableLEDBuffer LED_BUFFER = new AddressableLEDBuffer(RIGHT_CLIMBER_NUMBER_OF_LEDS + LEFT_CLIMBER_NUMBER_OF_LEDS); static final AddressableLED LED = new AddressableLED(PORT); From ebd561db7a4aa56681277046cc0e85dcaa2fe449 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 7 Nov 2024 17:14:01 +0200 Subject: [PATCH 234/261] added alternateColor because I was bored --- .../robot/subsystems/ledstrip/LEDStrip.java | 25 ++++++++++++++++++- .../subsystems/ledstrip/LEDStripCommands.java | 8 ++++++ .../ledstrip/LEDStripConstants.java | 8 ++++-- 3 files changed, 38 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 721261bb..2511c3a4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -21,11 +21,18 @@ public class LEDStrip extends SubsystemBase { private double rainbowFirstPixelHue = 0; private boolean areLEDsOnForBlinking = false; private double lastBlinkTime = 0; + private boolean alternateColor = true; + private double lastAlternateColorTime = 0; static { GeneralCommands.getDelayedCommand( 1, - () -> LEDStripConstants.LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getBlinkingCommand(Color.kRed, Color.kBlack, LEDStripConstants.LOW_BATTERY_BLINKING_INTERVAL_SECONDS, LED_STRIPS).withTimeout(LEDStripConstants.LOW_BATTERY_BLINKING_TIME_SECONDS)) + () -> LEDStripConstants.LOW_BATTERY_TRIGGER.whileTrue(LEDStripCommands.getAlternateColorCommand( + LEDStripConstants.LOW_BATTERY_FIRST_COLOR, + LEDStripConstants.LOW_BATTERY_SECOND_COLOR, + LEDStripConstants.LOW_BATTERY_ALTERNATE_COLOR_INTERVAL_SECONDS, + LED_STRIPS + ).withTimeout(LEDStripConstants.LOW_BATTERY_ALTERNATING_TIME_SECONDS)) ); } @@ -58,6 +65,8 @@ void resetLEDSettings() { rainbowFirstPixelHue = 0; areLEDsOnForBlinking = false; lastBlinkTime = 0; + alternateColor = true; + lastAlternateColorTime = 0; } void clearLEDColors() { @@ -116,6 +125,20 @@ else if (lastBreatheLED - i < indexOffset + numberOfLEDs) } } + void alternateColor(Color firstColor, Color secondColor, double IntervalSeconds) { + if (Timer.getFPGATimestamp() - lastAlternateColorTime > IntervalSeconds) { + alternateColor = !alternateColor; + lastAlternateColorTime = Timer.getFPGATimestamp(); + } + if (alternateColor) { + for (int i = 0; i < numberOfLEDs; i++) + LEDStripConstants.LED_BUFFER.setLED(i + indexOffset, i % 2 == 0 ? firstColor : secondColor); + return; + } + for (int i = 0; i < numberOfLEDs; i++) + LEDStripConstants.LED_BUFFER.setLED(i + indexOffset, i % 2 == 0 ? secondColor : firstColor); + } + void sectionColor(int amountOfSections, Supplier[] colors) { if (amountOfSections != colors.length) throw new IllegalArgumentException("Amount of sections must be equal to the amount of colors"); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index 8608af8b..ba207e70 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -47,6 +47,14 @@ public static Command getBreatheCommand(Color color, int breathingLEDs, double c ).ignoringDisable(true); } + public static Command getAlternateColorCommand(Color firstColor, Color secondColor, double intervalSeconds, LEDStrip... ledStrips) { + return new ExecuteEndCommand( + () -> runForLEDs((LEDStrip -> LEDStrip.alternateColor(firstColor, secondColor, intervalSeconds)), ledStrips), + () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), + ledStrips + ).ignoringDisable(true); + } + public static Command getSectionColorCommand(int amountOfSections, Supplier[] colors, LEDStrip... ledStrips) { return new InitExecuteCommand( () -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index 882bf5ae..b6489f0e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.Robot; @@ -20,8 +21,11 @@ public class LEDStripConstants { static final double MINIMUM_BATTERY_VOLTAGE = 10.5; static final Trigger LOW_BATTERY_TRIGGER = new Trigger(() -> !DriverStation.isEnabled() && Robot.IS_REAL && RobotController.getBatteryVoltage() < LEDStripConstants.MINIMUM_BATTERY_VOLTAGE); - static final double LOW_BATTERY_BLINKING_INTERVAL_SECONDS = 0.2; - static final double LOW_BATTERY_BLINKING_TIME_SECONDS = 5; + static final Color + LOW_BATTERY_FIRST_COLOR = Color.kOrange, + LOW_BATTERY_SECOND_COLOR = Color.kYellow; + static final double LOW_BATTERY_ALTERNATE_COLOR_INTERVAL_SECONDS = 0.2; + static final double LOW_BATTERY_ALTERNATING_TIME_SECONDS = 10; public static final LEDStrip RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, 0), From ac8ee7e738da5e04be00c42d796db62e0e954bad Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 7 Nov 2024 17:19:52 +0200 Subject: [PATCH 235/261] added inverted for rainbow command --- .../java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 2511c3a4..26c239ce 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -94,6 +94,12 @@ void rainbow() { final int hue = (int) (rainbowFirstPixelHue + (led * 180 / numberOfLEDs) % 180); LEDStripConstants.LED_BUFFER.setHSV(led + indexOffset, hue, 255, 128); } + if (inverted) { + rainbowFirstPixelHue -= 3; + if (rainbowFirstPixelHue < 0) + rainbowFirstPixelHue += 180; + return; + } rainbowFirstPixelHue += 3; rainbowFirstPixelHue %= 180; } From be3bc37581d45f0db9121adf0887f4e63dd28ffc Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 7 Nov 2024 20:09:05 +0200 Subject: [PATCH 236/261] cleaned up led command usages --- .../robot/commands/AlignToNoteCommand.java | 19 +++++++-- .../robot/commands/CommandConstants.java | 10 ++++- .../factories/AutonomousCommands.java | 5 +-- .../commands/factories/GeneralCommands.java | 10 ++++- .../robot/subsystems/intake/Intake.java | 26 ++++++++++-- .../subsystems/intake/IntakeConstants.java | 40 +++++++++++++------ .../ledstrip/LEDStripConstants.java | 6 +++ 7 files changed, 91 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index db3338c8..e8f3bcaf 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -34,8 +33,22 @@ public AlignToNoteCommand() { private Command getSetCurrentLEDColorCommand() { return GeneralCommands.getContinuousConditionalCommand( - LEDStripCommands.getBreatheCommand(Color.kGreen, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.INTAKE_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS), - LEDStripCommands.getBreatheCommand(Color.kRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.INTAKE_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS), + LEDStripCommands.getBreatheCommand( + IntakeConstants.NOTE_DETECTION_CAMERA_HAS_TARGETS_BREATHING_LEDS_COLOR, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_LEDS_AMOUNT, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_SHOULD_LOOP, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ), + LEDStripCommands.getBreatheCommand( + IntakeConstants.NOTE_DETECTION_CAMERA_HAS_NO_TARGETS_BREATHING_LEDS_COLOR, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_LEDS_AMOUNT, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_SHOULD_LOOP, + IntakeConstants.ALIGN_TO_NOTE_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ), CAMERA::hasTargets ).asProxy(); } diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 37484b5c..2a1128db 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -17,6 +17,7 @@ import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; +import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.Logger; @@ -85,7 +86,14 @@ public class CommandConstants { SHOULD_ALIGN_TO_NOTE = false; Logger.recordOutput("ShouldAlignToNote", false); }).ignoringDisable(true), - DEFAULT_LEDS_COMMAND = LEDStripCommands.getBreatheCommand(Color.kLightSeaGreen, 7, 1.5, true, false, LEDStrip.LED_STRIPS), + DEFAULT_LEDS_COMMAND = LEDStripCommands.getBreatheCommand( + LEDStripConstants.DEFAULT_COMMAND_COLOR, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_LEDS_AMOUNT, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_CYCLE_TIME_SECONDS, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_SHOULD_LOOP, + LEDStripConstants.DEFAULT_COMMAND_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ), DEFAULT_INTAKE_COMMAND = IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOP), DEFAULT_CLIMBER_COMMAND = ClimberCommands.getStopCommand(), MOVE_CLIMBER_DOWN_MANUALLY_COMMAND = ClimberCommands.getSetTargetVoltageCommand(ClimberConstants.MOVE_CLIMBER_DOWN_VOLTAGE), diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 51ae7fbc..8d1d4e90 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -115,8 +114,8 @@ private static void overrideRotation(Supplier> rotationOver private static Command getSetCurrentLEDColorCommand() { return GeneralCommands.getContinuousConditionalCommand( - LEDStripCommands.getStaticColorCommand(Color.kGreen, LEDStrip.LED_STRIPS), - LEDStripCommands.getStaticColorCommand(Color.kRed, LEDStrip.LED_STRIPS), + LEDStripCommands.getStaticColorCommand(IntakeConstants.NOTE_DETECTION_CAMERA_HAS_TARGETS_BREATHING_LEDS_COLOR, LEDStrip.LED_STRIPS), + LEDStripCommands.getStaticColorCommand(IntakeConstants.NOTE_DETECTION_CAMERA_HAS_NO_TARGETS_BREATHING_LEDS_COLOR, LEDStrip.LED_STRIPS), NOTE_DETECTION_CAMERA::hasTargets ).asProxy().until(() -> !IS_ALIGNING_TO_NOTE); } diff --git a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java index c7e49943..b77f326d 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/GeneralCommands.java @@ -1,6 +1,5 @@ package frc.trigon.robot.commands.factories; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.AlignToNoteCommand; @@ -43,7 +42,14 @@ public static Command getClimbCommand() { public static Command getNoteCollectionCommand() { return new ParallelCommandGroup( new AlignToNoteCommand().onlyIf(() -> CommandConstants.SHOULD_ALIGN_TO_NOTE), - LEDStripCommands.getBreatheCommand(Color.kOrangeRed, IntakeConstants.INTAKE_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.INTAKE_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.INTAKE_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), + LEDStripCommands.getBreatheCommand( + IntakeConstants.COLLECTION_BREATHING_LEDS_COLOR, + IntakeConstants.COLLECTION_BREATHING_LEDS_AMOUNT, + IntakeConstants.COLLECTION_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.COLLECTION_BREATHING_SHOULD_LOOP, + IntakeConstants.COLLECTION_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ).asProxy().onlyIf(() -> !CommandConstants.SHOULD_ALIGN_TO_NOTE), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND).unless(RobotContainer.INTAKE::hasNote) ).unless(RobotContainer.INTAKE::hasNote).alongWith(duplicate(CommandConstants.RUMBLE_COMMAND).onlyIf(RobotContainer.INTAKE::hasNote)); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 2e7e0ddf..72abf97f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -3,7 +3,6 @@ import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; @@ -90,14 +89,33 @@ void indicateCollection() { } private Command getCollectionIndicationLEDsCommand() { - return LEDStripCommands.getBlinkingCommand(Color.kOrangeRed, Color.kBlack, IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, LEDStrip.LED_STRIPS).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); + return LEDStripCommands.getBlinkingCommand( + IntakeConstants.COLLECTION_INDICATION_BLINKING_FIRST_COLOR, + IntakeConstants.COLLECTION_INDICATION_BLINKING_SECOND_COLOR, + IntakeConstants.COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS, + LEDStrip.LED_STRIPS + ).withTimeout(IntakeConstants.COLLECTION_INDICATION_BLINKING_TIME_SECONDS); } private Command getFeedingIndicationLEDsCommand() { - return LEDStripCommands.getBreatheCommand(Color.kPurple, IntakeConstants.FEEDING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.FEEDING_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.FEEDING_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS); + return LEDStripCommands.getBreatheCommand( + IntakeConstants.FEEDING_INDICATION_COLOR, + IntakeConstants.FEEDING_INDICATION_BREATHING_LEDS_AMOUNT, + IntakeConstants.FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.FEEDING_INDICATION_BREATHING_SHOULD_LOOP, + IntakeConstants.FEEDING_INDICATION_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ); } private Command getEjectingIndicationLEDsCommand() { - return LEDStripCommands.getBreatheCommand(Color.kDarkBlue, IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, IntakeConstants.EJECTING_INDICATION_BREATHING_IS_INVERTED, LEDStrip.LED_STRIPS); + return LEDStripCommands.getBreatheCommand( + IntakeConstants.EJECTING_INDICATION_COLOR, + IntakeConstants.EJECTING_INDICATION_BREATHING_LEDS_AMOUNT, + IntakeConstants.EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS, + IntakeConstants.EJECTING_INDICATION_BREATHING_SHOULD_LOOP, + IntakeConstants.EJECTING_INDICATION_BREATHING_IS_INVERTED, + LEDStrip.LED_STRIPS + ); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index a867f978..b1c05699 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.trigon.robot.misc.objectdetectioncamera.SimulationObjectDetectionCameraIO; import org.trigon.hardware.RobotHardwareStats; @@ -71,24 +72,39 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), () -> Math.abs(MASTER_MOTOR.getSignal(TalonFXSignal.TORQUE_CURRENT)) > IntakeConstants.NOTE_COLLECTION_CURRENT ).debounce(NOTE_COLLECTION_TIME_THRESHOLD_SECONDS); - static final double - COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; + static final Color + COLLECTION_INDICATION_BLINKING_FIRST_COLOR = Color.kOrangeRed, + COLLECTION_INDICATION_BLINKING_SECOND_COLOR = Color.kBlack, + FEEDING_INDICATION_COLOR = Color.kPurple, + EJECTING_INDICATION_COLOR = Color.kDarkBlue; + static final double COLLECTION_INDICATION_LEDS_BLINKING_INTERVAL_SECONDS = 0.2; static final double COLLECTION_INDICATION_BLINKING_TIME_SECONDS = 2; - public static final int + static final int FEEDING_INDICATION_BREATHING_LEDS_AMOUNT = 5, - EJECTING_INDICATION_BREATHING_LEDS_AMOUNT = 9, - INTAKE_INDICATION_BREATHING_LEDS_AMOUNT = 5; - public static final double + EJECTING_INDICATION_BREATHING_LEDS_AMOUNT = 9; + static final double FEEDING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.05, - EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 1, - INTAKE_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 0.2; - public static final boolean + EJECTING_INDICATION_BREATHING_CYCLE_TIME_SECONDS = 1; + static final boolean FEEDING_INDICATION_BREATHING_SHOULD_LOOP = false, FEEDING_INDICATION_BREATHING_IS_INVERTED = false, EJECTING_INDICATION_BREATHING_SHOULD_LOOP = true, - EJECTING_INDICATION_BREATHING_IS_INVERTED = true, - INTAKE_INDICATION_BREATHING_SHOULD_LOOP = true, - INTAKE_INDICATION_BREATHING_IS_INVERTED = false; + EJECTING_INDICATION_BREATHING_IS_INVERTED = true; + public static final Color + COLLECTION_BREATHING_LEDS_COLOR = Color.kOrangeRed, + NOTE_DETECTION_CAMERA_HAS_TARGETS_BREATHING_LEDS_COLOR = Color.kGreen, + NOTE_DETECTION_CAMERA_HAS_NO_TARGETS_BREATHING_LEDS_COLOR = Color.kRed; + public static final int + COLLECTION_BREATHING_LEDS_AMOUNT = 5, + ALIGN_TO_NOTE_BREATHING_LEDS_AMOUNT = 5; + public static final double + COLLECTION_BREATHING_CYCLE_TIME_SECONDS = 0.2, + ALIGN_TO_NOTE_BREATHING_CYCLE_TIME_SECONDS = 0.2; + public static final boolean + COLLECTION_BREATHING_SHOULD_LOOP = true, + COLLECTION_BREATHING_IS_INVERTED = false, + ALIGN_TO_NOTE_BREATHING_SHOULD_LOOP = true, + ALIGN_TO_NOTE_BREATHING_IS_INVERTED = false; static { configureMasterMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java index b6489f0e..09ba3679 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripConstants.java @@ -26,6 +26,12 @@ public class LEDStripConstants { LOW_BATTERY_SECOND_COLOR = Color.kYellow; static final double LOW_BATTERY_ALTERNATE_COLOR_INTERVAL_SECONDS = 0.2; static final double LOW_BATTERY_ALTERNATING_TIME_SECONDS = 10; + public static final Color DEFAULT_COMMAND_COLOR = Color.kLightSeaGreen; + public static final int DEFAULT_COMMAND_BREATHING_LEDS_AMOUNT = 7; + public static final double DEFAULT_COMMAND_BREATHING_CYCLE_TIME_SECONDS = 1.5; + public static final boolean + DEFAULT_COMMAND_BREATHING_SHOULD_LOOP = true, + DEFAULT_COMMAND_BREATHING_IS_INVERTED = false; public static final LEDStrip RIGHT_CLIMBER_LEDS = new LEDStrip(RIGHT_CLIMBER_INVERTED, RIGHT_CLIMBER_NUMBER_OF_LEDS, 0), From 86c17b26c70d58b194066938a427456949f1218a Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 8 Nov 2024 11:31:28 +0200 Subject: [PATCH 237/261] cleaned leds a bit --- .../frc/trigon/robot/subsystems/ledstrip/LEDStrip.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index 26c239ce..e20ad327 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.factories.GeneralCommands; import java.util.function.Supplier; @@ -118,7 +117,7 @@ void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean sh } if (inverted ? (lastBreatheLED < indexOffset) : (lastBreatheLED >= numberOfLEDs + indexOffset)) { if (!shouldLoop) { - CommandConstants.DEFAULT_LEDS_COMMAND.schedule(); + getDefaultCommand().schedule(); return; } lastBreatheLED = inverted ? indexOffset + numberOfLEDs : indexOffset; @@ -131,8 +130,8 @@ else if (lastBreatheLED - i < indexOffset + numberOfLEDs) } } - void alternateColor(Color firstColor, Color secondColor, double IntervalSeconds) { - if (Timer.getFPGATimestamp() - lastAlternateColorTime > IntervalSeconds) { + void alternateColor(Color firstColor, Color secondColor, double intervalSeconds) { + if (Timer.getFPGATimestamp() - lastAlternateColorTime > intervalSeconds) { alternateColor = !alternateColor; lastAlternateColorTime = Timer.getFPGATimestamp(); } From 59fb7283977f488396fca1c190727cf1567032b5 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sat, 9 Nov 2024 19:47:48 +0200 Subject: [PATCH 238/261] added colorFlow command --- .../robot/subsystems/ledstrip/LEDStrip.java | 32 +++++++++++++++++++ .../subsystems/ledstrip/LEDStripCommands.java | 13 ++++++++ 2 files changed, 45 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index e20ad327..c22c63ba 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -22,6 +22,8 @@ public class LEDStrip extends SubsystemBase { private double lastBlinkTime = 0; private boolean alternateColor = true; private double lastAlternateColorTime = 0; + private double lastColorFlowTime = 0; + private int amountOfColorFlowLEDs = 0; static { GeneralCommands.getDelayedCommand( @@ -66,6 +68,8 @@ void resetLEDSettings() { lastBlinkTime = 0; alternateColor = true; lastAlternateColorTime = 0; + lastColorFlowTime = 0; + amountOfColorFlowLEDs = 0; } void clearLEDColors() { @@ -130,6 +134,31 @@ else if (lastBreatheLED - i < indexOffset + numberOfLEDs) } } + void colorFlow(Color color, double cycleTimeSeconds, boolean shouldLoop, boolean inverted) { + clearLEDColors(); + inverted = this.inverted != inverted; + double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; + if (Timer.getFPGATimestamp() - lastColorFlowTime > moveLEDTimeSeconds) { + lastColorFlowTime = Timer.getFPGATimestamp(); + if (inverted) + amountOfColorFlowLEDs--; + else + amountOfColorFlowLEDs++; + } + if (inverted ? amountOfColorFlowLEDs < 0 : amountOfColorFlowLEDs >= numberOfLEDs) { + if (!shouldLoop) { + getDefaultCommand().schedule(); + return; + } + amountOfColorFlowLEDs = inverted ? numberOfLEDs : 0; + } + if (inverted) { + setLEDColors(color, numberOfLEDs - amountOfColorFlowLEDs, numberOfLEDs - 1); + return; + } + setLEDColors(color, 0, amountOfColorFlowLEDs); + } + void alternateColor(Color firstColor, Color secondColor, double intervalSeconds) { if (Timer.getFPGATimestamp() - lastAlternateColorTime > intervalSeconds) { alternateColor = !alternateColor; @@ -149,6 +178,9 @@ void sectionColor(int amountOfSections, Supplier[] colors) { throw new IllegalArgumentException("Amount of sections must be equal to the amount of colors"); final int LEDSPerSection = (int) Math.floor(numberOfLEDs / amountOfSections); setSectionColor(amountOfSections, LEDSPerSection, colors); + + for (int i = 0; i < amountOfSections; i++) + setLEDColors(colors[i].get(), LEDSPerSection * i, i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1); } private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier[] colors) { diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java index ba207e70..162a2e44 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStripCommands.java @@ -47,6 +47,19 @@ public static Command getBreatheCommand(Color color, int breathingLEDs, double c ).ignoringDisable(true); } + public static Command getColorFlowCommand(Color color, double cycleTimeSeconds, boolean shouldLoop, boolean inverted, LEDStrip... ledStrips) { + return new FunctionalCommand( + () -> { + if (!shouldLoop) + runForLEDs(LEDStrip::resetLEDSettings, ledStrips); + }, + () -> runForLEDs((LEDStrip) -> LEDStrip.colorFlow(color, cycleTimeSeconds, shouldLoop, inverted), ledStrips), + (interrupted) -> runForLEDs(LEDStrip::clearLEDColors, ledStrips), + () -> false, + ledStrips + ).ignoringDisable(true); + } + public static Command getAlternateColorCommand(Color firstColor, Color secondColor, double intervalSeconds, LEDStrip... ledStrips) { return new ExecuteEndCommand( () -> runForLEDs((LEDStrip -> LEDStrip.alternateColor(firstColor, secondColor, intervalSeconds)), ledStrips), From 84c6e5773e4d4ab00c440b32a865908645b8c47f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 10 Nov 2024 16:16:01 -0500 Subject: [PATCH 239/261] Lots of random stuff idek --- .../robot/constants/CameraConstants.java | 26 +++++++++++++++++++ .../robot/constants/FieldConstants.java | 6 +++++ .../apriltagcamera/AprilTagCamera.java | 7 ++--- .../io/AprilTagPhotonCameraIO.java | 4 +++ 4 files changed, 38 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 365a8ebf..8bb9c2cd 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -1,5 +1,6 @@ package frc.trigon.robot.constants; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -7,6 +8,7 @@ import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import org.photonvision.simulation.SimCameraProperties; public class CameraConstants { public static final double @@ -39,4 +41,28 @@ public class CameraConstants { TRANSLATIONS_STD_EXPONENT ); public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); + + private static final int + CAMERA_WIDTH = 720, + CAMERA_HEIGHT = 1920, + CAMERA_FPS = 60, + AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, + CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5; + private static final Rotation2d CAMERA_FOV = Rotation2d.fromDegrees(90); + private static final double + AVERAGE_PIXEL_ERROR = 0.25, + PIXEL_STANDARD_DEVIATIONS = 0.08; + public static final SimCameraProperties SIM_CAMERA_PROPERTIES = new SimCameraProperties(); + + static { + configureSimCameraProperties(); + } + + private static void configureSimCameraProperties() { + SIM_CAMERA_PROPERTIES.setCalibration(CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOV); + SIM_CAMERA_PROPERTIES.setCalibError(AVERAGE_PIXEL_ERROR, PIXEL_STANDARD_DEVIATIONS); + SIM_CAMERA_PROPERTIES.setFPS(CAMERA_FPS); + SIM_CAMERA_PROPERTIES.setAvgLatencyMs(AVERAGE_CAMERA_LATENCY_MILLISECONDS); + SIM_CAMERA_PROPERTIES.setLatencyStdDevMs(CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); + } } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index e41dfce8..472e6b4d 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -4,6 +4,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.*; +import org.photonvision.simulation.VisionSystemSim; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; @@ -26,6 +27,7 @@ public class FieldConstants { } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); + public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSim"); public static final double FIELD_LENGTH_METERS = 16.54175, FIELD_WIDTH_METERS = 8.21; @@ -49,4 +51,8 @@ private static HashMap fieldLayoutToTagIdToPoseMap() { tagIdToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); return tagIdToPose; } + + static { + VISION_SIMULATION.addAprilTags(APRIL_TAG_FIELD_LAYOUT); + } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c33487d1..bd805a8b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.trigon.robot.Robot; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; @@ -44,10 +43,7 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; this.translationStandardDeviationExponent = translationStandardDeviationExponent; - if (Robot.IS_REAL) - aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); - else - aprilTagCameraIO = new AprilTagCameraIO(); + aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); } public void update() { @@ -55,6 +51,7 @@ public void update() { robotPose = calculateBestRobotPose(); logCameraInfo(); + FieldConstants.VISION_SIMULATION.update(robotPose); } public boolean hasNewResult() { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index d6b080b9..d4f9e302 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -6,12 +6,14 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; import org.opencv.core.Point; import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -20,9 +22,11 @@ public class AprilTagPhotonCameraIO extends AprilTagCameraIO { private final PhotonCamera photonCamera; + private final PhotonCameraSim cameraSim; public AprilTagPhotonCameraIO(String cameraName) { photonCamera = new PhotonCamera(cameraName); + cameraSim = new PhotonCameraSim(photonCamera, CameraConstants.SIM_CAMERA_PROPERTIES); } protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { From 7ad48df7f35f378fb91bbba561bf8fafdfb4a1bb Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 11 Nov 2024 09:58:40 +0200 Subject: [PATCH 240/261] cleaned LEDs a bit. --- .../robot/subsystems/ledstrip/LEDStrip.java | 28 ++++++++----------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java index c22c63ba..d0556aa1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java +++ b/src/main/java/frc/trigon/robot/subsystems/ledstrip/LEDStrip.java @@ -16,13 +16,10 @@ public class LEDStrip extends SubsystemBase { private final boolean inverted; private final int numberOfLEDs; private int lastBreatheLED; - private double lastBreatheMovementTime = 0; + private double lastLEDMovementTime = 0; private double rainbowFirstPixelHue = 0; private boolean areLEDsOnForBlinking = false; - private double lastBlinkTime = 0; private boolean alternateColor = true; - private double lastAlternateColorTime = 0; - private double lastColorFlowTime = 0; private int amountOfColorFlowLEDs = 0; static { @@ -62,13 +59,10 @@ public int getNumberOfLEDS() { void resetLEDSettings() { lastBreatheLED = indexOffset; - lastBreatheMovementTime = Timer.getFPGATimestamp(); + lastLEDMovementTime = Timer.getFPGATimestamp(); rainbowFirstPixelHue = 0; areLEDsOnForBlinking = false; - lastBlinkTime = 0; alternateColor = true; - lastAlternateColorTime = 0; - lastColorFlowTime = 0; amountOfColorFlowLEDs = 0; } @@ -78,8 +72,8 @@ void clearLEDColors() { void blink(Color firstColor, Color secondColor, double blinkingIntervalSeconds) { double currentTime = Timer.getFPGATimestamp(); - if (currentTime - lastBlinkTime > blinkingIntervalSeconds) { - lastBlinkTime = currentTime; + if (currentTime - lastLEDMovementTime > blinkingIntervalSeconds) { + lastLEDMovementTime = currentTime; areLEDsOnForBlinking = !areLEDsOnForBlinking; } if (areLEDsOnForBlinking) @@ -112,8 +106,8 @@ void breathe(Color color, int breathingLEDs, double cycleTimeSeconds, boolean sh inverted = this.inverted != inverted; double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; double currentTime = Timer.getFPGATimestamp(); - if (currentTime - lastBreatheMovementTime > moveLEDTimeSeconds) { - lastBreatheMovementTime = currentTime; + if (currentTime - lastLEDMovementTime > moveLEDTimeSeconds) { + lastLEDMovementTime = currentTime; if (inverted) lastBreatheLED--; else @@ -138,8 +132,9 @@ void colorFlow(Color color, double cycleTimeSeconds, boolean shouldLoop, boolean clearLEDColors(); inverted = this.inverted != inverted; double moveLEDTimeSeconds = cycleTimeSeconds / numberOfLEDs; - if (Timer.getFPGATimestamp() - lastColorFlowTime > moveLEDTimeSeconds) { - lastColorFlowTime = Timer.getFPGATimestamp(); + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastLEDMovementTime > moveLEDTimeSeconds) { + lastLEDMovementTime = currentTime; if (inverted) amountOfColorFlowLEDs--; else @@ -160,9 +155,10 @@ void colorFlow(Color color, double cycleTimeSeconds, boolean shouldLoop, boolean } void alternateColor(Color firstColor, Color secondColor, double intervalSeconds) { - if (Timer.getFPGATimestamp() - lastAlternateColorTime > intervalSeconds) { + double currentTime = Timer.getFPGATimestamp(); + if (currentTime - lastLEDMovementTime > intervalSeconds) { alternateColor = !alternateColor; - lastAlternateColorTime = Timer.getFPGATimestamp(); + lastLEDMovementTime = currentTime; } if (alternateColor) { for (int i = 0; i < numberOfLEDs; i++) From 173c1d8dbad616aef69411fc743dde91807dfb98 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 11 Nov 2024 07:13:45 -0500 Subject: [PATCH 241/261] Changes --- .../robot/poseestimation/apriltagcamera/AprilTagCamera.java | 2 ++ .../poseestimation/apriltagcamera/AprilTagCameraIO.java | 4 ++++ .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 6 ++++++ 3 files changed, 12 insertions(+) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index bd805a8b..b29507da 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -44,6 +44,8 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT this.translationStandardDeviationExponent = translationStandardDeviationExponent; aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); + if (aprilTagCameraType == AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA) + aprilTagCameraIO.addSimCamera(robotCenterToCamera); } public void update() { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index c9233bb2..973b4187 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -1,12 +1,16 @@ package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; import org.littletonrobotics.junction.AutoLog; public class AprilTagCameraIO { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { } + protected void addSimCamera(Transform3d robotToCamera) { + } + @AutoLog public static class AprilTagCameraInputs { public boolean hasResult = false; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index d4f9e302..d9cd0f9c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -29,6 +29,7 @@ public AprilTagPhotonCameraIO(String cameraName) { cameraSim = new PhotonCameraSim(photonCamera, CameraConstants.SIM_CAMERA_PROPERTIES); } + @Override protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); @@ -39,6 +40,11 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { updateNoResultInputs(inputs); } + @Override + protected void addSimCamera(Transform3d robotToCamera) { + FieldConstants.VISION_SIMULATION.addCamera(cameraSim, robotToCamera); + } + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); From 0b47b1001d35b5ac204410f2e0ee893feb606158 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 11 Nov 2024 19:00:05 +0200 Subject: [PATCH 242/261] =?UTF-8?q?Stuff=20works=20=F0=9F=92=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../robot/constants/CameraConstants.java | 32 +++++++++---------- .../apriltagcamera/AprilTagCamera.java | 6 ++-- .../AprilTagCameraConstants.java | 11 ++++--- .../io/AprilTagPhotonCameraIO.java | 11 ++++--- 4 files changed, 33 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 8bb9c2cd..a3e085ef 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -25,6 +25,22 @@ public class CameraConstants { new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33), Math.PI + Units.degreesToRadians(0)) ); + private static final int + CAMERA_WIDTH = 1600, + CAMERA_HEIGHT = 1200, + CAMERA_FPS = 60, + AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, + CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5; + private static final Rotation2d CAMERA_FOV = Rotation2d.fromDegrees(90); + private static final double + AVERAGE_PIXEL_ERROR = 0.25, + PIXEL_STANDARD_DEVIATIONS = 0.08; + public static final SimCameraProperties SIM_CAMERA_PROPERTIES = new SimCameraProperties(); + + static { + configureSimCameraProperties(); + } + public static final AprilTagCamera FRONT_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, @@ -42,22 +58,6 @@ public class CameraConstants { ); public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); - private static final int - CAMERA_WIDTH = 720, - CAMERA_HEIGHT = 1920, - CAMERA_FPS = 60, - AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, - CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5; - private static final Rotation2d CAMERA_FOV = Rotation2d.fromDegrees(90); - private static final double - AVERAGE_PIXEL_ERROR = 0.25, - PIXEL_STANDARD_DEVIATIONS = 0.08; - public static final SimCameraProperties SIM_CAMERA_PROPERTIES = new SimCameraProperties(); - - static { - configureSimCameraProperties(); - } - private static void configureSimCameraProperties() { SIM_CAMERA_PROPERTIES.setCalibration(CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOV); SIM_CAMERA_PROPERTIES.setCalibError(AVERAGE_PIXEL_ERROR, PIXEL_STANDARD_DEVIATIONS); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index b29507da..a286c9e0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -5,6 +5,8 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; @@ -43,7 +45,7 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; this.translationStandardDeviationExponent = translationStandardDeviationExponent; - aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); + aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, CameraConstants.SIM_CAMERA_PROPERTIES); if (aprilTagCameraType == AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA) aprilTagCameraIO.addSimCamera(robotCenterToCamera); } @@ -52,8 +54,8 @@ public void update() { aprilTagCameraIO.updateInputs(inputs); robotPose = calculateBestRobotPose(); + FieldConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); logCameraInfo(); - FieldConstants.VISION_SIMULATION.update(robotPose); } public boolean hasNewResult() { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index deef468f..f121964a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -3,8 +3,9 @@ import edu.wpi.first.math.geometry.Pose2d; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; +import org.photonvision.simulation.SimCameraProperties; -import java.util.function.Function; +import java.util.function.BiFunction; public class AprilTagCameraConstants { static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2.5; @@ -15,12 +16,12 @@ public class AprilTagCameraConstants { public enum AprilTagCameraType { PHOTON_CAMERA(AprilTagPhotonCameraIO::new), - LIMELIGHT(AprilTagLimelightIO::new); + LIMELIGHT((name, properties) -> new AprilTagLimelightIO(name)); - final Function createIOFunction; + final BiFunction createIOBiFunction; - AprilTagCameraType(Function createIOFunction) { - this.createIOFunction = createIOFunction; + AprilTagCameraType(BiFunction createIOBiFunction) { + this.createIOBiFunction = createIOBiFunction; } } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index d9cd0f9c..36f5e499 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; -import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; @@ -14,6 +13,7 @@ import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -24,14 +24,17 @@ public class AprilTagPhotonCameraIO extends AprilTagCameraIO { private final PhotonCamera photonCamera; private final PhotonCameraSim cameraSim; - public AprilTagPhotonCameraIO(String cameraName) { + public AprilTagPhotonCameraIO(String cameraName, SimCameraProperties cameraProperties) { photonCamera = new PhotonCamera(cameraName); - cameraSim = new PhotonCameraSim(photonCamera, CameraConstants.SIM_CAMERA_PROPERTIES); + cameraSim = new PhotonCameraSim(photonCamera, cameraProperties); + cameraSim.enableProcessedStream(true); + cameraSim.enableDrawWireframe(true); } @Override protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { - final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); + final List unreadResults = photonCamera.getAllUnreadResults(); + final PhotonPipelineResult latestResult = unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1); inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; if (inputs.hasResult) From 11b846ddfb43459ffb838c60e20670a8b2b04f2c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 11 Nov 2024 13:34:36 -0500 Subject: [PATCH 243/261] Camera sim is working + fixed Front Camera problem (Thanks to the sim <3) --- .../robot/constants/CameraConstants.java | 3 ++- .../apriltagcamera/AprilTagCamera.java | 5 +++-- .../io/AprilTagPhotonCameraIO.java | 18 +++++++++++++----- 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index a3e085ef..5578ed88 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -17,7 +17,7 @@ public class CameraConstants { private static final Transform3d FRONT_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0.0465, 0.325, 0.192), + new Translation3d(0.325, 0.0465, 0.192), new Rotation3d(0, Units.degreesToRadians(-31), 0) ), REAR_CENTER_TO_CAMERA = new Transform3d( @@ -35,6 +35,7 @@ public class CameraConstants { private static final double AVERAGE_PIXEL_ERROR = 0.25, PIXEL_STANDARD_DEVIATIONS = 0.08; + public static final boolean SHOULD_USE_CAMERA_SIMULATION = true; public static final SimCameraProperties SIM_CAMERA_PROPERTIES = new SimCameraProperties(); static { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index a286c9e0..59ecdd93 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -46,7 +46,7 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT this.translationStandardDeviationExponent = translationStandardDeviationExponent; aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, CameraConstants.SIM_CAMERA_PROPERTIES); - if (aprilTagCameraType == AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA) + if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION && aprilTagCameraType == AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA) aprilTagCameraIO.addSimCamera(robotCenterToCamera); } @@ -54,8 +54,9 @@ public void update() { aprilTagCameraIO.updateInputs(inputs); robotPose = calculateBestRobotPose(); - FieldConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); logCameraInfo(); + if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION) + FieldConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); } public boolean hasNewResult() { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 36f5e499..0cab8d4a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -6,10 +6,12 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.robot.subsystems.MotorSubsystem; import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; @@ -26,15 +28,16 @@ public class AprilTagPhotonCameraIO extends AprilTagCameraIO { public AprilTagPhotonCameraIO(String cameraName, SimCameraProperties cameraProperties) { photonCamera = new PhotonCamera(cameraName); - cameraSim = new PhotonCameraSim(photonCamera, cameraProperties); - cameraSim.enableProcessedStream(true); - cameraSim.enableDrawWireframe(true); + if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION && MotorSubsystem.isExtensiveLoggingEnabled()) { + cameraSim = new PhotonCameraSim(photonCamera, cameraProperties); + cameraSim.enableDrawWireframe(true); + } else + cameraSim = null; } @Override protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { - final List unreadResults = photonCamera.getAllUnreadResults(); - final PhotonPipelineResult latestResult = unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1); + final PhotonPipelineResult latestResult = getLatestPipelineResult(); inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; if (inputs.hasResult) @@ -48,6 +51,11 @@ protected void addSimCamera(Transform3d robotToCamera) { FieldConstants.VISION_SIMULATION.addCamera(cameraSim, robotToCamera); } + private PhotonPipelineResult getLatestPipelineResult() { + final List unreadResults = photonCamera.getAllUnreadResults(); + return unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1); + } + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); From d104869bba9533264fadd24529c5ec49966a5f3d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 11 Nov 2024 13:36:46 -0500 Subject: [PATCH 244/261] Removed LoggedDashboardNumber --- .../java/frc/trigon/robot/constants/ShootingConstants.java | 5 ++--- .../java/frc/trigon/robot/misc/ShootingCalculations.java | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index dcce3377..1b5620fd 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -4,15 +4,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35, - FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10; - public static final LoggedDashboardNumber SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = new LoggedDashboardNumber("SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND", 3); + FINISHED_INTAKE_SHOOTER_VELOCITY_ROTATIONS_PER_SECOND = -10, + SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND = 3; public static final double CLOSE_SHOT_VELOCITY_ROTATIONS_PER_SECOND = 45, diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index d89c9a9e..48fb0ef6 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -137,7 +137,7 @@ private TargetShootingState calculateTargetShootingState(TargetShootingState sta private TargetShootingState calculateTargetShootingState(Translation3d shootingVector) { final MirrorableRotation2d targetRobotAngle = new MirrorableRotation2d(getYaw(shootingVector), false); final Rotation2d targetPitch = getPitch(shootingVector); - final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()) + ShootingConstants.SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND.get(); + final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()) + ShootingConstants.SPEAKER_SHOT_STANDING_EXTRA_VELOCITY_BUFFER_ROTATIONS_PER_SECOND; return new TargetShootingState(targetRobotAngle, targetPitch, targetVelocity); } From fa0fe7c6dea572bfa01e021a5d1b5923605b1b09 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 13 Nov 2024 12:30:44 +0200 Subject: [PATCH 245/261] Added sim camera properties stuff --- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 5578ed88..923e5c36 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -30,7 +30,8 @@ public class CameraConstants { CAMERA_HEIGHT = 1200, CAMERA_FPS = 60, AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, - CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5; + CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5, + CAMERA_EXPOSURE_TIME_MILLISECONDS = 10; private static final Rotation2d CAMERA_FOV = Rotation2d.fromDegrees(90); private static final double AVERAGE_PIXEL_ERROR = 0.25, @@ -65,5 +66,6 @@ private static void configureSimCameraProperties() { SIM_CAMERA_PROPERTIES.setFPS(CAMERA_FPS); SIM_CAMERA_PROPERTIES.setAvgLatencyMs(AVERAGE_CAMERA_LATENCY_MILLISECONDS); SIM_CAMERA_PROPERTIES.setLatencyStdDevMs(CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); + SIM_CAMERA_PROPERTIES.setExposureTimeMs(CAMERA_EXPOSURE_TIME_MILLISECONDS); } } From f8515633b462abc5286cab1fa5b7cb361a828bc0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 13 Nov 2024 18:01:01 +0200 Subject: [PATCH 246/261] Minor sim camera clean --- .../robot/constants/CameraConstants.java | 18 +++++++++--------- .../apriltagcamera/AprilTagCamera.java | 2 +- .../io/AprilTagPhotonCameraIO.java | 5 +++-- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 923e5c36..44b5f397 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -37,10 +37,10 @@ public class CameraConstants { AVERAGE_PIXEL_ERROR = 0.25, PIXEL_STANDARD_DEVIATIONS = 0.08; public static final boolean SHOULD_USE_CAMERA_SIMULATION = true; - public static final SimCameraProperties SIM_CAMERA_PROPERTIES = new SimCameraProperties(); + public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); static { - configureSimCameraProperties(); + configureSimulationCameraProperties(); } public static final AprilTagCamera @@ -60,12 +60,12 @@ public class CameraConstants { ); public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); - private static void configureSimCameraProperties() { - SIM_CAMERA_PROPERTIES.setCalibration(CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOV); - SIM_CAMERA_PROPERTIES.setCalibError(AVERAGE_PIXEL_ERROR, PIXEL_STANDARD_DEVIATIONS); - SIM_CAMERA_PROPERTIES.setFPS(CAMERA_FPS); - SIM_CAMERA_PROPERTIES.setAvgLatencyMs(AVERAGE_CAMERA_LATENCY_MILLISECONDS); - SIM_CAMERA_PROPERTIES.setLatencyStdDevMs(CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); - SIM_CAMERA_PROPERTIES.setExposureTimeMs(CAMERA_EXPOSURE_TIME_MILLISECONDS); + private static void configureSimulationCameraProperties() { + SIMULATION_CAMERA_PROPERTIES.setCalibration(CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOV); + SIMULATION_CAMERA_PROPERTIES.setCalibError(AVERAGE_PIXEL_ERROR, PIXEL_STANDARD_DEVIATIONS); + SIMULATION_CAMERA_PROPERTIES.setFPS(CAMERA_FPS); + SIMULATION_CAMERA_PROPERTIES.setAvgLatencyMs(AVERAGE_CAMERA_LATENCY_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setLatencyStdDevMs(CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setExposureTimeMs(CAMERA_EXPOSURE_TIME_MILLISECONDS); } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 59ecdd93..c6bca9c9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -45,7 +45,7 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; this.translationStandardDeviationExponent = translationStandardDeviationExponent; - aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, CameraConstants.SIM_CAMERA_PROPERTIES); + aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, CameraConstants.SIMULATION_CAMERA_PROPERTIES); if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION && aprilTagCameraType == AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA) aprilTagCameraIO.addSimCamera(robotCenterToCamera); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 0cab8d4a..233d9ae2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -31,8 +31,9 @@ public AprilTagPhotonCameraIO(String cameraName, SimCameraProperties cameraPrope if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION && MotorSubsystem.isExtensiveLoggingEnabled()) { cameraSim = new PhotonCameraSim(photonCamera, cameraProperties); cameraSim.enableDrawWireframe(true); - } else - cameraSim = null; + return; + } + cameraSim = null; } @Override From 6238e483cb349066abcda82d5a83f87c358d70bc Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 13 Nov 2024 18:05:54 +0200 Subject: [PATCH 247/261] Better camera IO stuffies idk --- .../java/frc/trigon/robot/constants/CameraConstants.java | 6 ++++-- .../robot/poseestimation/apriltagcamera/AprilTagCamera.java | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 44b5f397..310eefc8 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -49,14 +49,16 @@ public class CameraConstants { "FrontTagCamera", FRONT_CENTER_TO_CAMERA, THETA_STD_EXPONENT, - TRANSLATIONS_STD_EXPONENT + TRANSLATIONS_STD_EXPONENT, + SIMULATION_CAMERA_PROPERTIES ), REAR_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "RearTagCamera", REAR_CENTER_TO_CAMERA, THETA_STD_EXPONENT, - TRANSLATIONS_STD_EXPONENT + TRANSLATIONS_STD_EXPONENT, + SIMULATION_CAMERA_PROPERTIES ); public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index c6bca9c9..b694c8c8 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -10,6 +10,7 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; +import org.photonvision.simulation.SimCameraProperties; /** * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. @@ -39,13 +40,14 @@ public class AprilTagCamera { public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotCenterToCamera, double thetaStandardDeviationExponent, - double translationStandardDeviationExponent) { + double translationStandardDeviationExponent, + SimCameraProperties simulationCameraProperties) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; this.translationStandardDeviationExponent = translationStandardDeviationExponent; - aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, CameraConstants.SIMULATION_CAMERA_PROPERTIES); + aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, simulationCameraProperties); if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION && aprilTagCameraType == AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA) aprilTagCameraIO.addSimCamera(robotCenterToCamera); } From 4ec3a0b10552bf91d44428be24fa05e0451a0700 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 14 Nov 2024 16:28:38 -0500 Subject: [PATCH 248/261] Added IO for sim camera --- .../robot/constants/CameraConstants.java | 39 +---- .../robot/constants/FieldConstants.java | 28 ++- .../apriltagcamera/AprilTagCamera.java | 11 +- .../AprilTagCameraConstants.java | 40 ++++- .../apriltagcamera/AprilTagCameraIO.java | 2 +- .../io/AprilTagPhotonCameraIO.java | 18 +- .../io/AprilTagSimulationCameraIO.java | 160 ++++++++++++++++++ .../poseestimator/PoseEstimator.java | 6 +- .../robot/subsystems/swerve/SwerveModule.java | 3 +- 9 files changed, 229 insertions(+), 78 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 310eefc8..3840a6d5 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -1,6 +1,5 @@ package frc.trigon.robot.constants; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -8,7 +7,7 @@ import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; -import org.photonvision.simulation.SimCameraProperties; +import org.trigon.hardware.RobotHardwareStats; public class CameraConstants { public static final double @@ -25,49 +24,23 @@ public class CameraConstants { new Rotation3d(Math.PI - Units.degreesToRadians(0), Units.degreesToRadians(-33), Math.PI + Units.degreesToRadians(0)) ); - private static final int - CAMERA_WIDTH = 1600, - CAMERA_HEIGHT = 1200, - CAMERA_FPS = 60, - AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, - CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5, - CAMERA_EXPOSURE_TIME_MILLISECONDS = 10; - private static final Rotation2d CAMERA_FOV = Rotation2d.fromDegrees(90); - private static final double - AVERAGE_PIXEL_ERROR = 0.25, - PIXEL_STANDARD_DEVIATIONS = 0.08; - public static final boolean SHOULD_USE_CAMERA_SIMULATION = true; - public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); - - static { - configureSimulationCameraProperties(); - } - public static final AprilTagCamera FRONT_TAG_CAMERA = new AprilTagCamera( - AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + RobotHardwareStats.isSimulation() ? AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA : AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "FrontTagCamera", FRONT_CENTER_TO_CAMERA, THETA_STD_EXPONENT, TRANSLATIONS_STD_EXPONENT, - SIMULATION_CAMERA_PROPERTIES + AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES ), REAR_TAG_CAMERA = new AprilTagCamera( - AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + RobotHardwareStats.isSimulation() ? AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA : AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "RearTagCamera", REAR_CENTER_TO_CAMERA, THETA_STD_EXPONENT, TRANSLATIONS_STD_EXPONENT, - SIMULATION_CAMERA_PROPERTIES + AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES ); - public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); - private static void configureSimulationCameraProperties() { - SIMULATION_CAMERA_PROPERTIES.setCalibration(CAMERA_WIDTH, CAMERA_HEIGHT, CAMERA_FOV); - SIMULATION_CAMERA_PROPERTIES.setCalibError(AVERAGE_PIXEL_ERROR, PIXEL_STANDARD_DEVIATIONS); - SIMULATION_CAMERA_PROPERTIES.setFPS(CAMERA_FPS); - SIMULATION_CAMERA_PROPERTIES.setAvgLatencyMs(AVERAGE_CAMERA_LATENCY_MILLISECONDS); - SIMULATION_CAMERA_PROPERTIES.setLatencyStdDevMs(CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); - SIMULATION_CAMERA_PROPERTIES.setExposureTimeMs(CAMERA_EXPOSURE_TIME_MILLISECONDS); - } + public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 472e6b4d..8773fbc4 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -4,7 +4,6 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.*; -import org.photonvision.simulation.VisionSystemSim; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; @@ -12,22 +11,11 @@ import java.util.HashMap; public class FieldConstants { + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; - - static { - try { - APRIL_TAG_FIELD_LAYOUT = SHOULD_USE_HOME_TAG_LAYOUT ? - AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json") : - AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - } catch (IOException e) { - throw new RuntimeException(e); - } - } public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); - public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSim"); public static final double FIELD_LENGTH_METERS = 16.54175, FIELD_WIDTH_METERS = 8.21; @@ -45,14 +33,20 @@ public class FieldConstants { public static final MirrorablePose2d IN_FRONT_OF_AMP_POSE = new MirrorablePose2d(1.842, 8.204 - 0.405, Rotation2d.fromDegrees(-90), true); public static final double MAXIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS = 2.5; + private static AprilTagFieldLayout createAprilTagFieldLayout() { + try { + return SHOULD_USE_HOME_TAG_LAYOUT ? + AprilTagFieldLayout.loadFromResource("2024-crescendo-home-tag-layout.json") : + AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + private static HashMap fieldLayoutToTagIdToPoseMap() { final HashMap tagIdToPose = new HashMap<>(); for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) tagIdToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); return tagIdToPose; } - - static { - VISION_SIMULATION.addAprilTags(APRIL_TAG_FIELD_LAYOUT); - } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index b694c8c8..00bc7ee6 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -6,11 +6,12 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; +import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; import org.photonvision.simulation.SimCameraProperties; +import org.trigon.hardware.RobotHardwareStats; /** * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. @@ -48,8 +49,8 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT this.translationStandardDeviationExponent = translationStandardDeviationExponent; aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, simulationCameraProperties); - if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION && aprilTagCameraType == AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA) - aprilTagCameraIO.addSimCamera(robotCenterToCamera); + if (RobotHardwareStats.isSimulation()) + aprilTagCameraIO.addSimulatedCameraToVisionSimulation(robotCenterToCamera); } public void update() { @@ -57,8 +58,8 @@ public void update() { robotPose = calculateBestRobotPose(); logCameraInfo(); - if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION) - FieldConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); + if (RobotHardwareStats.isSimulation() && MotorSubsystem.isExtensiveLoggingEnabled()) + AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); } public boolean hasNewResult() { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index f121964a..903aeb32 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -1,9 +1,15 @@ package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO; +import frc.trigon.robot.subsystems.MotorSubsystem; import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.trigon.hardware.RobotHardwareStats; import java.util.function.BiFunction; @@ -14,8 +20,40 @@ public class AprilTagCameraConstants { public static final double MAXIMUM_AMBIGUITY = 0.5; + public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSim"); + + private static final int + SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600, + SIMULATION_CAMERA_RESOLUTION_HEIGHT = 1200, + SIMULATION_CAMERA_FPS = 60, + SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, + SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5, + SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 10; + private static final Rotation2d SIMULATION_CAMERA_FOV = Rotation2d.fromDegrees(70); + private static final double + SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0.25, + SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0.08; + public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); + + static { + if (RobotHardwareStats.isSimulation() && MotorSubsystem.isExtensiveLoggingEnabled()) { + configureSimulationCameraProperties(); + VISION_SIMULATION.addAprilTags(FieldConstants.APRIL_TAG_FIELD_LAYOUT); + } + } + + private static void configureSimulationCameraProperties() { + SIMULATION_CAMERA_PROPERTIES.setCalibration(SIMULATION_CAMERA_RESOLUTION_WIDTH, SIMULATION_CAMERA_RESOLUTION_HEIGHT, SIMULATION_CAMERA_FOV); + SIMULATION_CAMERA_PROPERTIES.setCalibError(SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR, SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS); + SIMULATION_CAMERA_PROPERTIES.setFPS(SIMULATION_CAMERA_FPS); + SIMULATION_CAMERA_PROPERTIES.setAvgLatencyMs(SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setLatencyStdDevMs(SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS); + SIMULATION_CAMERA_PROPERTIES.setExposureTimeMs(SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS); + } + public enum AprilTagCameraType { - PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + PHOTON_CAMERA((name, properties) -> new AprilTagPhotonCameraIO(name)), + SIMULATION_CAMERA(AprilTagSimulationCameraIO::new), LIMELIGHT((name, properties) -> new AprilTagLimelightIO(name)); final BiFunction createIOBiFunction; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index 973b4187..d5d5e4aa 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -8,7 +8,7 @@ public class AprilTagCameraIO { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { } - protected void addSimCamera(Transform3d robotToCamera) { + protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) { } @AutoLog diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 233d9ae2..e6f45f8d 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -6,16 +6,12 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; -import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; -import frc.trigon.robot.subsystems.MotorSubsystem; import org.opencv.core.Point; import org.photonvision.PhotonCamera; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -24,16 +20,9 @@ public class AprilTagPhotonCameraIO extends AprilTagCameraIO { private final PhotonCamera photonCamera; - private final PhotonCameraSim cameraSim; - public AprilTagPhotonCameraIO(String cameraName, SimCameraProperties cameraProperties) { + public AprilTagPhotonCameraIO(String cameraName) { photonCamera = new PhotonCamera(cameraName); - if (CameraConstants.SHOULD_USE_CAMERA_SIMULATION && MotorSubsystem.isExtensiveLoggingEnabled()) { - cameraSim = new PhotonCameraSim(photonCamera, cameraProperties); - cameraSim.enableDrawWireframe(true); - return; - } - cameraSim = null; } @Override @@ -47,11 +36,6 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { updateNoResultInputs(inputs); } - @Override - protected void addSimCamera(Transform3d robotToCamera) { - FieldConstants.VISION_SIMULATION.addCamera(cameraSim, robotToCamera); - } - private PhotonPipelineResult getLatestPipelineResult() { final List unreadResults = photonCamera.getAllUnreadResults(); return unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java new file mode 100644 index 00000000..622d869d --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -0,0 +1,160 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.opencv.core.Point; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import java.util.List; + +public class AprilTagSimulationCameraIO extends AprilTagCameraIO { + private final PhotonCamera photonCamera; + private final PhotonCameraSim cameraSim; + + public AprilTagSimulationCameraIO(String cameraName, SimCameraProperties cameraProperties) { + photonCamera = new PhotonCamera(cameraName); + + cameraSim = new PhotonCameraSim(photonCamera, cameraProperties); + cameraSim.enableDrawWireframe(true); + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final PhotonPipelineResult latestResult = getLatestPipelineResult(); + + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; + if (inputs.hasResult) + updateHasResultInputs(inputs, latestResult); + else + updateNoResultInputs(inputs); + } + + @Override + protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) { + AprilTagCameraConstants.VISION_SIMULATION.addCamera(cameraSim, robotToCamera); + } + + private PhotonPipelineResult getLatestPipelineResult() { + final List unreadResults = photonCamera.getAllUnreadResults(); + return unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1); + } + + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); + inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); + inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + } + + private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[]{}; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private double getPoseAmbiguity(PhotonPipelineResult result) { + return getBestTarget(result).getPoseAmbiguity(); + } + + private Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param result the camera's pipeline result + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { + final List tagCorners = getBestTarget(result).getDetectedCorners(); + final Point tagCenter = getTagCenter(tagCorners); + if (photonCamera.getCameraMatrix().isPresent()) + return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); + return null; + } + + /** + * Estimates the camera's pose using Solve PNP using as many tags as possible. + * + * @param result the camera's pipeline result + * @return the estimated pose + */ + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + if (result.getMultiTagResult().isPresent()) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; + return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); + } + + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(getBestTarget(result).getFiducialId()); + final Transform3d targetToCamera = getBestTarget(result).getBestCameraToTarget().inverse(); + return tagPose.transformBy(targetToCamera); + } + + private int[] getVisibleTagIDs(PhotonPipelineResult result) { + final int[] visibleTagIDs = new int[result.getTargets().size()]; + visibleTagIDs[0] = getBestTarget(result).getFiducialId(); + int idAddition = 1; + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = result.getTargets().get(i).getFiducialId(); + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + visibleTagIDs[i + idAddition] = currentID; + } + return visibleTagIDs; + } + + private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { + PhotonTrackedTarget bestTarget = result.getBestTarget(); + for (PhotonTrackedTarget target : result.getTargets()) { + if (target.getArea() > bestTarget.area) + bestTarget = target; + } + return bestTarget; + } + + private double getDistanceFromBestTag(PhotonPipelineResult result) { + return getBestTarget(result).getBestCameraToTarget().getTranslation().getNorm(); + } + + private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { + double fx = camIntrinsics.get(0, 0); + double cx = camIntrinsics.get(0, 2); + double xOffset = cx - pixel.x; + + double fy = camIntrinsics.get(1, 1); + double cy = camIntrinsics.get(1, 2); + double yOffset = cy - pixel.y; + + // calculate yaw normally + var yaw = new Rotation2d(fx, xOffset); + // correct pitch based on yaw + var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); + + return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 2d9a9d26..552cca91 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -87,9 +87,9 @@ public void setGyroHeadingToBestSolvePNPHeading() { if (aprilTagCameras[i].hasResult() && aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) closestCameraToTag = i; } - - final Rotation2d bestCameraHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); - resetPose(new Pose2d(getCurrentPose().getTranslation(), bestCameraHeading)); + + final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); } private void updateFromVision() { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index c33811b8..5f98ed52 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -175,11 +175,12 @@ private void configureHardware(double offsetRotations) { private void configureSignals() { driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); From 7580fab5a135ccb2cd0a6b672b17a1aa0beb5737 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 14 Nov 2024 16:32:01 -0500 Subject: [PATCH 249/261] Updated TRIGONLib --- build.gradle | 2 +- .../java/frc/trigon/robot/constants/RobotConstants.java | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/build.gradle b/build.gradle index 7341a1d4..774465e5 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:debugging-SNAPSHOT' + implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.13' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index dda4e7a0..ac27a683 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -6,14 +6,15 @@ public class RobotConstants { private static final boolean - IS_SIMULATION = false, + IS_SIMULATION = true, IS_REPLAY = false; + private static final RobotHardwareStats.ReplayType REPLAY_TYPE = IS_SIMULATION ? RobotHardwareStats.ReplayType.SIMULATION_REPLAY : RobotHardwareStats.ReplayType.REAL_REPLAY; private static final double PERIODIC_TIME_SECONDS = 0.02; - public static final String CANIVORE_NAME = "SwerveCANivore"; + public static final String CANIVORE_NAME = "CANivore"; public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; public static void init() { - RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY); + RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_REPLAY ? REPLAY_TYPE : RobotHardwareStats.ReplayType.NONE); RobotHardwareStats.setPeriodicTimeSeconds(PERIODIC_TIME_SECONDS); } -} +} \ No newline at end of file From 45f4d0d30caadb93390f62b670a0abc57b97f268 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 14 Nov 2024 16:34:28 -0500 Subject: [PATCH 250/261] Updated PhotonLib --- vendordeps/photonlib.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8f3d842e..6d1f485d 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "dev-v2024.3.0-89-gd9b6199c", + "version": "v2025.0.0-beta-2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "dev-v2024.3.0-89-gd9b6199c", + "version": "v2025.0.0-beta-2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "dev-v2024.3.0-89-gd9b6199c", + "version": "v2025.0.0-beta-2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "dev-v2024.3.0-89-gd9b6199c" + "version": "v2025.0.0-beta-2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "dev-v2024.3.0-89-gd9b6199c" + "version": "v2025.0.0-beta-2" } ] } \ No newline at end of file From 99afb9be572db722406d5cc21bf936e90b276acc Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 15 Nov 2024 12:55:30 +0200 Subject: [PATCH 251/261] Fixed reset auto pose --- .../trigon/robot/commands/factories/AutonomousCommands.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 8d1d4e90..2086abd6 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -38,7 +38,8 @@ public static Command getResetPoseToAutoPoseCommand(Supplier pathName) { if (DriverStation.isEnabled()) return; final Pose2d autoStartPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathName.get()); - RobotContainer.POSE_ESTIMATOR.resetPose(new MirrorablePose2d(autoStartPose, true).get()); + final MirrorablePose2d correctedAutoStartPose = new MirrorablePose2d(autoStartPose, true); + RobotContainer.POSE_ESTIMATOR.resetPose(correctedAutoStartPose.get()); } ).ignoringDisable(true); } From c90e25782c5f72eea5c9edb4375d378a690fec8c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 17 Nov 2024 12:43:34 +0200 Subject: [PATCH 252/261] `getLatestPipelineResult` returns null --- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 4 ++-- .../apriltagcamera/io/AprilTagSimulationCameraIO.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index e6f45f8d..b8a89dc2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -29,7 +29,7 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = getLatestPipelineResult(); - inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; + inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; if (inputs.hasResult) updateHasResultInputs(inputs, latestResult); else @@ -38,7 +38,7 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { private PhotonPipelineResult getLatestPipelineResult() { final List unreadResults = photonCamera.getAllUnreadResults(); - return unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1); + return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java index 622d869d..362d38d6 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -35,7 +35,7 @@ public AprilTagSimulationCameraIO(String cameraName, SimCameraProperties cameraP protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = getLatestPipelineResult(); - inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; + inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; if (inputs.hasResult) updateHasResultInputs(inputs, latestResult); else @@ -49,7 +49,7 @@ protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) { private PhotonPipelineResult getLatestPipelineResult() { final List unreadResults = photonCamera.getAllUnreadResults(); - return unreadResults.isEmpty() ? new PhotonPipelineResult() : unreadResults.get(unreadResults.size() - 1); + return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { From 586dcf21d41d0f26ea9a75d4c5914458e28b6d95 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 17 Nov 2024 14:46:55 -0500 Subject: [PATCH 253/261] Minor cleaning --- .../apriltagcamera/AprilTagCamera.java | 14 +++---- .../AprilTagCameraConstants.java | 5 +-- .../io/AprilTagLimelightIO.java | 10 +++-- .../io/AprilTagPhotonCameraIO.java | 38 ++++++++++--------- .../io/AprilTagSimulationCameraIO.java | 38 ++++++++++--------- .../poseestimator/PoseEstimator.java | 2 +- 6 files changed, 55 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 00bc7ee6..4b1b633e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -8,7 +8,6 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; -import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; import org.photonvision.simulation.SimCameraProperties; import org.trigon.hardware.RobotHardwareStats; @@ -58,7 +57,7 @@ public void update() { robotPose = calculateBestRobotPose(); logCameraInfo(); - if (RobotHardwareStats.isSimulation() && MotorSubsystem.isExtensiveLoggingEnabled()) + if (RobotHardwareStats.isSimulation()) AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); } @@ -66,10 +65,6 @@ public boolean hasNewResult() { return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp(); } - public boolean hasResult() { - return inputs.hasResult; - } - public Pose2d getEstimatedRobotPose() { return robotPose; } @@ -87,7 +82,7 @@ public double getLatestResultTimestampSeconds() { } /** - * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calibrated gain. * The theta deviation is infinity when we assume the robot's heading because we already assume that the heading is correct. * * @return the standard deviations for the pose estimation strategy used @@ -128,7 +123,7 @@ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { if (!isWithinBestTagRangeForSolvePNP()) return new Pose2d(getFieldRelativeRobotTranslation(gyroHeading), gyroHeading); - final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + final Rotation2d solvePNPHeading = getSolvePNPHeading(); return new Pose2d(getFieldRelativeRobotTranslation(solvePNPHeading), solvePNPHeading); } @@ -145,6 +140,9 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); } + /** + * If the roll of the camera is 180 degrees, then the result needs to be flipped + */ private void setProperCameraRotation() { if (robotCenterToCamera.getRotation().getX() == Math.PI) { inputs.bestTargetRelativePitchRadians = -inputs.bestTargetRelativePitchRadians; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 903aeb32..5fefc502 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -6,7 +6,6 @@ import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO; -import frc.trigon.robot.subsystems.MotorSubsystem; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; import org.trigon.hardware.RobotHardwareStats; @@ -20,7 +19,7 @@ public class AprilTagCameraConstants { public static final double MAXIMUM_AMBIGUITY = 0.5; - public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSim"); + public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSimulation"); private static final int SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600, @@ -36,7 +35,7 @@ public class AprilTagCameraConstants { public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); static { - if (RobotHardwareStats.isSimulation() && MotorSubsystem.isExtensiveLoggingEnabled()) { + if (RobotHardwareStats.isSimulation()) { configureSimulationCameraProperties(); VISION_SIMULATION.addAprilTags(FieldConstants.APRIL_TAG_FIELD_LAYOUT); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 065e8275..1f10f356 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -21,10 +21,11 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { inputs.hasResult = results.targets_Fiducials.length > 0; - if (inputs.hasResult) + if (inputs.hasResult) { updateHasResultInputs(inputs, results); - else - updateNoResultInputs(inputs); + return; + } + updateNoResultInputs(inputs); } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { @@ -39,8 +40,9 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Limeli } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { - inputs.visibleTagIDs = new int[0]; inputs.cameraSolvePNPPose = new Pose3d(); + inputs.visibleTagIDs = new int[0]; + inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; } private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index b8a89dc2..47b79edc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -30,10 +30,11 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = getLatestPipelineResult(); inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; - if (inputs.hasResult) + if (inputs.hasResult) { updateHasResultInputs(inputs, latestResult); - else - updateNoResultInputs(inputs); + return; + } + updateNoResultInputs(inputs); } private PhotonPipelineResult getLatestPipelineResult() { @@ -41,6 +42,10 @@ private PhotonPipelineResult getLatestPipelineResult() { return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); } + private double getPoseAmbiguity(PhotonPipelineResult result) { + return getBestTarget(result).getPoseAmbiguity(); + } + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); @@ -53,22 +58,9 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Photon } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { - inputs.visibleTagIDs = new int[]{}; inputs.cameraSolvePNPPose = new Pose3d(); - } - - private double getPoseAmbiguity(PhotonPipelineResult result) { - return getBestTarget(result).getPoseAmbiguity(); - } - - private Point getTagCenter(List tagCorners) { - double tagCornerSumX = 0; - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) { - tagCornerSumX += tagCorner.x; - tagCornerSumY += tagCorner.y; - } - return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + inputs.visibleTagIDs = new int[0]; + inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; } /** @@ -85,6 +77,16 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { return null; } + private Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + /** * Estimates the camera's pose using Solve PNP using as many tags as possible. * diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java index 362d38d6..8aaf6f55 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -36,10 +36,11 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = getLatestPipelineResult(); inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; - if (inputs.hasResult) + if (inputs.hasResult) { updateHasResultInputs(inputs, latestResult); - else - updateNoResultInputs(inputs); + return; + } + updateNoResultInputs(inputs); } @Override @@ -52,6 +53,10 @@ private PhotonPipelineResult getLatestPipelineResult() { return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); } + private double getPoseAmbiguity(PhotonPipelineResult result) { + return getBestTarget(result).getPoseAmbiguity(); + } + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); @@ -64,22 +69,9 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Photon } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { - inputs.visibleTagIDs = new int[]{}; inputs.cameraSolvePNPPose = new Pose3d(); - } - - private double getPoseAmbiguity(PhotonPipelineResult result) { - return getBestTarget(result).getPoseAmbiguity(); - } - - private Point getTagCenter(List tagCorners) { - double tagCornerSumX = 0; - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) { - tagCornerSumX += tagCorner.x; - tagCornerSumY += tagCorner.y; - } - return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + inputs.visibleTagIDs = new int[0]; + inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; } /** @@ -96,6 +88,16 @@ private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { return null; } + private Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + /** * Estimates the camera's pose using Solve PNP using as many tags as possible. * diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 552cca91..2d8e8f24 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -84,7 +84,7 @@ public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPos public void setGyroHeadingToBestSolvePNPHeading() { int closestCameraToTag = 0; for (int i = 0; i < aprilTagCameras.length; i++) { - if (aprilTagCameras[i].hasResult() && aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) + if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) closestCameraToTag = i; } From a7b6d64d051ea27da337be3fcb802369a78138bd Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 18 Nov 2024 13:44:44 +0200 Subject: [PATCH 254/261] It still needs debugging... --- .../robot/constants/CameraConstants.java | 11 +- .../robot/constants/FieldConstants.java | 2 +- .../robot/constants/RobotConstants.java | 9 +- .../apriltagcamera/AprilTagCamera.java | 28 ++-- .../AprilTagCameraConstants.java | 15 +- .../apriltagcamera/AprilTagCameraIO.java | 6 + .../io/AprilTagPhotonCameraIO.java | 110 +++++-------- .../io/AprilTagSimulationCameraIO.java | 149 +----------------- .../poseestimator/PoseEstimator.java | 15 +- .../robot/subsystems/swerve/SwerveModule.java | 6 +- 10 files changed, 92 insertions(+), 259 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 3840a6d5..2b944864 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -7,7 +7,6 @@ import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; -import org.trigon.hardware.RobotHardwareStats; public class CameraConstants { public static final double @@ -26,20 +25,18 @@ public class CameraConstants { public static final AprilTagCamera FRONT_TAG_CAMERA = new AprilTagCamera( - RobotHardwareStats.isSimulation() ? AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA : AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "FrontTagCamera", FRONT_CENTER_TO_CAMERA, THETA_STD_EXPONENT, - TRANSLATIONS_STD_EXPONENT, - AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES + TRANSLATIONS_STD_EXPONENT ), REAR_TAG_CAMERA = new AprilTagCamera( - RobotHardwareStats.isSimulation() ? AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA : AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "RearTagCamera", REAR_CENTER_TO_CAMERA, THETA_STD_EXPONENT, - TRANSLATIONS_STD_EXPONENT, - AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES + TRANSLATIONS_STD_EXPONENT ); public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("NoteDetectionCamera"); diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 8773fbc4..8852cd24 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -11,8 +11,8 @@ import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index ac27a683..4eac1cc6 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -5,16 +5,13 @@ import org.trigon.utilities.FilesHandler; public class RobotConstants { - private static final boolean - IS_SIMULATION = true, - IS_REPLAY = false; - private static final RobotHardwareStats.ReplayType REPLAY_TYPE = IS_SIMULATION ? RobotHardwareStats.ReplayType.SIMULATION_REPLAY : RobotHardwareStats.ReplayType.REAL_REPLAY; + private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE; private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; - public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; + public static final String LOGGING_PATH = Robot.IS_REAL ? "/media/sda1/akitlogs/" : FilesHandler.DEPLOY_PATH + "logs/"; public static void init() { - RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_REPLAY ? REPLAY_TYPE : RobotHardwareStats.ReplayType.NONE); + RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, REPLAY_TYPE); RobotHardwareStats.setPeriodicTimeSeconds(PERIODIC_TIME_SECONDS); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 4b1b633e..ea29ae1b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -9,7 +9,6 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; import org.littletonrobotics.junction.Logger; -import org.photonvision.simulation.SimCameraProperties; import org.trigon.hardware.RobotHardwareStats; /** @@ -40,16 +39,18 @@ public class AprilTagCamera { public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotCenterToCamera, double thetaStandardDeviationExponent, - double translationStandardDeviationExponent, - SimCameraProperties simulationCameraProperties) { + double translationStandardDeviationExponent) { this.name = name; this.robotCenterToCamera = robotCenterToCamera; this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; this.translationStandardDeviationExponent = translationStandardDeviationExponent; - aprilTagCameraIO = aprilTagCameraType.createIOBiFunction.apply(name, simulationCameraProperties); - if (RobotHardwareStats.isSimulation()) + if (RobotHardwareStats.isSimulation()) { + aprilTagCameraIO = AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA.createIOFunction.apply(name); aprilTagCameraIO.addSimulatedCameraToVisionSimulation(robotCenterToCamera); + return; + } + aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); } public void update() { @@ -118,7 +119,7 @@ private Pose2d calculateBestRobotPose() { * @return the robot's pose */ private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { - if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult) + if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult || inputs.poseAmbiguity > AprilTagCameraConstants.MAXIMUM_AMBIGUITY) return null; if (!isWithinBestTagRangeForSolvePNP()) @@ -141,18 +142,19 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading } /** - * If the roll of the camera is 180 degrees, then the result needs to be flipped + * When the roll of the camera changes, the target pitch and yaw are also effected. + * This method corrects the yaw and pitch based on the camera's mount position roll. */ private void setProperCameraRotation() { - if (robotCenterToCamera.getRotation().getX() == Math.PI) { - inputs.bestTargetRelativePitchRadians = -inputs.bestTargetRelativePitchRadians; - inputs.bestTargetRelativeYawRadians = -inputs.bestTargetRelativeYawRadians; - } + Translation2d targetRotationVector = new Translation2d(inputs.bestTargetRelativeYawRadians, inputs.bestTargetRelativePitchRadians); + targetRotationVector.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); + inputs.bestTargetRelativeYawRadians = targetRotationVector.getY(); + inputs.bestTargetRelativePitchRadians = targetRotationVector.getX(); } private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); - final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneDistanceToTag(tagPose, robotPlaneTargetYawRadians); + final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneXYDistanceToTag(tagPose, robotPlaneTargetYawRadians); final double headingOffsetToUsedTagRadians = gyroHeading.getRadians() - robotPlaneTargetYawRadians + robotCenterToCamera.getRotation().getZ(); return new Translation2d(robotPlaneCameraDistanceToUsedTagMeters, Rotation2d.fromRadians(headingOffsetToUsedTagRadians)); } @@ -172,7 +174,7 @@ private double projectToPlane(double targetAngleRadians, double cameraAngleRadia return Math.atan(Math.tan(targetAngleRadians) * Math.cos(cameraAngleRadians)); } - private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { + private double calculateRobotPlaneXYDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { final double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); final double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 5fefc502..651dcc32 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -10,17 +10,16 @@ import org.photonvision.simulation.VisionSystemSim; import org.trigon.hardware.RobotHardwareStats; -import java.util.function.BiFunction; +import java.util.function.Function; public class AprilTagCameraConstants { - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2.5; + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; public static final double MAXIMUM_AMBIGUITY = 0.5; public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSimulation"); - private static final int SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600, SIMULATION_CAMERA_RESOLUTION_HEIGHT = 1200, @@ -51,14 +50,14 @@ private static void configureSimulationCameraProperties() { } public enum AprilTagCameraType { - PHOTON_CAMERA((name, properties) -> new AprilTagPhotonCameraIO(name)), + PHOTON_CAMERA(AprilTagPhotonCameraIO::new), SIMULATION_CAMERA(AprilTagSimulationCameraIO::new), - LIMELIGHT((name, properties) -> new AprilTagLimelightIO(name)); + LIMELIGHT(AprilTagLimelightIO::new); - final BiFunction createIOBiFunction; + final Function createIOFunction; - AprilTagCameraType(BiFunction createIOBiFunction) { - this.createIOBiFunction = createIOBiFunction; + AprilTagCameraType(Function createIOFunction) { + this.createIOFunction = createIOFunction; } } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index d5d5e4aa..5ad54f5b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -8,6 +8,11 @@ public class AprilTagCameraIO { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { } + /** + * Adds the simulated camera to the pose estimation simulation. + * + * @param robotToCamera the transform of the robot's origin point to the camera + */ protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) { } @@ -20,5 +25,6 @@ public static class AprilTagCameraInputs { public double bestTargetRelativeYawRadians = 0; public double bestTargetRelativePitchRadians = 0; public double distanceFromBestTag = 0; + public double poseAmbiguity = 0; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 47b79edc..e353d9a1 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,25 +1,19 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N3; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; -import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; import java.util.List; public class AprilTagPhotonCameraIO extends AprilTagCameraIO { - private final PhotonCamera photonCamera; + protected final PhotonCamera photonCamera; public AprilTagPhotonCameraIO(String cameraName) { photonCamera = new PhotonCamera(cameraName); @@ -29,7 +23,7 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = getLatestPipelineResult(); - inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; + inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); if (inputs.hasResult) { updateHasResultInputs(inputs, latestResult); return; @@ -42,19 +36,17 @@ private PhotonPipelineResult getLatestPipelineResult() { return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); } - private double getPoseAmbiguity(PhotonPipelineResult result) { - return getBestTarget(result).getPoseAmbiguity(); - } - private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { - final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + final PhotonTrackedTarget bestTarget = getBestTarget(latestResult); + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(bestTarget); - inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult, bestTarget); inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); - inputs.visibleTagIDs = getVisibleTagIDs(latestResult); - inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult, bestTarget); + inputs.distanceFromBestTag = getDistanceFromBestTag(bestTarget); + inputs.poseAmbiguity = bestTarget.getPoseAmbiguity(); } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { @@ -64,27 +56,28 @@ private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { } /** - * Estimates the camera's rotation relative to the apriltag. + * Calculates the best tag from the pipeline result based on the area that the tag takes up. * * @param result the camera's pipeline result - * @return the estimated rotation + * @return the best target */ - private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - final List tagCorners = getBestTarget(result).getDetectedCorners(); - final Point tagCenter = getTagCenter(tagCorners); - if (photonCamera.getCameraMatrix().isPresent()) - return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); - return null; + private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { + PhotonTrackedTarget bestTarget = result.getBestTarget(); + for (PhotonTrackedTarget target : result.getTargets()) { + if (target.getArea() > bestTarget.area) + bestTarget = target; + } + return bestTarget; } - private Point getTagCenter(List tagCorners) { - double tagCornerSumX = 0; - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) { - tagCornerSumX += tagCorner.x; - tagCornerSumY += tagCorner.y; - } - return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param bestTag the best apriltag visible to the camera + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) { + return bestTag.getBestCameraToTarget().getRotation(); } /** @@ -93,59 +86,34 @@ private Point getTagCenter(List tagCorners) { * @param result the camera's pipeline result * @return the estimated pose */ - private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + private Pose3d getSolvePNPPose(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) { if (result.getMultiTagResult().isPresent()) { final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(getBestTarget(result).getFiducialId()); - final Transform3d targetToCamera = getBestTarget(result).getBestCameraToTarget().inverse(); + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(bestTarget.getFiducialId()); + final Transform3d targetToCamera = bestTarget.getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } - private int[] getVisibleTagIDs(PhotonPipelineResult result) { - final int[] visibleTagIDs = new int[result.getTargets().size()]; - visibleTagIDs[0] = getBestTarget(result).getFiducialId(); - int idAddition = 1; + private int[] getVisibleTagIDs(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) { + final List targets = result.getTargets(); + final int[] visibleTagIDs = new int[targets.size()]; + boolean hasSeenBestTarget = false; + visibleTagIDs[0] = bestTarget.getFiducialId(); for (int i = 0; i < visibleTagIDs.length; i++) { - final int currentID = result.getTargets().get(i).getFiducialId(); - if (currentID == visibleTagIDs[0]) { - idAddition = 0; + final int targetID = targets.get(i).getFiducialId(); + if (targetID == visibleTagIDs[0]) { + hasSeenBestTarget = true; continue; } - visibleTagIDs[i + idAddition] = currentID; + visibleTagIDs[hasSeenBestTarget ? i : i + 1] = targetID; } return visibleTagIDs; } - private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { - PhotonTrackedTarget bestTarget = result.getBestTarget(); - for (PhotonTrackedTarget target : result.getTargets()) { - if (target.getArea() > bestTarget.area) - bestTarget = target; - } - return bestTarget; - } - - private double getDistanceFromBestTag(PhotonPipelineResult result) { - return getBestTarget(result).getBestCameraToTarget().getTranslation().getNorm(); - } - - private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { - double fx = camIntrinsics.get(0, 0); - double cx = camIntrinsics.get(0, 2); - double xOffset = cx - pixel.x; - - double fy = camIntrinsics.get(1, 1); - double cy = camIntrinsics.get(1, 2); - double yOffset = cy - pixel.y; - - // calculate yaw normally - var yaw = new Rotation2d(fx, xOffset); - // correct pitch based on yaw - var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); - - return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + private double getDistanceFromBestTag(PhotonTrackedTarget bestTag) { + return bestTag.getBestCameraToTarget().getTranslation().getNorm(); } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java index 8aaf6f55..8c7d31ac 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -1,162 +1,21 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N3; -import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; -import org.opencv.core.Point; -import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; -import java.util.List; - -public class AprilTagSimulationCameraIO extends AprilTagCameraIO { - private final PhotonCamera photonCamera; +public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { private final PhotonCameraSim cameraSim; - public AprilTagSimulationCameraIO(String cameraName, SimCameraProperties cameraProperties) { - photonCamera = new PhotonCamera(cameraName); + public AprilTagSimulationCameraIO(String cameraName) { + super(cameraName); - cameraSim = new PhotonCameraSim(photonCamera, cameraProperties); + cameraSim = new PhotonCameraSim(photonCamera, AprilTagCameraConstants.SIMULATION_CAMERA_PROPERTIES); cameraSim.enableDrawWireframe(true); } - @Override - protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { - final PhotonPipelineResult latestResult = getLatestPipelineResult(); - - inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getPoseAmbiguity(latestResult) < AprilTagCameraConstants.MAXIMUM_AMBIGUITY; - if (inputs.hasResult) { - updateHasResultInputs(inputs, latestResult); - return; - } - updateNoResultInputs(inputs); - } - @Override protected void addSimulatedCameraToVisionSimulation(Transform3d robotToCamera) { AprilTagCameraConstants.VISION_SIMULATION.addCamera(cameraSim, robotToCamera); } - - private PhotonPipelineResult getLatestPipelineResult() { - final List unreadResults = photonCamera.getAllUnreadResults(); - return unreadResults.isEmpty() ? null : unreadResults.get(unreadResults.size() - 1); - } - - private double getPoseAmbiguity(PhotonPipelineResult result) { - return getBestTarget(result).getPoseAmbiguity(); - } - - private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { - final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); - - inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); - inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); - inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); - inputs.visibleTagIDs = getVisibleTagIDs(latestResult); - inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); - } - - private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { - inputs.cameraSolvePNPPose = new Pose3d(); - inputs.visibleTagIDs = new int[0]; - inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; - } - - /** - * Estimates the camera's rotation relative to the apriltag. - * - * @param result the camera's pipeline result - * @return the estimated rotation - */ - private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { - final List tagCorners = getBestTarget(result).getDetectedCorners(); - final Point tagCenter = getTagCenter(tagCorners); - if (photonCamera.getCameraMatrix().isPresent()) - return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); - return null; - } - - private Point getTagCenter(List tagCorners) { - double tagCornerSumX = 0; - double tagCornerSumY = 0; - for (TargetCorner tagCorner : tagCorners) { - tagCornerSumX += tagCorner.x; - tagCornerSumY += tagCorner.y; - } - return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); - } - - /** - * Estimates the camera's pose using Solve PNP using as many tags as possible. - * - * @param result the camera's pipeline result - * @return the estimated pose - */ - private Pose3d getSolvePNPPose(PhotonPipelineResult result) { - if (result.getMultiTagResult().isPresent()) { - final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; - return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); - } - - final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(getBestTarget(result).getFiducialId()); - final Transform3d targetToCamera = getBestTarget(result).getBestCameraToTarget().inverse(); - return tagPose.transformBy(targetToCamera); - } - - private int[] getVisibleTagIDs(PhotonPipelineResult result) { - final int[] visibleTagIDs = new int[result.getTargets().size()]; - visibleTagIDs[0] = getBestTarget(result).getFiducialId(); - int idAddition = 1; - for (int i = 0; i < visibleTagIDs.length; i++) { - final int currentID = result.getTargets().get(i).getFiducialId(); - if (currentID == visibleTagIDs[0]) { - idAddition = 0; - continue; - } - visibleTagIDs[i + idAddition] = currentID; - } - return visibleTagIDs; - } - - private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { - PhotonTrackedTarget bestTarget = result.getBestTarget(); - for (PhotonTrackedTarget target : result.getTargets()) { - if (target.getArea() > bestTarget.area) - bestTarget = target; - } - return bestTarget; - } - - private double getDistanceFromBestTag(PhotonPipelineResult result) { - return getBestTarget(result).getBestCameraToTarget().getTranslation().getNorm(); - } - - private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { - double fx = camIntrinsics.get(0, 0); - double cx = camIntrinsics.get(0, 2); - double xOffset = cx - pixel.x; - - double fy = camIntrinsics.get(1, 1); - double cy = camIntrinsics.get(1, 2); - double yOffset = cy - pixel.y; - - // calculate yaw normally - var yaw = new Rotation2d(fx, xOffset); - // correct pitch based on yaw - var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); - - return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); - } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 2d8e8f24..71be5a0c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -34,11 +34,8 @@ public PoseEstimator(AprilTagCamera... aprilTagCameras) { this.aprilTagCameras = aprilTagCameras; putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); - PathPlannerLogging.setLogActivePathCallback((pose) -> { - field.getObject("path").setPoses(pose); - Logger.recordOutput("Path", pose.toArray(new Pose2d[0])); - }); - PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("TargetPPPose", pose)); + + logTargetPath(); } @Override @@ -92,6 +89,14 @@ public void setGyroHeadingToBestSolvePNPHeading() { resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); } + private void logTargetPath() { + PathPlannerLogging.setLogActivePathCallback((pose) -> { + field.getObject("path").setPoses(pose); + Logger.recordOutput("Path", pose.toArray(new Pose2d[0])); + }); + PathPlannerLogging.setLogTargetPoseCallback((pose) -> Logger.recordOutput("TargetPPPose", pose)); + } + private void updateFromVision() { getViableVisionObservations().stream() .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp)) diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index 5f98ed52..67f41447 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -174,9 +174,6 @@ private void configureHardware(double offsetRotations) { } private void configureSignals() { - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); - driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); @@ -186,5 +183,8 @@ private void configureSignals() { steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); + + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } \ No newline at end of file From 5e3fd2da3624300878278f65a31e1b12a142c63f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 18 Nov 2024 14:15:06 +0200 Subject: [PATCH 255/261] Fixed camera rotation logic --- .../poseestimation/apriltagcamera/AprilTagCamera.java | 7 ++++--- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 1 + 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index ea29ae1b..efa93f1c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -133,7 +133,8 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading if (bestTagPose == null) return null; - setProperCameraRotation(); + if (robotCenterToCamera.getRotation().getX() != 0) + setProperCameraRotation(); final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); @@ -148,8 +149,8 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading private void setProperCameraRotation() { Translation2d targetRotationVector = new Translation2d(inputs.bestTargetRelativeYawRadians, inputs.bestTargetRelativePitchRadians); targetRotationVector.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); - inputs.bestTargetRelativeYawRadians = targetRotationVector.getY(); - inputs.bestTargetRelativePitchRadians = targetRotationVector.getX(); + inputs.bestTargetRelativeYawRadians = targetRotationVector.getX(); + inputs.bestTargetRelativePitchRadians = targetRotationVector.getY(); } private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index e353d9a1..d67b1986 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -101,6 +101,7 @@ private int[] getVisibleTagIDs(PhotonPipelineResult result, PhotonTrackedTarget final List targets = result.getTargets(); final int[] visibleTagIDs = new int[targets.size()]; boolean hasSeenBestTarget = false; + visibleTagIDs[0] = bestTarget.getFiducialId(); for (int i = 0; i < visibleTagIDs.length; i++) { final int targetID = targets.get(i).getFiducialId(); From 7137725ab8cd74ee6d9f9f7fc92a206a35befe5a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 18 Nov 2024 14:17:10 +0200 Subject: [PATCH 256/261] Renamed --- .../poseestimation/apriltagcamera/AprilTagCamera.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index efa93f1c..83714cd7 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -147,10 +147,10 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading * This method corrects the yaw and pitch based on the camera's mount position roll. */ private void setProperCameraRotation() { - Translation2d targetRotationVector = new Translation2d(inputs.bestTargetRelativeYawRadians, inputs.bestTargetRelativePitchRadians); - targetRotationVector.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); - inputs.bestTargetRelativeYawRadians = targetRotationVector.getX(); - inputs.bestTargetRelativePitchRadians = targetRotationVector.getY(); + Translation2d targetRotation = new Translation2d(inputs.bestTargetRelativeYawRadians, inputs.bestTargetRelativePitchRadians); + targetRotation.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); + inputs.bestTargetRelativeYawRadians = targetRotation.getX(); + inputs.bestTargetRelativePitchRadians = targetRotation.getY(); } private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { From 38685d5f6440575f8407a4c9a93df3f8b36906d8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 19 Nov 2024 12:02:32 +0200 Subject: [PATCH 257/261] Cleaned and almost fixed --- .../apriltagcamera/AprilTagCamera.java | 12 ++++++------ .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 7 ++++--- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 83714cd7..08c7257c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -57,6 +57,7 @@ public void update() { aprilTagCameraIO.updateInputs(inputs); robotPose = calculateBestRobotPose(); + logCameraInfo(); if (RobotHardwareStats.isSimulation()) AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); @@ -133,8 +134,7 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading if (bestTagPose == null) return null; - if (robotCenterToCamera.getRotation().getX() != 0) - setProperCameraRotation(); + setProperCameraRotation(); final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); @@ -143,14 +143,14 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading } /** - * When the roll of the camera changes, the target pitch and yaw are also effected. + * When the roll of the camera changes, the target pitch and yaw are also affected. * This method corrects the yaw and pitch based on the camera's mount position roll. */ private void setProperCameraRotation() { - Translation2d targetRotation = new Translation2d(inputs.bestTargetRelativeYawRadians, inputs.bestTargetRelativePitchRadians); + final Translation2d targetRotation = new Translation2d(inputs.bestTargetRelativePitchRadians, inputs.bestTargetRelativeYawRadians); targetRotation.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); - inputs.bestTargetRelativeYawRadians = targetRotation.getX(); - inputs.bestTargetRelativePitchRadians = targetRotation.getY(); + inputs.bestTargetRelativePitchRadians = targetRotation.getX(); + inputs.bestTargetRelativeYawRadians = targetRotation.getY(); } private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index d67b1986..f3e11049 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -77,7 +77,8 @@ private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { * @return the estimated rotation */ private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) { - return bestTag.getBestCameraToTarget().getRotation(); + final Rotation3d cameraRotation = bestTag.getBestCameraToTarget().getRotation(); + return new Rotation3d(0, cameraRotation.getX(), -cameraRotation.getY()); } /** @@ -100,9 +101,9 @@ private Pose3d getSolvePNPPose(PhotonPipelineResult result, PhotonTrackedTarget private int[] getVisibleTagIDs(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) { final List targets = result.getTargets(); final int[] visibleTagIDs = new int[targets.size()]; - boolean hasSeenBestTarget = false; - visibleTagIDs[0] = bestTarget.getFiducialId(); + + boolean hasSeenBestTarget = false; for (int i = 0; i < visibleTagIDs.length; i++) { final int targetID = targets.get(i).getFiducialId(); if (targetID == visibleTagIDs[0]) { From 28b5981eb5cd255216359cafd23eb1f09bda99ec Mon Sep 17 00:00:00 2001 From: Strflightmight09 <> Date: Thu, 21 Nov 2024 22:37:20 +0200 Subject: [PATCH 258/261] Changed sim logic (still not working) --- .../robot/commands/AlignToNoteCommand.java | 2 +- .../robot/commands/CommandConstants.java | 2 +- .../robot/commands/LEDAutoSetupCommand.java | 2 +- .../commands/VisualizeNoteShootingCommand.java | 2 +- .../robot/commands/factories/AmpCommands.java | 2 +- .../commands/factories/AutonomousCommands.java | 2 +- .../robot/misc/ShootingCalculations.java | 4 ++-- .../SimulationObjectDetectionCameraIO.java | 6 +++--- .../apriltagcamera/AprilTagCamera.java | 4 ++-- .../AprilTagCameraConstants.java | 10 +++++----- .../io/AprilTagPhotonCameraIO.java | 2 +- .../poseestimator/PoseEstimator.java | 14 +++++++++++--- .../trigon/robot/subsystems/swerve/Swerve.java | 18 +++++++++--------- .../subsystems/swerve/SwerveCommands.java | 4 ++-- 14 files changed, 41 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index e8f3bcaf..1d00b378 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -70,7 +70,7 @@ private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPo } private MirrorableRotation2d getTargetAngle() { - final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); } diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 2a1128db..8cf855ea 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -45,7 +45,7 @@ public class CommandConstants { () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), - RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), false), new Rotation2d()).get())), + RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new MirrorablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), false), new Rotation2d()).get())), SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), diff --git a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java index b5e3b8ad..b71e0f1e 100644 --- a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java +++ b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java @@ -75,6 +75,6 @@ else if (difference > tolerance) } private Pose2d getCurrentRobotPose() { - return RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + return RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java index 41f4810f..8b1e47b9 100644 --- a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java +++ b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java @@ -71,7 +71,7 @@ private double calculateNoteZDifference(double t) { private void configureStartingStats() { startingTimeSeconds = Timer.getFPGATimestamp(); - final Pose2d currentRobotPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d currentRobotPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); final Rotation2d currentRobotAngle = currentRobotPose.getRotation(); final double startingTangentialVelocity = getStartingTangentialVelocity(); diff --git a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java index 32356fa7..561723d8 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AmpCommands.java @@ -48,7 +48,7 @@ private static Command getAutonomousPrepareForAmpCommand() { GeneralCommands.runWhen( AmpAlignerCommands.getSetTargetStateCommand(AmpAlignerConstants.AmpAlignerState.OPEN) .alongWith(PitcherCommands.getSetTargetPitchCommand(ShootingConstants.PREPARE_AMP_PITCH)), - () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation().getDistance( + () -> RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation().getDistance( FieldConstants.IN_FRONT_OF_AMP_POSE.get().getTranslation()) < FieldConstants.MAXIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS ), ShooterCommands.getSetTargetVelocityCommand(ShootingConstants.AMP_SHOOTING_VELOCITY_ROTATIONS_PER_SECOND) diff --git a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java index 2086abd6..195262a7 100644 --- a/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/factories/AutonomousCommands.java @@ -104,7 +104,7 @@ private static Optional calculateRotationOverride() { if (RobotContainer.INTAKE.hasNote() || !NOTE_DETECTION_CAMERA.hasTargets()) return Optional.empty(); - final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); final Rotation2d targetRotation = NOTE_DETECTION_CAMERA.getTrackedObjectYaw().minus(currentRotation); return Optional.of(targetRotation); } diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index 48fb0ef6..3906b0b9 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -86,7 +86,7 @@ public Translation3d getRobotFieldRelativeVelocity() { */ private Translation2d predictFutureTranslation(double predictionTime) { final Translation2d fieldRelativeVelocity = getRobotFieldRelativeVelocity().toTranslation2d(); - final Translation2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); + final Translation2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation(); return currentPose.plus(fieldRelativeVelocity.times(predictionTime)); } @@ -101,7 +101,7 @@ private Translation2d predictFutureTranslation(double predictionTime) { * @return the target state of the robot so the note will reach the shooting target, as a {@linkplain ShootingCalculations.TargetShootingState} */ private TargetShootingState calculateTargetShootingState(MirrorableTranslation3d shootingTarget, double standingShootingVelocityRotationsPerSecond, boolean reachFromAbove) { - final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); + final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getTranslation(); final MirrorableRotation2d standingTargetRobotAngle = getAngleToTarget(currentTranslation, shootingTarget); final double standingTangentialVelocity = angularVelocityToTangentialVelocity(standingShootingVelocityRotationsPerSecond); final Rotation2d standingTargetPitch = calculateTargetPitch(standingTangentialVelocity, reachFromAbove, currentTranslation, standingTargetRobotAngle, shootingTarget); diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java index 8e943d1f..0be42fa9 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/SimulationObjectDetectionCameraIO.java @@ -43,7 +43,7 @@ protected SimulationObjectDetectionCameraIO(String hostname) { @Override protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - final Rotation2d closestObjectYaw = getClosestVisibleObjectYaw(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); + final Rotation2d closestObjectYaw = getClosestVisibleObjectYaw(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose()); if (closestObjectYaw == null) { inputs.hasTargets = false; } else { @@ -63,7 +63,7 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { private void updateHeldObjectPose() { if (heldObject.length == 0) return; - heldObject[0] = getHeldObjectPose(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); + heldObject[0] = getHeldObjectPose(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose()); } private void updateObjectEjection() { @@ -79,7 +79,7 @@ private void updateObjectEjection() { private void updateObjectCollection() { if (heldObject.length == 1 || !isCollecting()) return; - final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); final Translation2d robotTranslation = robotPose.getTranslation(); for (Translation2d objectPlacement : objectsOnField) { if (objectPlacement.getDistance(robotTranslation) <= PICKING_UP_TOLERANCE_METERS) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 08c7257c..8dff8f0e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -60,7 +60,7 @@ public void update() { logCameraInfo(); if (RobotHardwareStats.isSimulation()) - AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); + AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose()); } public boolean hasNewResult() { @@ -109,7 +109,7 @@ public double getDistanceToBestTagMeters() { * @return the robot's pose */ private Pose2d calculateBestRobotPose() { - final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + final Rotation2d gyroHeadingAtTimestamp = RobotHardwareStats.isSimulation() ? RobotContainer.SWERVE.getHeading() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 651dcc32..674b0b05 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -24,13 +24,13 @@ public class AprilTagCameraConstants { SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600, SIMULATION_CAMERA_RESOLUTION_HEIGHT = 1200, SIMULATION_CAMERA_FPS = 60, - SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, - SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5, - SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 10; + SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 0,//35, + SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 0,//5, + SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 0;//10; private static final Rotation2d SIMULATION_CAMERA_FOV = Rotation2d.fromDegrees(70); private static final double - SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0.25, - SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0.08; + SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0,//0.25, + SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0;//0.08; public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); static { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index f3e11049..29376753 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -78,7 +78,7 @@ private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { */ private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) { final Rotation3d cameraRotation = bestTag.getBestCameraToTarget().getRotation(); - return new Rotation3d(0, cameraRotation.getX(), -cameraRotation.getY()); + return new Rotation3d(0, -cameraRotation.getY(), -cameraRotation.getZ()); } /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 71be5a0c..34e0781f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -45,7 +45,7 @@ public void close() { public void periodic() { updateFromVision(); - field.setRobotPose(getCurrentPose()); + field.setRobotPose(getCurrentEstimatedPose()); } /** @@ -61,10 +61,18 @@ public void resetPose(Pose2d currentPose) { /** * @return the estimated pose of the robot, relative to the blue alliance's driver station right corner */ - public Pose2d getCurrentPose() { + public Pose2d getCurrentEstimatedPose() { return poseEstimator6328.getEstimatedPose(); } + /** + * @return the odometry's estimated pose of the robot, relative to the blue alliance's driver station right corner + */ + + public Pose2d getCurrentOdometryPose() { + return poseEstimator6328.getOdometryPose(); + } + /** * Updates the pose estimator with the given SWERVE wheel positions and gyro rotations. * This function accepts an array of SWERVE wheel positions and an array of gyro rotations because the odometry can be updated at a faster rate than the main loop (which is 50 hertz). @@ -86,7 +94,7 @@ public void setGyroHeadingToBestSolvePNPHeading() { } final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); - resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + resetPose(new Pose2d(getCurrentEstimatedPose().getTranslation(), bestRobotHeading)); } private void logTargetPath() { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 5b13b9c8..6e42773a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -95,7 +95,7 @@ public ChassisSpeeds getSelfRelativeVelocity() { } public ChassisSpeeds getFieldRelativeVelocity() { - return ChassisSpeeds.fromRobotRelativeSpeeds(getSelfRelativeVelocity(), RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation()); + return ChassisSpeeds.fromRobotRelativeSpeeds(getSelfRelativeVelocity(), RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation()); } /** @@ -111,16 +111,16 @@ public boolean atPose(MirrorablePose2d pose2d) { public boolean atXAxisPosition(double xAxisPosition) { final double currentXAxisVelocity = getFieldRelativeVelocity().vxMetersPerSecond; - return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getX(), xAxisPosition, currentXAxisVelocity); + return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getX(), xAxisPosition, currentXAxisVelocity); } public boolean atYAxisPosition(double yAxisPosition) { final double currentYAxisVelocity = getFieldRelativeVelocity().vyMetersPerSecond; - return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getY(), yAxisPosition, currentYAxisVelocity); + return atTranslationPosition(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getY(), yAxisPosition, currentYAxisVelocity); } public boolean atAngle(MirrorableRotation2d angle) { - final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES; + final boolean atTargetAngle = Math.abs(angle.get().minus(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation()).getDegrees()) < SwerveConstants.ROTATION_TOLERANCE_DEGREES; final boolean isAngleStill = Math.abs(getSelfRelativeVelocity().omegaRadiansPerSecond) < SwerveConstants.ROTATION_VELOCITY_TOLERANCE; Logger.recordOutput("Swerve/AtTargetAngle/isStill", isAngleStill); Logger.recordOutput("Swerve/AtTargetAngle/atTargetAngle", atTargetAngle); @@ -135,7 +135,7 @@ public double[] getDriveWheelPositionsRadians() { } public Rotation2d getDriveRelativeAngle() { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; } @@ -163,7 +163,7 @@ void lockSwerve() { * @param targetPose the target pose, relative to the blue alliance driver station's right corner */ void pidToPose(MirrorablePose2d targetPose) { - final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); final Pose2d mirroredTargetPose = targetPose.get(); final double xSpeed = SwerveConstants.TRANSLATION_PID_CONTROLLER.calculate(currentPose.getX(), mirroredTargetPose.getX()); final double ySpeed = SwerveConstants.TRANSLATION_PID_CONTROLLER.calculate(currentPose.getY(), mirroredTargetPose.getY()); @@ -182,7 +182,7 @@ void initializeDrive(boolean closedLoop) { } void resetRotationController() { - SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.reset(RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getDegrees()); + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.reset(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation().getDegrees()); } void setClosedLoop(boolean closedLoop) { @@ -303,7 +303,7 @@ private void updateHardware() { private void configurePathPlanner() { AutoBuilder.configureHolonomic( - RobotContainer.POSE_ESTIMATOR::getCurrentPose, + RobotContainer.POSE_ESTIMATOR::getCurrentEstimatedPose, (pose) -> { }, this::getSelfRelativeVelocity, @@ -321,7 +321,7 @@ private boolean atTranslationPosition(double currentTranslationPosition, double } private double calculateProfiledAngleSpeedToTargetAngle(MirrorableRotation2d targetAngle) { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose().getRotation(); return Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees(), targetAngle.get().getDegrees())); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 95ecaf8e..30b588c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -149,7 +149,7 @@ private static Command getCurrentDriveToPoseCommand(MirrorablePose2d targetPose, private static Command getPathfindToPoseCommand(MirrorablePose2d targetPose, PathConstraints pathConstraints) { final Pose2d targetMirroredPose = targetPose.get(); - final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentPose(); + final Pose2d currentPose = RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(); if (currentPose.getTranslation().getDistance(targetMirroredPose.getTranslation()) < 0.35) return createOnTheFlyPathCommand(targetPose, pathConstraints); return AutoBuilder.pathfindToPose(targetMirroredPose, pathConstraints); @@ -163,7 +163,7 @@ private static Command getPIDToPoseCommand(MirrorablePose2d targetPose) { private static Command createOnTheFlyPathCommand(MirrorablePose2d targetPose, PathConstraints constraints) { List bezierPoints = PathPlannerPath.bezierFromPoses( - RobotContainer.POSE_ESTIMATOR.getCurrentPose(), + RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), targetPose.get() ); From aa360002309bd9e97c11c8825690f4f67fe70754 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <> Date: Thu, 21 Nov 2024 23:01:25 +0200 Subject: [PATCH 259/261] It was the units... --- .../frc/trigon/robot/constants/CameraConstants.java | 3 ++- .../apriltagcamera/AprilTagCameraConstants.java | 12 ++++++------ .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 3 ++- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 2b944864..5532f946 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -7,10 +7,11 @@ import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import org.trigon.hardware.RobotHardwareStats; public class CameraConstants { public static final double - TRANSLATIONS_STD_EXPONENT = 0.02, + TRANSLATIONS_STD_EXPONENT = RobotHardwareStats.isSimulation() ? 0.2 : 0.002, THETA_STD_EXPONENT = 0.02; private static final Transform3d diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 674b0b05..8d9cd9d5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -12,7 +12,7 @@ import java.util.function.Function; -public class AprilTagCameraConstants { + public class AprilTagCameraConstants { static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; @@ -24,13 +24,13 @@ public class AprilTagCameraConstants { SIMULATION_CAMERA_RESOLUTION_WIDTH = 1600, SIMULATION_CAMERA_RESOLUTION_HEIGHT = 1200, SIMULATION_CAMERA_FPS = 60, - SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 0,//35, - SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 0,//5, - SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 0;//10; + SIMULATION_AVERAGE_CAMERA_LATENCY_MILLISECONDS = 35, + SIMULATION_CAMERA_LATENCY_STANDARD_DEVIATIONS_MILLISECONDS = 5, + SIMULATION_CAMERA_EXPOSURE_TIME_MILLISECONDS = 10; private static final Rotation2d SIMULATION_CAMERA_FOV = Rotation2d.fromDegrees(70); private static final double - SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0,//0.25, - SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0;//0.08; + SIMULATION_CAMERA_AVERAGE_PIXEL_ERROR = 0.25, + SIMULATION_CAMERA_PIXEL_STANDARD_DEVIATIONS = 0.08; public static final SimCameraProperties SIMULATION_CAMERA_PROPERTIES = new SimCameraProperties(); static { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 29376753..204b0f07 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; @@ -78,7 +79,7 @@ private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { */ private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) { final Rotation3d cameraRotation = bestTag.getBestCameraToTarget().getRotation(); - return new Rotation3d(0, -cameraRotation.getY(), -cameraRotation.getZ()); + return new Rotation3d(0, Units.degreesToRadians(cameraRotation.getY()), Units.degreesToRadians(cameraRotation.getZ())); } /** From 7a52176fb52c9d35feb302f6a9855c5e4c4862df Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 23 Nov 2024 20:06:01 +0200 Subject: [PATCH 260/261] Minor cleaning stuff --- .../poseestimation/apriltagcamera/AprilTagCameraConstants.java | 2 +- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 2 +- .../robot/poseestimation/poseestimator/PoseEstimator.java | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 8d9cd9d5..651dcc32 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -12,7 +12,7 @@ import java.util.function.Function; - public class AprilTagCameraConstants { +public class AprilTagCameraConstants { static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2; static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 204b0f07..ea9a39f6 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -14,7 +14,7 @@ import java.util.List; public class AprilTagPhotonCameraIO extends AprilTagCameraIO { - protected final PhotonCamera photonCamera; + final PhotonCamera photonCamera; public AprilTagPhotonCameraIO(String cameraName) { photonCamera = new PhotonCamera(cameraName); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 34e0781f..2b5d83c7 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -68,7 +68,6 @@ public Pose2d getCurrentEstimatedPose() { /** * @return the odometry's estimated pose of the robot, relative to the blue alliance's driver station right corner */ - public Pose2d getCurrentOdometryPose() { return poseEstimator6328.getOdometryPose(); } From b9e96590f1ae8e97029ae1d9366950d59df0c62c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 25 Nov 2024 13:09:38 +0200 Subject: [PATCH 261/261] Logic fixes + cleaning for sim cams --- .../trigon/robot/commands/LEDAutoSetupCommand.java | 2 +- .../apriltagcamera/AprilTagCamera.java | 13 +++++-------- .../apriltagcamera/AprilTagCameraConstants.java | 2 +- .../poseestimation/poseestimator/PoseEstimator.java | 4 ++++ .../robot/subsystems/swerve/SwerveModule.java | 7 ++++--- 5 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java index b71e0f1e..a47124b2 100644 --- a/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java +++ b/src/main/java/frc/trigon/robot/commands/LEDAutoSetupCommand.java @@ -23,7 +23,7 @@ public class LEDAutoSetupCommand extends SequentialCommandGroup { TOLERANCE_METERS = 0.1, TOLERANCE_DEGREES = 2; private static final int AMOUNT_OF_SECTIONS = 3; - private final Supplier[] LEDColors = new Supplier[] { + private final Supplier[] LEDColors = new Supplier[]{ () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getRotation().getDegrees() - getCurrentRobotPose().getRotation().getDegrees(), TOLERANCE_DEGREES), () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getX() - getCurrentRobotPose().getX(), TOLERANCE_METERS), () -> getDesiredLEDColorFromRobotPose(this.autoStartPose.getY() - getCurrentRobotPose().getY(), TOLERANCE_METERS) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 8dff8f0e..ad9ea89d 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -57,14 +57,11 @@ public void update() { aprilTagCameraIO.updateInputs(inputs); robotPose = calculateBestRobotPose(); - logCameraInfo(); - if (RobotHardwareStats.isSimulation()) - AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose()); } public boolean hasNewResult() { - return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp(); + return (inputs.hasResult && inputs.distanceFromBestTag != Double.POSITIVE_INFINITY) && isNewTimestamp(); } public Pose2d getEstimatedRobotPose() { @@ -109,7 +106,7 @@ public double getDistanceToBestTagMeters() { * @return the robot's pose */ private Pose2d calculateBestRobotPose() { - final Rotation2d gyroHeadingAtTimestamp = RobotHardwareStats.isSimulation() ? RobotContainer.SWERVE.getHeading() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + final Rotation2d gyroHeadingAtTimestamp = RobotHardwareStats.isSimulation() ? RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose().getRotation() : PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp); } @@ -134,7 +131,7 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading if (bestTagPose == null) return null; - setProperCameraRotation(); + correctTargetRelativeRotation(); final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose); final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); @@ -146,7 +143,7 @@ private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading * When the roll of the camera changes, the target pitch and yaw are also affected. * This method corrects the yaw and pitch based on the camera's mount position roll. */ - private void setProperCameraRotation() { + private void correctTargetRelativeRotation() { final Translation2d targetRotation = new Translation2d(inputs.bestTargetRelativePitchRadians, inputs.bestTargetRelativeYawRadians); targetRotation.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); inputs.bestTargetRelativePitchRadians = targetRotation.getX(); @@ -214,7 +211,7 @@ private void logCameraInfo() { Logger.processInputs("Cameras/" + name, inputs); if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) logUsedTags(); - if (!inputs.hasResult || inputs.distanceFromBestTag == 0 || robotPose == null) { + if (!inputs.hasResult || inputs.distanceFromBestTag == Double.POSITIVE_INFINITY || robotPose == null) { logEstimatedRobotPose(); logSolvePNPPose(); } else { diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 651dcc32..960a6ee0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -17,7 +17,7 @@ public class AprilTagCameraConstants { static final int CALCULATE_YAW_ITERATIONS = 3; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; - public static final double MAXIMUM_AMBIGUITY = 0.5; + public static final double MAXIMUM_AMBIGUITY = 0.4; public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSimulation"); private static final int diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 2b5d83c7..23481254 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -10,7 +10,9 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; import java.util.ArrayList; import java.util.Comparator; @@ -108,6 +110,8 @@ private void updateFromVision() { getViableVisionObservations().stream() .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp)) .forEach(poseEstimator6328::addVisionObservation); + if (RobotHardwareStats.isSimulation()) + AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose()); } private List getViableVisionObservations() { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index 67f41447..b02b4fe9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -178,13 +178,14 @@ private void configureSignals() { driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); - - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } \ No newline at end of file