From 6f0a9e1c074b1050dd2d8f45c35c6c75caf34aed Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Tue, 16 Apr 2024 16:06:57 -0500 Subject: [PATCH 01/37] set I to zero if error is too large, otherwise set I to tuned value --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 70ca4c4f..e339d9a2 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -226,6 +226,13 @@ private void updateMotors(){ isRevingR = updateIsReving(isRevingR, targetVelocityR, shooterR.getVelocity().getValueAsDouble()); updateMotor(shooterL, isRevingL, targetVelocityL); updateMotor(shooterR, isRevingR, targetVelocityR); + if(shooterL.getClosedLoopError().getValueAsDouble()>7) { + configuratorL.apply(PIDLeftconfigs.withKI(0)); + configuratorR.apply(PIDRightconfigs.withKI(0)); + } else { + configuratorL.apply(PIDLeftconfigs.withKI(0.004)); + configuratorR.apply(PIDRightconfigs.withKI(0.004)); + } } public double getRSpeed() { return shooterR.getVelocity().getValueAsDouble(); From e3288a39c7169fd17edaa6354cfa1e689742bbbf Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Thu, 11 Apr 2024 20:18:05 -0500 Subject: [PATCH 02/37] changed path of 3-note close --- .../autos/(AP)StartAmpScore3FarAmp.auto | 2 +- .../autos/(AP)StartMidScore3Close.auto | 14 ++-- .../paths/CloseSourcePickupFromMidAuto.path | 70 +++++++++++++++++++ 3 files changed, 81 insertions(+), 5 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index ba96f6ea..372dacea 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -5,7 +5,7 @@ "x": 0.6842044526319189, "y": 6.690138362483131 }, - "rotation": -116.0 + "rotation": -125.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto index 74797177..636b29d0 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto @@ -14,13 +14,19 @@ { "type": "named", "data": { - "name": "initialShot" + "name": "shooterOn" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseAmpPickupAuto" } }, { "type": "named", "data": { - "name": "shooterOn" + "name": "autoShootNote" } }, { @@ -38,7 +44,7 @@ { "type": "path", "data": { - "pathName": "CloseAmpPickupFromMidAuto" + "pathName": " CloseMidPickupFromAmpAuto" } }, { @@ -62,7 +68,7 @@ { "type": "path", "data": { - "pathName": "CloseSourcePickupFromAmpAuto" + "pathName": "CloseSourcePickupFromMidAuto" } }, { diff --git a/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path b/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path new file mode 100644 index 00000000..669d2ffe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.183886123411755, + "y": 4.859358141022456 + }, + "prevControl": null, + "nextControl": { + "x": 2.1281941601759766, + "y": 4.645872281951973 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9956286826153902, + "y": 4.39239001934794 + }, + "prevControl": { + "x": 1.9406557155265987, + "y": 4.377730561457596 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.35, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 500.0 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 2.3, + "maxAngularVelocity": 650.0, + "maxAngularAcceleration": 350.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 176.90685335882964, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 176.9071636922732, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From cf57a4702a6a78185e6a36f7856c1495f48dd9e7 Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Fri, 12 Apr 2024 09:12:33 -0500 Subject: [PATCH 03/37] integrated MegaTag2 - untested --- src/main/java/frc/robot/LimelightHelpers.java | 270 +++++++++++++++++- .../java/frc/robot/settings/Constants.java | 2 +- .../robot/subsystems/DrivetrainSubsystem.java | 19 +- .../java/frc/robot/subsystems/Limelight.java | 18 +- 4 files changed, 287 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index e519c58b..cc6258be 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.3.0 (Feb 24, 2024) +//LimelightHelpers v1.6 (April 9, 2024) package frc.robot; @@ -385,6 +385,62 @@ public LimelightResults() { } + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -393,9 +449,12 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; + public RawFiducial[] rawFiducials; + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea) { this.pose = pose; this.timestampSeconds = timestampSeconds; this.latency = latency; @@ -403,6 +462,7 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int ta this.tagSpan = tagSpan; this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -443,7 +503,7 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } - private static double extractBotPoseEntry(double[] inData, int position){ + private static double extractArrayEntry(double[] inData, int position){ if(inData.length < position+1) { return 0; @@ -455,14 +515,131 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); var poseArray = poseEntry.getDoubleArray(new double[0]); var pose = toPose2D(poseArray); - double latency = extractBotPoseEntry(poseArray,6); - int tagCount = (int)extractBotPoseEntry(poseArray,7); - double tagSpan = extractBotPoseEntry(poseArray,8); - double tagDist = extractBotPoseEntry(poseArray,9); - double tagArea = extractBotPoseEntry(poseArray,10); + double latency = extractArrayEntry(poseArray,6); + int tagCount = (int)extractArrayEntry(poseArray,7); + double tagSpan = extractArrayEntry(poseArray,8); + double tagDist = extractArrayEntry(poseArray,9); + double tagArea = extractArrayEntry(poseArray,10); //getlastchange() in microseconds, ll latency in milliseconds var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); - return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 11; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } } public static NetworkTable getLimelightNTTable(String tableName) { @@ -604,7 +781,11 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static double getNeuralClassIDDouble(String limelightName) { return getLimelightNTDouble(limelightName, "tclass"); } @@ -675,6 +856,17 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -699,6 +891,16 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -782,6 +984,54 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 6762db47..78ce73fe 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -503,7 +503,7 @@ public final class Field{ public final class Vision{ public static final String APRILTAG_LIMELIGHT2_NAME = "limelight-aprill"; public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-aprilr"; - public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural"; + public static final String OBJ_DETECTION_LIMELIGHT_NAME = "limelight-neural"; public static final String LIMELIGHT_SHUFFLEBOARD_TAB = "Vision"; diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index ace2eebe..3859a1cc 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -20,6 +20,9 @@ import static frc.robot.settings.Constants.DriveConstants.FR_STEER_ENCODER_ID; import static frc.robot.settings.Constants.DriveConstants.FR_STEER_MOTOR_ID; import static frc.robot.settings.Constants.ShooterConstants.OFFSET_MULTIPLIER; +import static frc.robot.settings.Constants.Vision.APRILTAG_LIMELIGHT2_NAME; +import static frc.robot.settings.Constants.Vision.APRILTAG_LIMELIGHT3_NAME; + import java.util.Arrays; import java.util.Collections; import java.util.Optional; @@ -47,6 +50,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.LimelightHelpers; import frc.robot.LimelightHelpers.PoseEstimate; import frc.robot.commands.AngleShooter; import frc.robot.commands.RotateRobot; @@ -163,7 +167,7 @@ public DrivetrainSubsystem() { getGyroscopeRotation(), getModulePositions(), DRIVE_ODOMETRY_ORIGIN); - odometer.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 100)); + odometer.setVisionMeasurementStdDevs(VecBuilder.fill(0.9, 0.9, 99999999)); } /** * Sets the gyroscope angle to zero. This can be used to set the direction the robot is currently facing to the @@ -274,7 +278,18 @@ public void updateOdometry() { public void updateOdometryWithVision() { PoseEstimate estimate = limelight.getTrustedPose(getPose()); if (estimate != null) { - odometer.addVisionMeasurement(new Pose2d(estimate.pose.getTranslation(), getOdometryRotation()), estimate.timestampSeconds); + LimelightHelpers.SetRobotOrientation(APRILTAG_LIMELIGHT2_NAME, odometer.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(APRILTAG_LIMELIGHT3_NAME, odometer.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + boolean doRejectUpdate = false; + if(Math.abs(pigeon.getRate()) > 720) { + doRejectUpdate = true; + } + if(estimate.tagCount == 0) { + doRejectUpdate = true; + } + if(!doRejectUpdate) { + odometer.addVisionMeasurement(estimate.pose, estimate.timestampSeconds); + } RobotState.getInstance().LimelightsUpdated = true; } else { RobotState.getInstance().LimelightsUpdated = false; diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index fb83624d..8d4e7f3f 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -56,8 +56,8 @@ public static void useDetectorLimelight(boolean enabled) { * prioritized by lowest tagDistance. */ public PoseEstimate getTrustedPose(Pose2d odometryPose) { - PoseEstimate pose1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(APRILTAG_LIMELIGHT2_NAME); - PoseEstimate pose2 = LimelightHelpers.getBotPoseEstimate_wpiBlue(APRILTAG_LIMELIGHT3_NAME); + PoseEstimate pose1 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT2_NAME); + PoseEstimate pose2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT3_NAME); Boolean pose1Trust = isTrustworthy(APRILTAG_LIMELIGHT2_NAME, pose1, odometryPose); Boolean pose2Trust = isTrustworthy(APRILTAG_LIMELIGHT3_NAME, pose2, odometryPose); @@ -83,8 +83,8 @@ public PoseEstimate getTrustedPose(Pose2d odometryPose) { * are prioritized by lowest tagDistance. */ public PoseEstimate getValidPose() { - PoseEstimate pose1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(APRILTAG_LIMELIGHT2_NAME); - PoseEstimate pose2 = LimelightHelpers.getBotPoseEstimate_wpiBlue(APRILTAG_LIMELIGHT3_NAME); + PoseEstimate pose1 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT2_NAME); + PoseEstimate pose2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT3_NAME); Boolean pose1Valid = isValid(APRILTAG_LIMELIGHT2_NAME, pose1); Boolean pose2Valid = isValid(APRILTAG_LIMELIGHT3_NAME, pose2); @@ -101,11 +101,11 @@ public PoseEstimate getValidPose() { public LimelightDetectorData getNeuralDetectorValues() { return new LimelightDetectorData( - LimelightHelpers.getTX(OBJ_DETECITON_LIMELIGHT_NAME), - LimelightHelpers.getTY(OBJ_DETECITON_LIMELIGHT_NAME), - LimelightHelpers.getTA(OBJ_DETECITON_LIMELIGHT_NAME), - LimelightHelpers.getNeuralClassID(OBJ_DETECITON_LIMELIGHT_NAME), - LimelightHelpers.getTV(OBJ_DETECITON_LIMELIGHT_NAME)); + LimelightHelpers.getTX(OBJ_DETECTION_LIMELIGHT_NAME), + LimelightHelpers.getTY(OBJ_DETECTION_LIMELIGHT_NAME), + LimelightHelpers.getTA(OBJ_DETECTION_LIMELIGHT_NAME), + LimelightHelpers.getNeuralClassIDDouble(OBJ_DETECTION_LIMELIGHT_NAME), + LimelightHelpers.getTV(OBJ_DETECTION_LIMELIGHT_NAME)); } private boolean isValid(String limelightName, PoseEstimate estimate) { From c1504ca13bd76634e78cd76e03243da5f206ec9e Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Fri, 12 Apr 2024 09:28:00 -0500 Subject: [PATCH 04/37] used provide dmethod for getting the neraul class ID --- src/main/java/frc/robot/settings/LimelightDetectorData.java | 4 ++-- src/main/java/frc/robot/subsystems/Limelight.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/settings/LimelightDetectorData.java b/src/main/java/frc/robot/settings/LimelightDetectorData.java index 70dc91ea..57c72b81 100644 --- a/src/main/java/frc/robot/settings/LimelightDetectorData.java +++ b/src/main/java/frc/robot/settings/LimelightDetectorData.java @@ -19,9 +19,9 @@ public class LimelightDetectorData { /**Target Area (0% of image to 100% of image) */ public double ta; - public double classID; + public String classID; - public LimelightDetectorData(double tx, double ty, double ta, double classID, boolean valid) { + public LimelightDetectorData(double tx, double ty, double ta, String classID, boolean valid) { this.tx = tx; this.ty = ty; this.ta = ta; diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 8d4e7f3f..13d77a6b 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -104,7 +104,7 @@ public LimelightDetectorData getNeuralDetectorValues() { LimelightHelpers.getTX(OBJ_DETECTION_LIMELIGHT_NAME), LimelightHelpers.getTY(OBJ_DETECTION_LIMELIGHT_NAME), LimelightHelpers.getTA(OBJ_DETECTION_LIMELIGHT_NAME), - LimelightHelpers.getNeuralClassIDDouble(OBJ_DETECTION_LIMELIGHT_NAME), + LimelightHelpers.getNeuralClassID(OBJ_DETECTION_LIMELIGHT_NAME), LimelightHelpers.getTV(OBJ_DETECTION_LIMELIGHT_NAME)); } From d5b0b0cd0c023ba850e2a315f03206926abb99ee Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Fri, 12 Apr 2024 22:02:15 -0500 Subject: [PATCH 05/37] callibrated shooting for our field --- src/main/java/frc/robot/settings/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 78ce73fe..794f34bd 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -466,8 +466,8 @@ public final class Field{ public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263; - public static final double RED_SPEAKER_Y = 5.58; - public static final double SHOOTER_RED_SPEAKER_X = 16.45;//changed so that shots from the side wil aim to the opposite side, and bank in + public static final double RED_SPEAKER_Y = 5.613; + public static final double SHOOTER_RED_SPEAKER_X = 16.582;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in From 67ee22394e940fc9a2359ed2198aec815214b0e1 Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Fri, 12 Apr 2024 22:02:52 -0500 Subject: [PATCH 06/37] faster callibration and MT2 --- .../subsystems/AngleShooterSubsystem.java | 79 +++++++++++++++++ .../robot/subsystems/DrivetrainSubsystem.java | 86 ++++++++++++++++++- 2 files changed, 163 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index 2723a0f0..43396cf1 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -5,6 +5,7 @@ import pabeles.concurrency.IntOperatorTask.Max; import java.util.Optional; +import java.util.function.DoubleSupplier; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; @@ -39,6 +40,12 @@ public class AngleShooterSubsystem extends SubsystemBase { double speakerDistGlobal; public AngleShooterSubsystem() { + + SmartDashboard.putNumber("CALLIBRATION/redShooterX", Field.CALCULATED_SHOOTER_RED_SPEAKER_X); + SmartDashboard.putNumber("CALLIBRATION/blueShooterX", Field.CALCULATED_SHOOTER_BLUE_SPEAKER_X); + SmartDashboard.putNumber("CALLIBRATION/blueY", Field.CALCULATED_BLUE_SPEAKER_Y); + SmartDashboard.putNumber("CALLIBRATION/redY", Field.CALCULATED_RED_SPEAKER_Y); + runsValid = 0; pitchMotor = new CANSparkMax(PITCH_MOTOR_ID, MotorType.kBrushless); pitchMotor.restoreFactoryDefaults(); @@ -191,6 +198,76 @@ public double calculateSpeakerAngle() { SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle); + speakerDistGlobal = offsetSpeakerdist; + // double differenceAngle = (desiredShooterAngle - this.getShooterAngle()); + // SmartDashboard.putNumber("differenceAngleShooter", differenceAngle); + + return desiredShooterAngle; + } + + public double calculateSpeakerAngleWithSuppliers(DoubleSupplier redShooterXSup, DoubleSupplier blueShooterXSup, DoubleSupplier redYSup, DoubleSupplier blueYSup) { + double deltaX; + double deltaY; + double redX = redShooterXSup.getAsDouble(); + double blueX = blueShooterXSup.getAsDouble(); + double redY = redYSup.getAsDouble(); + double blueY = blueYSup.getAsDouble(); + shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; + // triangle for robot angle + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == Alliance.Red) { + deltaX = Math.abs(dtvalues.getX() - redX); + deltaY = Math.abs(dtvalues.getY() - redY); + } else { + deltaX = Math.abs(dtvalues.getX() - blueX); + deltaY = Math.abs(dtvalues.getY() - blueY); + } + double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2)); + // SmartDashboard.putNumber("dist to speaker", speakerDist); + Rotation2d unadjustedAngle = Rotation2d.fromRadians(Math.asin(deltaX/speakerDist)); + double totalDistToSpeaker = Math.sqrt(Math.pow(Field.SPEAKER_Z-ShooterConstants.SHOOTER_HEIGHT, 2) + Math.pow(speakerDist, 2)); + double shootingTime = totalDistToSpeaker / shootingSpeed; // calculates how long the note will take to reach the target + double currentXSpeed = DTChassisSpeeds.vxMetersPerSecond; + double currentYSpeed = DTChassisSpeeds.vyMetersPerSecond; + Translation2d targetOffset = new Translation2d(currentXSpeed * shootingTime*OFFSET_MULTIPLIER * unadjustedAngle.getRadians(), currentYSpeed * shootingTime*OFFSET_MULTIPLIER * unadjustedAngle.getRadians()); + // line above calculates how much our current speed will affect the ending + // location of the note if it's in the air for ShootingTime + + // next 3 lines set where we actually want to aim, given the offset our shooting + // will have based on our speed + double offsetSpeakerX; + double offsetSpeakerY; + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { + offsetSpeakerX = redX - targetOffset.getX(); + offsetSpeakerY = redY - targetOffset.getY(); + } else { + offsetSpeakerX = redX - targetOffset.getX(); + offsetSpeakerY = redY - targetOffset.getY(); + } + double offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX); + double offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY); + double offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaX, 2) + Math.pow(offsetDeltaY, 2)); + offsetSpeakerdist = offsetSpeakerdist+0.127; //to compensate for the pivot point of the shooter bieng offset from the center of the robot + SmartDashboard.putString("offset amount", targetOffset.toString()); + SmartDashboard.putString("offset speaker location", + new Translation2d(offsetSpeakerX, offsetSpeakerY).toString()); + // getting desired robot angle + double totalOffsetDistToSpeaker = Math + .sqrt(Math.pow(offsetSpeakerdist, 2) + Math.pow(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT, 2)); + SmartDashboard.putNumber("MATH/shooter's speaker dist", totalOffsetDistToSpeaker); + double desiredShooterAngle = Math + .toDegrees(Math.asin((Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT) / totalOffsetDistToSpeaker)); + desiredShooterAngle = adjustAngleForDistance(desiredShooterAngle, offsetSpeakerdist); + if(desiredShooterAngleShooterConstants.PRAC_MAXIMUM_SHOOTER_ANGLE) { + desiredShooterAngle = ShooterConstants.PRAC_MAXIMUM_SHOOTER_ANGLE; + } + if(offsetSpeakerdist>Field.MAX_SHOOTING_DISTANCE) { + desiredShooterAngle = SAFE_SHOOTER_ANGLE; + } + speakerDistGlobal = offsetSpeakerdist; // double differenceAngle = (desiredShooterAngle - this.getShooterAngle()); // SmartDashboard.putNumber("differenceAngleShooter", differenceAngle); @@ -253,6 +330,8 @@ public void periodic() { SmartDashboard.putNumber("ANGLE SHOOTER encoder zero offset", absoluteEncoder.getZeroOffset()); SmartDashboard.putNumber("ANGLE SHOOTER speaker angle error", calculateSpeakerAngleDifference()); + + SmartDashboard.putNumber("CALLIBRATION/Supplied calculated shooter angle", calculateSpeakerAngleWithSuppliers(()->SmartDashboard.getNumber("CALLIBRATION/redShooterX", 0), ()->SmartDashboard.getNumber("CALLIBRATION/blueShooterX", 0), ()->SmartDashboard.getNumber("CALLIBRATION/redY", 0), ()->SmartDashboard.getNumber("CALLIBRATION/blueY", 0))); if(calculateSpeakerAngleDifference() 720) { doRejectUpdate = true; @@ -431,6 +435,80 @@ public double calculateSpeakerAngleMoving(){ SmartDashboard.putString("adjusted target", adjustedTarget.toString()); return m_desiredRobotAngle; } + + public double calculateSpeakerAngleMovingWithSuppliers(DoubleSupplier redYSup, DoubleSupplier blueYSup, DoubleSupplier redRobotXSup, DoubleSupplier blueRobotXSup){ + double redY = redYSup.getAsDouble(); + double blueY = blueYSup.getAsDouble(); + double blueRobotX = blueRobotXSup.getAsDouble(); + double redRobotX = redRobotXSup.getAsDouble(); + dtvalues = this.getPose(); + Optional alliance = DriverStation.getAlliance(); + double speakerY; + shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; + //triangle for robot angle + if (alliance.isPresent() && alliance.get() == Alliance.Red) { + deltaX = Math.abs(dtvalues.getX() - redRobotX); + speakerY = redY; + } else { + deltaX = Math.abs(dtvalues.getX() - blueRobotX); + speakerY = blueY; + } + deltaY = Math.abs(dtvalues.getY() - speakerY); + speakerDist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2)); + // SmartDashboard.putNumber("dist to speaker", speakerDist); + + Rotation2d unadjustedAngle = Rotation2d.fromRadians(Math.asin(deltaX/speakerDist)); + double totalDistToSpeaker = Math.sqrt(Math.pow(Field.SPEAKER_Z-ShooterConstants.SHOOTER_HEIGHT, 2) + Math.pow(speakerDist, 2)); + shootingTime = totalDistToSpeaker/shootingSpeed; //calculates how long the note will take to reach the target + currentXSpeed = this.getChassisSpeeds().vxMetersPerSecond; + currentYSpeed = this.getChassisSpeeds().vyMetersPerSecond; + targetOffset = new Translation2d(currentXSpeed*shootingTime*OFFSET_MULTIPLIER*unadjustedAngle.getRadians(), currentYSpeed*shootingTime*OFFSET_MULTIPLIER); + //line above calculates how much our current speed will affect the ending location of the note if it's in the air for ShootingTime + + //next 3 lines set where we actually want to aim, given the offset our shooting will have based on our speed + int correctionDirection; + double speakerX; + if(DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Blue) { + correctionDirection = 1; + speakerX = blueRobotX; + } else { + correctionDirection = -1; + speakerX = redRobotX; + } + offsetSpeakerX = speakerX+(targetOffset.getX()*correctionDirection); + offsetSpeakerY = speakerY+(targetOffset.getY()*correctionDirection); + offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX); + offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY); + + adjustedTarget = new Translation2d(offsetSpeakerX, offsetSpeakerY); + offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaY, 2) + Math.pow(offsetDeltaX, 2)); + SmartDashboard.putNumber("MATH/Robot speaker dist", offsetSpeakerdist); + RobotState.getInstance().ShooterInRange = offsetSpeakerdist= adjustedTarget.getY()) { + double thetaAbove = -Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))-90; + m_desiredRobotAngle = thetaAbove; + } + else{ + double thetaBelow = Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))+90; + m_desiredRobotAngle = thetaBelow; + } } else { + if (dtvalues.getY() >= adjustedTarget.getY()) { + double thetaAbove = Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))-90; + m_desiredRobotAngle = thetaAbove; + } + else{ + double thetaBelow = -Math.toDegrees(Math.asin(offsetDeltaX / offsetSpeakerdist))+90; + m_desiredRobotAngle = thetaBelow; + } + } + MathRanNumber++; + SmartDashboard.putString("adjusted target", adjustedTarget.toString()); + return m_desiredRobotAngle; + } private double getSpeakerAngleDifference() { return calculateSpeakerAngleMoving()-(getOdometryRotation().getDegrees()%360); } @@ -448,6 +526,8 @@ public void periodic() { AngleShooterSubsystem.setDTPose(getPose()); AngleShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds()); if (Preferences.getBoolean("Use Limelight", false)) { + LimelightHelpers.SetRobotOrientation(APRILTAG_LIMELIGHT2_NAME, odometer.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(APRILTAG_LIMELIGHT3_NAME, odometer.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); if (SmartDashboard.getBoolean("Vision/force use limelight", false)) { forceUpdateOdometryWithVision(); } else { @@ -474,6 +554,8 @@ public void periodic() { SmartDashboard.putNumber("DRIVETRAIN/rotational speed", Math.toDegrees(getChassisSpeeds().omegaRadiansPerSecond)); SmartDashboard.putNumber("DRIVETRAIN/gyroscope rotation degrees", getPose().getRotation().getDegrees()); SmartDashboard.putNumber("DRIVETRAIN/degrees per second", Math.toDegrees(getChassisSpeeds().omegaRadiansPerSecond)); + + SmartDashboard.putNumber("CALLIBRATION/Supplied calculated robot angle", calculateSpeakerAngleMovingWithSuppliers(()->SmartDashboard.getNumber("CALLIBRATION/redY", 0), ()->SmartDashboard.getNumber("CALLIBRATION/blueY", 0), ()->SmartDashboard.getNumber("CALLIBRATION/redRobotX", 0), ()->SmartDashboard.getNumber("CALLIBRATION/blueRobotX", 0))); for (int i = 0; i < 4; i++) { motorLoggers[i].log(modules[i].getDriveMotor()); } From 48d4bca0fa3a66fe306e306f55ad4a696492dcd3 Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Sat, 13 Apr 2024 14:20:21 -0500 Subject: [PATCH 07/37] added shooting while moving to 4 note close auto --- .../autos/(AP)StartMidScore3Close.auto | 73 ++++++++++++------- .../paths/ CloseMidPickupFromAmpAuto.path | 28 +++++-- .../pathplanner/paths/CloseAmpPickupAuto.path | 28 +++++-- .../paths/CloseSourcePickupFromMidAuto.path | 36 ++++++--- src/main/java/frc/robot/RobotContainer.java | 12 ++- .../commands/NamedCommands/ShootNote.java | 12 +-- .../robot/subsystems/DrivetrainSubsystem.java | 21 ++++++ 7 files changed, 156 insertions(+), 54 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto index 636b29d0..19d6d2f3 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto @@ -18,15 +18,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "CloseAmpPickupAuto" - } - }, - { - "type": "named", - "data": { - "name": "autoShootNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "CloseAmpPickupAuto" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + } + ] } }, { @@ -36,21 +43,22 @@ } }, { - "type": "named", - "data": { - "name": "autoShootNote" - } - }, - { - "type": "path", - "data": { - "pathName": " CloseMidPickupFromAmpAuto" - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "hardStop" + "commands": [ + { + "type": "path", + "data": { + "pathName": " CloseMidPickupFromAmpAuto" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + } + ] } }, { @@ -60,15 +68,28 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "autoShootNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "CloseSourcePickupFromMidAuto" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + } + ] } }, { - "type": "path", + "type": "named", "data": { - "pathName": "CloseSourcePickupFromMidAuto" + "name": "shootNote" } }, { diff --git a/src/main/deploy/pathplanner/paths/ CloseMidPickupFromAmpAuto.path b/src/main/deploy/pathplanner/paths/ CloseMidPickupFromAmpAuto.path index d3f3b650..56d2e5cd 100644 --- a/src/main/deploy/pathplanner/paths/ CloseMidPickupFromAmpAuto.path +++ b/src/main/deploy/pathplanner/paths/ CloseMidPickupFromAmpAuto.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.8138348020468287, - "y": 5.901344756426168 + "x": 1.9112167287205282, + "y": 5.589722591070332 }, "prevControl": { - "x": 1.7825016096094515, - "y": 5.943122346342671 + "x": 1.879883536283151, + "y": 5.631500180986835 }, "nextControl": null, "isLocked": false, @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.5, "maxAcceleration": 2.3, diff --git a/src/main/deploy/pathplanner/paths/CloseAmpPickupAuto.path b/src/main/deploy/pathplanner/paths/CloseAmpPickupAuto.path index 137de77b..f12d7313 100644 --- a/src/main/deploy/pathplanner/paths/CloseAmpPickupAuto.path +++ b/src/main/deploy/pathplanner/paths/CloseAmpPickupAuto.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.940431306722638, - "y": 6.417468967796773 + "x": 2.018336848061597, + "y": 6.592756435809432 }, "prevControl": { - "x": 2.044875281513896, - "y": 6.43313556401546 + "x": 1.9793840773921174, + "y": 6.514850894470473 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.5, "maxAcceleration": 2.3, diff --git a/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path b/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path index 669d2ffe..4dd36a4d 100644 --- a/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path +++ b/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMidAuto.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.183886123411755, - "y": 4.859358141022456 + "x": 2.6443798011133075, + "y": 4.964008887705681 }, "prevControl": null, "nextControl": { - "x": 2.1281941601759766, - "y": 4.645872281951973 + "x": 2.4415493304328604, + "y": 4.867931296330732 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.9956286826153902, - "y": 4.39239001934794 + "x": 2.105980582067926, + "y": 4.57695055366386 }, "prevControl": { - "x": 1.9406557155265987, - "y": 4.377730561457596 + "x": 2.0510076149791345, + "y": 4.562291095773515 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,25 @@ } } ], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.5, "maxAcceleration": 2.3, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7e17de1..fe6dfeea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -499,7 +499,7 @@ private void configureDriveTrain() { driveTrain::getPose, // Pose2d supplier driveTrain::resetOdometry, // Pose2d consumer, used to reset odometry at the beginning of auto driveTrain::getChassisSpeeds, - driveTrain::drive, + driveTrain::driveWhileAimed, new HolonomicPathFollowerConfig( new PIDConstants( k_XY_P, @@ -547,7 +547,10 @@ private void registerNamedCommands() { if(shooterExists) { NamedCommands.registerCommand("shooterOn", new InstantCommand(()->shooter.shootRPS(LONG_SHOOTING_RPS), shooter)); SmartDashboard.putData("shooterOn", new InstantCommand(()->shooter.shootRPS(LONG_SHOOTING_RPS), shooter)); - } + } + if(intakeExists&&indexerExists&&angleShooterExists) { + NamedCommands.registerCommand("shootNote", new ShootNote(indexer, 0.25, 0, intake)); + } if(indexerExists) { // NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED), indexer)); // NamedCommands.registerCommand("stopFeedingShooter", new InstantCommand(indexer::off, indexer)); @@ -564,7 +567,10 @@ private void registerNamedCommands() { //when either command finishes. the AimRobotMoving command will never finish, but the shootNote finishes when shootTime is reached. NamedCommands.registerCommand("autoShootNote", new ParallelRaceGroup( new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup), - new ShootNote(indexer, 1.5, angleShooterSubsystem, intake))); + new SequentialCommandGroup( + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle()), angleShooterSubsystem), + new ShootNote(indexer, 1.5, 0.5,intake) + ))); // NamedCommands.registerCommand("setFeedTrue", new InstantCommand(()->SmartDashboard.putBoolean("feedMotor", true))); // NamedCommands.registerCommand("setFeedFalse", new InstantCommand(()->SmartDashboard.putBoolean("feedMotor", false))); } diff --git a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java index 6d82d2c0..1cb53ab0 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java +++ b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java @@ -14,25 +14,26 @@ public class ShootNote extends Command { IndexerSubsystem indexer; - AngleShooterSubsystem angleShooter; Timer timer; double shootTime; double revTime; IntakeSubsystem intake; /** Creates a new shootNote. */ - public ShootNote(IndexerSubsystem indexer, double shootTime, AngleShooterSubsystem angleShooter, IntakeSubsystem intake) { + public ShootNote(IndexerSubsystem indexer, double shootTime, double revTime, IntakeSubsystem intake) { + SmartDashboard.putNumber("notes shot", 0); this.indexer = indexer; this.shootTime = shootTime; - this.angleShooter = angleShooter; this.intake = intake; + this.revTime = revTime; timer = new Timer(); - addRequirements(indexer, angleShooter); + addRequirements(indexer); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() { + SmartDashboard.putNumber("notes shot", SmartDashboard.getNumber("notes shot", 0)+1); timer.reset(); timer.start(); indexer.off(); @@ -41,8 +42,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - angleShooter.setDesiredShooterAngle(angleShooter.calculateSpeakerAngle()); - if(timer.get()>=1) { + if(timer.get()>revTime) { indexer.set(IndexerConstants.INDEXER_SHOOTING_POWER); } SmartDashboard.putNumber("auto timer", timer.get()); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 8ba4a541..babdfb51 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -19,6 +19,9 @@ import static frc.robot.settings.Constants.DriveConstants.FR_DRIVE_MOTOR_ID; import static frc.robot.settings.Constants.DriveConstants.FR_STEER_ENCODER_ID; import static frc.robot.settings.Constants.DriveConstants.FR_STEER_MOTOR_ID; +import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kD; +import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kI; +import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kP; import static frc.robot.settings.Constants.ShooterConstants.OFFSET_MULTIPLIER; import static frc.robot.settings.Constants.Vision.APRILTAG_LIMELIGHT2_NAME; import static frc.robot.settings.Constants.Vision.APRILTAG_LIMELIGHT3_NAME; @@ -33,6 +36,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -110,6 +114,7 @@ public class DrivetrainSubsystem extends SubsystemBase { int runsValid; double MathRanNumber; MotorLogger[] motorLoggers; + PIDController speedController; public DrivetrainSubsystem() { SmartDashboard.putNumber("CALLIBRATION/redRobotX", Field.CALCULATED_SHOOTER_RED_SPEAKER_X); @@ -117,6 +122,13 @@ public DrivetrainSubsystem() { SmartDashboard.putNumber("CALLIBRATION/blueY", Field.CALCULATED_BLUE_SPEAKER_Y); SmartDashboard.putNumber("CALLIBRATION/redY", Field.CALCULATED_RED_SPEAKER_Y); + speedController = new PIDController( + AUTO_AIM_ROBOT_kP, + AUTO_AIM_ROBOT_kI, + AUTO_AIM_ROBOT_kD); + speedController.setTolerance(ShooterConstants.ROBOT_ANGLE_TOLERANCE); + speedController.enableContinuousInput(-180, 180); + MathRanNumber = 0; runsValid = 0; this.limelight=Limelight.getInstance(); @@ -254,6 +266,14 @@ public void drive(ChassisSpeeds chassisSpeeds) { setModuleStates(desiredStates); } } + + public void driveWhileAimed(ChassisSpeeds chassisSpeeds) { + ChassisSpeeds desiredSpeeds = new ChassisSpeeds( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + speedController.calculate(getOdometryRotation().getDegrees())); + drive(desiredSpeeds); + } /** * Sets all module drive speeds to 0, but leaves the wheel angles where they were. */ @@ -522,6 +542,7 @@ public void periodic() { if (DriverStation.getAlliance().isPresent()) { SmartDashboard.putString("alliance:", DriverStation.getAlliance().get().toString()); } + speedController.setSetpoint(calculateSpeakerAngleMoving()); updateOdometry(); AngleShooterSubsystem.setDTPose(getPose()); AngleShooterSubsystem.setDTChassisSpeeds(getChassisSpeeds()); From 5cfd1101862beccf265e4a5d1299bc34dfa635be Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Sat, 13 Apr 2024 15:28:00 -0500 Subject: [PATCH 08/37] put quicker shooting into amp side far auto --- .../autos/(AP)StartAmpScore3FarAmp.auto | 73 +++++++++++++++---- .../paths/Blue1AutoPickupFromStart.path | 46 ++++++++++-- .../pathplanner/paths/Blue2AutoPickup.path | 8 +- src/main/java/frc/robot/RobotContainer.java | 2 + .../java/frc/robot/subsystems/Limelight.java | 2 +- 5 files changed, 106 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index 372dacea..f000c633 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -11,12 +11,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "initialShot" - } - }, { "type": "named", "data": { @@ -24,9 +18,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "Blue1AutoPickupFromStart" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue1AutoPickupFromStart" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + } + ] } }, { @@ -36,15 +43,34 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "BlueAmpShoot1" + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueAmpShoot1" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] } }, { "type": "named", "data": { - "name": "autoShootNote" + "name": "shootNote" } }, { @@ -60,15 +86,34 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "BlueMiddleShoot2" + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueAmpShoot2" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] } }, { "type": "named", "data": { - "name": "autoShootNote" + "name": "shootNote" } } ] diff --git a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path index 35a13386..91fa5b84 100644 --- a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path @@ -28,16 +28,32 @@ "y": 7.780317782857788 }, "isLocked": false, - "linkedName": "CollectNote1Blue" + "linkedName": null + }, + { + "anchor": { + "x": 4.852150914266242, + "y": 7.45 + }, + "prevControl": { + "x": 4.719085501741837, + "y": 7.479677359878633 + }, + "nextControl": { + "x": 5.202725850291558, + "y": 7.371811849199025 + }, + "isLocked": false, + "linkedName": null }, { "anchor": { - "x": 6.507643667719127, - "y": 7.449717390537984 + "x": 6.585549209058087, + "y": 7.225738959188476 }, "prevControl": { - "x": 4.9261774267149905, - "y": 7.406300835188908 + "x": 4.986149852656257, + "y": 7.181901774728098 }, "nextControl": null, "isLocked": false, @@ -52,7 +68,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.5, "maxAcceleration": 2.3, diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index 90f86e2f..f11c588c 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.682931135731786, - "y": 6.125323187775677 + "x": 6.9361241450834035, + "y": 5.62867536173981 }, "prevControl": { - "x": 6.449214511714907, - "y": 6.212966921782006 + "x": 6.790051255072855, + "y": 6.115584995108306 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe6dfeea..f18e4687 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,6 +65,7 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -560,6 +561,7 @@ private void registerNamedCommands() { if(sideWheelsExists){ NamedCommands.registerCommand("intakeSideWheels", new InstantCommand(()-> intake.intakeSideWheels(1))); } + NamedCommands.registerCommand("note isn't held", new WaitUntilCommand(()->!intake.isNoteSeen())); } if(indexerExists&&shooterExists) { NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 1.0, 1.25, angleShooterSubsystem)); diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 13d77a6b..b2fa2adb 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -145,7 +145,7 @@ private boolean isTrustworthy(String limelightName, PoseEstimate estimate, Pose2 isValid(limelightName, estimate) && // estimate.pose.getTranslation().getDistance(odometryPose.getTranslation()) < ALLOWABLE_POSE_DIFFERENCE && // Unused // ((estimate.avgTagDist < MAX_TAG_DISTANCE) || (estimate.tagCount >= 2 && estimate.avgTagDist<6))); // Trust poses when there are two tags, or when the tags are close to the camera. - estimate.tagCount >= 2 && estimate.avgTagDist<6); + estimate.avgTagDist<7); if (limelightName.equalsIgnoreCase(APRILTAG_LIMELIGHT2_NAME)) { SmartDashboard.putBoolean("Vision/Left/trusted", trusted); From 29baafee2b480ec8f9129e23c875112bdf2724f2 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 20 Apr 2024 10:54:05 -0500 Subject: [PATCH 09/37] callibrate for HCPA field --- src/main/java/frc/robot/settings/Constants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 794f34bd..b45f7508 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -466,15 +466,15 @@ public final class Field{ public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263; - public static final double RED_SPEAKER_Y = 5.613; - public static final double SHOOTER_RED_SPEAKER_X = 16.582;//changed so that shots from the side wil aim to the opposite side, and bank in + public static final double RED_SPEAKER_Y = 5.613;//home field: 5.613 + public static final double SHOOTER_RED_SPEAKER_X = 16.582;//home field: 16.582 public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_BLUE_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263; - public static final double BLUE_SPEAKER_Y = 5.39; - public static final double SHOOTER_BLUE_SPEAKER_X = 0; - public static final double ROBOT_BLUE_SPEAKER_X =SHOOTER_BLUE_SPEAKER_X+0.6; + public static final double BLUE_SPEAKER_Y = 5.348;//home field: HCPA: 5.348 + public static final double SHOOTER_BLUE_SPEAKER_X = 0.13; //HCPA: 0.13 + public static final double ROBOT_BLUE_SPEAKER_X = SHOOTER_BLUE_SPEAKER_X+0.2; public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in From 3374852c42c810fc5340a1fd6e96cd2883526ba7 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 20 Apr 2024 10:54:32 -0500 Subject: [PATCH 10/37] speed up initial shot --- .../pathplanner/autos/(AP)StartAmpScore3FarAmp.auto | 8 +++++++- .../robot/commands/NamedCommands/InitialShot.java | 13 +++++++++---- .../java/frc/robot/subsystems/ShooterSubsystem.java | 3 +++ 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index f000c633..0a8de481 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -5,12 +5,18 @@ "x": 0.6842044526319189, "y": 6.690138362483131 }, - "rotation": -125.0 + "rotation": -116.0 }, "command": { "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "initialShot" + } + }, { "type": "named", "data": { diff --git a/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java b/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java index 3942d93b..3847f8c4 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java +++ b/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java @@ -19,6 +19,7 @@ public class InitialShot extends Command { double revTime; double shootTime; AngleShooterSubsystem angleShooter; + boolean shot; /** Creates a new shootThing. */ public InitialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double revTime, double shootTime, AngleShooterSubsystem angleShooter) { this.indexer = indexer; @@ -30,22 +31,25 @@ public InitialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double re addRequirements(shooter, indexer, angleShooter); // Use addRequirements() here to declare subsystem dependencies. } - + // Called when the command is initially scheduled. @Override public void initialize() { + shot = false; timer.reset(); timer.start(); indexer.off(); - shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS); + shooter.shootRPSWithCurrent(90, 600, 600); angleShooter.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(timer.get()>revTime) { + if(shooter.getRSpeed()>45) { indexer.on(); + timer.reset(); + shot = true; } } @@ -58,6 +62,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return timer.get()>shootTime; + return timer.get()>shootTime&&shot; + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 70ca4c4f..03e31fe7 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -128,6 +128,9 @@ public void shootRPS(double RPS) { public void shootSameRPS(double RPS) { setTargetVelocity(RPS, RPS, ShooterConstants.CURRENT_LIMIT, 100); } + public void shootSameRPSWithCurrent(double RPS, double supplyLimit, double statorLimit) { + setTargetVelocity(RPS, RPS, supplyLimit, statorLimit); + } /** * allows you to set the shooter's speed using a supplier. this way you can use a value on SmartDashboard to tune * the shooter's speed. From 0c12958307ce0b05311a6f75d3a4d4eccfaf8ce1 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 20 Apr 2024 10:54:39 -0500 Subject: [PATCH 11/37] speed up amp side auto --- .../paths/Blue1AutoPickupFromStart.path | 62 +++++-------------- .../pathplanner/paths/Blue2AutoPickup.path | 13 ++-- .../pathplanner/paths/BlueAmpShoot1.path | 4 +- .../pathplanner/paths/BlueAmpShoot2.path | 9 ++- src/main/java/frc/robot/RobotContainer.java | 4 +- .../robot/subsystems/DrivetrainSubsystem.java | 2 +- 6 files changed, 33 insertions(+), 61 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path index 91fa5b84..0f1e3267 100644 --- a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.0581473014797882, - "y": 7.409038027952599 + "x": 1.1126849299961952, + "y": 7.108880647180037 }, "prevControl": null, "nextControl": { - "x": 1.263190077007704, - "y": 7.416781454542863 + "x": 1.9209549213878978, + "y": 7.352335463864286 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.2091145416858735, - "y": 7.761753795112528 + "x": 2.79739226145119, + "y": 7.741863170559082 }, "prevControl": { - "x": 1.4065244211409311, - "y": 7.7560004609150734 + "x": 2.057289618731077, + "y": 7.741863170559082 }, "nextControl": { - "x": 4.798790832149566, - "y": 7.780317782857788 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.852150914266242, - "y": 7.45 - }, - "prevControl": { - "x": 4.719085501741837, - "y": 7.479677359878633 - }, - "nextControl": { - "x": 5.202725850291558, - "y": 7.371811849199025 + "x": 4.170511959714229, + "y": 7.741863170559082 }, "isLocked": false, "linkedName": null @@ -52,8 +36,8 @@ "y": 7.225738959188476 }, "prevControl": { - "x": 4.986149852656257, - "y": 7.181901774728098 + "x": 5.650682712990575, + "y": 7.264691729857955 }, "nextControl": null, "isLocked": false, @@ -68,25 +52,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 1.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shootNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.5, "maxAcceleration": 2.3, @@ -94,7 +60,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 2.0, "rotation": 180.0, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index f11c588c..678bad2e 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.9361241450834035, - "y": 5.62867536173981 + "x": 6.897171374413923, + "y": 5.68710451774403 }, "prevControl": { "x": 6.790051255072855, - "y": 6.115584995108306 + "y": 5.65788993974192 }, "nextControl": null, "isLocked": false, @@ -49,12 +49,15 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 2.0, "rotation": 163.54, "rotateFast": false }, "reversed": false, "folder": null, - "previewStartingState": null, + "previewStartingState": { + "rotation": 174.05471649527038, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path index ab0164f1..e04beac4 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path @@ -20,8 +20,8 @@ "y": 6.3787367080907 }, "prevControl": { - "x": 5.207198562545273, - "y": 6.70360649363274 + "x": 4.832674528931501, + "y": 6.81673486715894 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path index fd058514..6a07d460 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path @@ -20,8 +20,8 @@ "y": 6.027941261101977 }, "prevControl": { - "x": 5.864922951672713, - "y": 7.167309803184257 + "x": 5.08586753828312, + "y": 6.583018243142062 }, "nextControl": null, "isLocked": false, @@ -50,6 +50,9 @@ }, "reversed": false, "folder": null, - "previewStartingState": null, + "previewStartingState": { + "rotation": 173.91416260815873, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f18e4687..6179a99f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -550,7 +550,7 @@ private void registerNamedCommands() { SmartDashboard.putData("shooterOn", new InstantCommand(()->shooter.shootRPS(LONG_SHOOTING_RPS), shooter)); } if(intakeExists&&indexerExists&&angleShooterExists) { - NamedCommands.registerCommand("shootNote", new ShootNote(indexer, 0.25, 0, intake)); + NamedCommands.registerCommand("shootNote", new ShootNote(indexer, 0.2, 0, intake)); } if(indexerExists) { // NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED), indexer)); @@ -564,7 +564,7 @@ private void registerNamedCommands() { NamedCommands.registerCommand("note isn't held", new WaitUntilCommand(()->!intake.isNoteSeen())); } if(indexerExists&&shooterExists) { - NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 1.0, 1.25, angleShooterSubsystem)); + NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 0.55, 0.15, angleShooterSubsystem)); //the following command will both aim the robot at the speaker (with the AimRobotMoving), and shoot a note while aiming the shooter (with shootNote). As a race group, it ends //when either command finishes. the AimRobotMoving command will never finish, but the shootNote finishes when shootTime is reached. NamedCommands.registerCommand("autoShootNote", new ParallelRaceGroup( diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index babdfb51..b28bdf9a 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -185,7 +185,7 @@ public DrivetrainSubsystem() { getGyroscopeRotation(), getModulePositions(), DRIVE_ODOMETRY_ORIGIN); - odometer.setVisionMeasurementStdDevs(VecBuilder.fill(0.9, 0.9, 99999999)); + odometer.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 99999999)); } /** * Sets the gyroscope angle to zero. This can be used to set the direction the robot is currently facing to the From 18f19b2934e70ab8e161a4778948dc7e4ce4344e Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Mon, 22 Apr 2024 09:11:51 -0500 Subject: [PATCH 12/37] lowered tolerance, tested iZone --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/ShooterSubsystem.java | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f18e4687..6622a0f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -392,7 +392,7 @@ private void configureBindings() { new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), new InstantCommand(driveTrain::pointWheelsInward, driveTrain), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(15), angleShooterSubsystem), - new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<1)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)), + new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), new WaitCommand(0.1), new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index e339d9a2..d9d1e7ef 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -224,15 +224,15 @@ private static void updateMotor(TalonFX shooterMotor, boolean revState, double t private void updateMotors(){ isRevingL = updateIsReving(isRevingL, targetVelocityL, shooterL.getVelocity().getValueAsDouble()); isRevingR = updateIsReving(isRevingR, targetVelocityR, shooterR.getVelocity().getValueAsDouble()); - updateMotor(shooterL, isRevingL, targetVelocityL); - updateMotor(shooterR, isRevingR, targetVelocityR); - if(shooterL.getClosedLoopError().getValueAsDouble()>7) { + if(Math.abs(shooterL.getClosedLoopError().getValueAsDouble())>1.5) { configuratorL.apply(PIDLeftconfigs.withKI(0)); configuratorR.apply(PIDRightconfigs.withKI(0)); } else { - configuratorL.apply(PIDLeftconfigs.withKI(0.004)); - configuratorR.apply(PIDRightconfigs.withKI(0.004)); + configuratorL.apply(PIDLeftconfigs.withKI(0.00));//was 0.004 for both + configuratorR.apply(PIDRightconfigs.withKI(0.00)); } + updateMotor(shooterL, isRevingL, targetVelocityL); + updateMotor(shooterR, isRevingR, targetVelocityR); } public double getRSpeed() { return shooterR.getVelocity().getValueAsDouble(); @@ -253,6 +253,7 @@ public void periodic() { SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Right shooter speed", shooterR.getVelocity().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Left shooter speed", shooterL.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Shooter/right integral value", shooterR.getClosedLoopIntegratedOutput().getValueAsDouble()); double error = getSignedError(); RobotState.getInstance().ShooterError = error; if(Math.abs(error) Date: Tue, 23 Apr 2024 16:14:09 -0500 Subject: [PATCH 13/37] moved waypoint for shooting, organized source side auto --- .../autos/(AP)StartSourceScore3FarSource.auto | 81 ++++++++++++++----- .../paths/Blue3AutoPickupSourceSide.path | 8 +- .../paths/Blue3AutoUnderStageSource.path | 8 +- .../paths/Blue4AutoPickupSourceSide.path | 8 +- .../paths/Blue5AutoPickupSourceSide.path | 8 +- .../pathplanner/paths/BlueSourceShoot4.path | 8 +- .../paths/BlueSourceShoot4UnderStage.path | 38 ++++++--- .../pathplanner/paths/BlueSourceShoot5.path | 28 +++++-- 8 files changed, 131 insertions(+), 56 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto index b0c27942..40fb66c3 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7607347530216694, - "y": 4.480646518544946 + "x": 0.7523718013035084, + "y": 4.450354048988052 }, "rotation": 116.0 }, @@ -24,9 +24,22 @@ } }, { - "type": "path", + "type": "race", "data": { - "pathName": "Blue5AutoPickupFromStart" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue5AutoPickupFromStart" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + } + ] } }, { @@ -36,21 +49,34 @@ } }, { - "type": "path", - "data": { - "pathName": "BlueSourceShoot5" - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "autoShootNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueSourceShoot5" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] } }, { "type": "path", "data": { - "pathName": "Blue4AutoPickupSourceSide" + "pathName": "Blue4AutoPickupUnderStage" } }, { @@ -60,15 +86,28 @@ } }, { - "type": "path", - "data": { - "pathName": "BlueSourceShoot4" - } - }, - { - "type": "named", + "type": "race", "data": { - "name": "autoShootNote" + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueSourceShoot4UnderStage" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path index 4d8d9a63..137e7937 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 4.073095500876648, + "y": 3.291509121571031 }, "prevControl": null, "nextControl": { - "x": 4.590121955578446, - "y": 3.1185271540507364 + "x": 5.386673619416802, + "y": 3.3913813494230265 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path index d61cb3bf..3025c16d 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 4.073095500876648, + "y": 3.291509121571031 }, "prevControl": null, "nextControl": { - "x": 0.8809484408652919, - "y": 5.3558211663675195 + "x": 1.6775001047036486, + "y": 5.628675361739809 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path index 9c8d1a43..9b51001a 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.9848151084202525, - "y": 2.113187808819271 + "x": 6.994553301087623, + "y": 2.4929773228466994 }, "prevControl": { - "x": 5.8746611443400845, - "y": 1.674969138787625 + "x": 5.884399337007455, + "y": 2.054758652815053 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path index 4daafbed..d7889fd8 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 4.073095500876648, + "y": 3.291509121571031 }, "prevControl": null, "nextControl": { - "x": 4.334691138518079, - "y": 1.2179481149085758 + "x": 5.131242802356436, + "y": 1.4908023102808658 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path index bedeb531..ea0edfd2 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 4.073095500876648, + "y": 3.291509121571031 }, "prevControl": { - "x": 4.454865149790057, - "y": 2.3759342101523275 + "x": 5.251416813628413, + "y": 2.6487884055246176 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path index e34b8194..e2d7b32c 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.029894257986713, - "y": 2.4960698570045796 + "x": 6.955600530418143, + "y": 3.5057493602531693 }, "prevControl": null, "nextControl": { - "x": 6.634240172394935, - "y": 4.48930681965753 + "x": 4.297073932226157, + "y": 4.51852139765964 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.511314170908295, - "y": 4.7814525996786275 + "x": 4.267859354224047, + "y": 5.531293435066112 }, "prevControl": { - "x": 5.7772792176663845, - "y": 3.7784187549395263 + "x": 4.209430198219828, + "y": 4.674332480337559 }, "nextControl": null, "isLocked": false, - "linkedName": "ShootUnderStage" + "linkedName": null } ], "rotationTargets": [ @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path index a7fea243..5b2cca51 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.2765438370382913, - "y": 3.018654926198741 + "x": 4.073095500876648, + "y": 3.291509121571031 }, "prevControl": { - "x": 4.367682945957438, - "y": 2.350923222652966 + "x": 3.2940400874870557, + "y": 2.4053335888403695 }, "nextControl": null, "isLocked": false, @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, From 13a338711249bfd89a91dcbd4495275b06cc4e7f Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Tue, 23 Apr 2024 17:02:51 -0500 Subject: [PATCH 14/37] source side dont stop auto --- .../paths/Blue3AutoPickupSourceSide.path | 8 ++--- .../paths/Blue3AutoUnderStageSource.path | 8 ++--- .../paths/Blue5AutoPickupSourceSide.path | 8 ++--- .../pathplanner/paths/BlueSourceShoot4.path | 30 +++++++++++++++---- .../pathplanner/paths/BlueSourceShoot5.path | 10 +++---- 5 files changed, 41 insertions(+), 23 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path index 137e7937..030968b9 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.073095500876648, - "y": 3.291509121571031 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": null, "nextControl": { - "x": 5.386673619416802, - "y": 3.3913813494230265 + "x": 5.0847896467283356, + "y": 3.167402918073519 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path index 3025c16d..18a3a384 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.073095500876648, - "y": 3.291509121571031 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": null, "nextControl": { - "x": 1.6775001047036486, - "y": 5.628675361739809 + "x": 1.3756161320151823, + "y": 5.404696930390301 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path index d7889fd8..a4b71d0c 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.073095500876648, - "y": 3.291509121571031 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": null, "nextControl": { - "x": 5.131242802356436, - "y": 1.4908023102808658 + "x": 4.829358829667969, + "y": 1.2668238789313584 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path index ea0edfd2..cde3eccb 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.073095500876648, - "y": 3.291509121571031 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": { - "x": 5.251416813628413, - "y": 2.6487884055246176 + "x": 2.8460832247880394, + "y": 2.2592606988298214 }, "nextControl": null, "isLocked": false, @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -44,7 +62,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 0.8, "rotation": 148.60298946245044, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path index 5b2cca51..b57f2955 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.073095500876648, - "y": 3.291509121571031 + "x": 3.7712115281881817, + "y": 3.0675306902215236 }, "prevControl": { - "x": 3.2940400874870557, - "y": 2.4053335888403695 + "x": 2.9921561147985893, + "y": 2.181355157490862 }, "nextControl": null, "isLocked": false, @@ -62,7 +62,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 0.8, "rotation": 150.19409724626237, "rotateFast": false }, From f967f34242638cc4c4a444c4622afe45280132ab Mon Sep 17 00:00:00 2001 From: 2491NoMythic <2491nomythic@gmail.com> Date: Tue, 23 Apr 2024 18:28:16 -0500 Subject: [PATCH 15/37] removed I-value for motors completely instead of periodically setting to 0 --- .../frc/robot/subsystems/ShooterSubsystem.java | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 76a66dc5..edfaeb44 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -63,11 +63,11 @@ public class ShooterSubsystem extends SubsystemBase { /** Creates a new Shooter. */ public ShooterSubsystem(double runSpeed) { if(Preferences.getBoolean("CompBot", true)) { - PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.CompRightkP).withKV(ShooterConstants.CompRightkFF).withKI(0.004); - PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.CompLeftkP).withKV(ShooterConstants.CompLeftkFF).withKI(0.004); + PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.CompRightkP).withKV(ShooterConstants.CompRightkFF).withKI(0); + PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.CompLeftkP).withKV(ShooterConstants.CompLeftkFF).withKI(0); } else { - PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.PracRightkP).withKV(ShooterConstants.PracRightkFF).withKI(0.004); - PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.PracLeftkP).withKV(ShooterConstants.PracLeftkFF).withKI(0.004); + PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.PracRightkP).withKV(ShooterConstants.PracRightkFF).withKI(0); + PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.PracLeftkP).withKV(ShooterConstants.PracLeftkFF).withKI(0); } runsValid = 0; shooterR = new TalonFX(ShooterConstants.SHOOTER_R_MOTORID); @@ -227,13 +227,6 @@ private static void updateMotor(TalonFX shooterMotor, boolean revState, double t private void updateMotors(){ isRevingL = updateIsReving(isRevingL, targetVelocityL, shooterL.getVelocity().getValueAsDouble()); isRevingR = updateIsReving(isRevingR, targetVelocityR, shooterR.getVelocity().getValueAsDouble()); - if(Math.abs(shooterL.getClosedLoopError().getValueAsDouble())>1.5) { - configuratorL.apply(PIDLeftconfigs.withKI(0)); - configuratorR.apply(PIDRightconfigs.withKI(0)); - } else { - configuratorL.apply(PIDLeftconfigs.withKI(0.00));//was 0.004 for both - configuratorR.apply(PIDRightconfigs.withKI(0.00)); - } updateMotor(shooterL, isRevingL, targetVelocityL); updateMotor(shooterR, isRevingR, targetVelocityR); } From 739199047266157094bef051924601ac3494fbe9 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Tue, 23 Apr 2024 20:48:12 -0500 Subject: [PATCH 16/37] added smoother shooting to auto shooting on the Amp side auto --- .../autos/(AP)StartAmpScore3FarAmp.auto | 6 ---- .../pathplanner/paths/Blue2AutoPickup.path | 8 ++--- .../pathplanner/paths/BlueAmpShoot1.path | 34 ++++++++++++++----- .../pathplanner/paths/BlueAmpShoot2.path | 24 +++++++++++-- 4 files changed, 51 insertions(+), 21 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index 0a8de481..39548182 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -73,12 +73,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "shootNote" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index 678bad2e..f5102ca0 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.318582489016831, - "y": 6.582940573288553 + "x": 6.215497887698029, + "y": 6.212966921782006 }, "prevControl": null, "nextControl": { - "x": 5.447041047159915, - "y": 6.546428133560659 + "x": 6.343956445841113, + "y": 6.176454482054112 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path index e04beac4..cea213b4 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.078981161244491, - "y": 6.774673444449585 + "x": 4.530790556243034, + "y": 7.1380952251821475 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.845200801512714, - "y": 6.3787367080907 + "x": 4.725554409590433, + "y": 6.436945353131514 }, "prevControl": { - "x": 4.832674528931501, - "y": 6.81673486715894 + "x": 4.71302813700922, + "y": 6.874943512199754 }, "nextControl": null, "isLocked": false, @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -44,7 +62,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 0.8, "rotation": -177.7974018382342, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path index 6a07d460..1e3e883d 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path @@ -20,8 +20,8 @@ "y": 6.027941261101977 }, "prevControl": { - "x": 5.08586753828312, - "y": 6.583018243142062 + "x": 3.94649899620084, + "y": 7.751601363226451 }, "nextControl": null, "isLocked": false, @@ -36,7 +36,25 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.95, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, From cdda363f78b2803db1be2768b61add120e2a46f9 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 18:50:17 -0500 Subject: [PATCH 17/37] adjusted amp shot for higher consistency --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7d7919f4..fe16553b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -394,7 +394,7 @@ private void configureBindings() { new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(15), angleShooterSubsystem), new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), - new WaitCommand(0.1), + new WaitCommand(0.08), new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), new WaitCommand(0.5), new InstantCommand(()->intake.setNoteHeld(false)) From 4bfa8c8f27c6b9643d6ad8e3548c2dd50c295057 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 22:53:16 -0500 Subject: [PATCH 18/37] attempted to speed up intial shot --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/NamedCommands/InitialShot.java | 6 ++++-- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3cb8978e..1b3dcc27 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -61,6 +61,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + m_robotContainer.autonomousInit(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe16553b..fd14b10e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -564,7 +564,7 @@ private void registerNamedCommands() { NamedCommands.registerCommand("note isn't held", new WaitUntilCommand(()->!intake.isNoteSeen())); } if(indexerExists&&shooterExists) { - NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 0.55, 0.15, angleShooterSubsystem)); + NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 0.9, 0.1, angleShooterSubsystem)); //the following command will both aim the robot at the speaker (with the AimRobotMoving), and shoot a note while aiming the shooter (with shootNote). As a race group, it ends //when either command finishes. the AimRobotMoving command will never finish, but the shootNote finishes when shootTime is reached. NamedCommands.registerCommand("autoShootNote", new ParallelRaceGroup( diff --git a/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java b/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java index 3847f8c4..a3cd5dbb 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java +++ b/src/main/java/frc/robot/commands/NamedCommands/InitialShot.java @@ -5,6 +5,7 @@ package frc.robot.commands.NamedCommands; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.Field; import frc.robot.settings.Constants.ShooterConstants; @@ -39,14 +40,15 @@ public void initialize() { timer.reset(); timer.start(); indexer.off(); - shooter.shootRPSWithCurrent(90, 600, 600); + shooter.setTargetVelocity(65, 50, 600, 600); + SmartDashboard.putNumber("shooter resets", SmartDashboard.getNumber("shooter resets", 0)+1); angleShooter.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(shooter.getRSpeed()>45) { + if(shooter.getRSpeed()>35&&shooter.getLSpeed()>60) { indexer.on(); timer.reset(); shot = true; From ba7f7bef09e66d388d0a791df8fbd75e85d10bea Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 22:54:18 -0500 Subject: [PATCH 19/37] changed subwoofer starting angles --- src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto | 2 +- .../pathplanner/autos/(AP)StartSourceScore3FarSource.auto | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index 39548182..f70d093e 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -5,7 +5,7 @@ "x": 0.6842044526319189, "y": 6.690138362483131 }, - "rotation": -116.0 + "rotation": -124.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto index 40fb66c3..9292466a 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto @@ -5,7 +5,7 @@ "x": 0.7523718013035084, "y": 4.450354048988052 }, - "rotation": 116.0 + "rotation": 119.0 }, "command": { "type": "sequential", From 18a57d6f7ae176a125361649ca1ffc54f741ffee Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 22:55:09 -0500 Subject: [PATCH 20/37] shoot during paths --- .../deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index f70d093e..c009000a 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -109,12 +109,6 @@ } ] } - }, - { - "type": "named", - "data": { - "name": "shootNote" - } } ] } From ce255e024a1afff45a98216258ed84af9e4a100c Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 22:55:38 -0500 Subject: [PATCH 21/37] used custom WaitUntil command with SmartDashboard logging --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- src/main/java/frc/robot/commands/WaitUntil.java | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fd14b10e..291eaa44 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -487,6 +487,9 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } + public void autonomousInit() { + SmartDashboard.putNumber("autos ran", SmartDashboard.getNumber("autos ran", 0)+1); + } private double modifyAxis(double value, double deadband) { // Deadband value = MathUtil.applyDeadband(value, deadband); @@ -561,7 +564,7 @@ private void registerNamedCommands() { if(sideWheelsExists){ NamedCommands.registerCommand("intakeSideWheels", new InstantCommand(()-> intake.intakeSideWheels(1))); } - NamedCommands.registerCommand("note isn't held", new WaitUntilCommand(()->!intake.isNoteSeen())); + NamedCommands.registerCommand("note isn't held", new WaitUntil(()->!intake.isNoteSeen())); } if(indexerExists&&shooterExists) { NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 0.9, 0.1, angleShooterSubsystem)); diff --git a/src/main/java/frc/robot/commands/WaitUntil.java b/src/main/java/frc/robot/commands/WaitUntil.java index f9a7750e..ba9a0268 100644 --- a/src/main/java/frc/robot/commands/WaitUntil.java +++ b/src/main/java/frc/robot/commands/WaitUntil.java @@ -6,6 +6,7 @@ import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class WaitUntil extends Command { @@ -31,6 +32,8 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return condition.getAsBoolean(); + boolean finish = condition.getAsBoolean(); + SmartDashboard.putBoolean("wait until end condition", finish); + return finish; } } From 9ea51d461810243ee2ef9a4b231c04b5da0f493e Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 22:58:15 -0500 Subject: [PATCH 22/37] reach pick up pose while moving --- .../deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path | 2 +- src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path index 9758ae01..af7b64ee 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path @@ -44,7 +44,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 150.02, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path index 65ff1d5c..91746031 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path @@ -44,7 +44,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 162.4744316262771, "rotateFast": false }, From 3bb1f9ba4012d375facb783969e0022293c3c34d Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 22:58:53 -0500 Subject: [PATCH 23/37] tuned auto timings for shooting --- .../paths/Blue1AutoPickupFromStart.path | 4 +-- .../pathplanner/paths/Blue2AutoPickup.path | 4 +-- .../paths/Blue5AutoPickupFromStart.path | 12 ++++----- .../paths/BlueSourceShoot4UnderStage.path | 27 ++++++++++++++----- .../pathplanner/paths/BlueSourceShoot5.path | 10 +++---- 5 files changed, 36 insertions(+), 21 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path index 0f1e3267..6e2776fd 100644 --- a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path @@ -36,8 +36,8 @@ "y": 7.225738959188476 }, "prevControl": { - "x": 5.650682712990575, - "y": 7.264691729857955 + "x": 3.9951899595376896, + "y": 7.206262573853738 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index f5102ca0..fcbdccb1 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -20,8 +20,8 @@ "y": 5.68710451774403 }, "prevControl": { - "x": 6.790051255072855, - "y": 5.65788993974192 + "x": 6.19602150236329, + "y": 5.67736632507666 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path index 91746031..2aca7816 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.7779158761164506, - "y": 2.34690443283615 + "x": 3.7127823721839626, + "y": 1.2075358907538694 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.60877963731236, - "y": 1.412869986233799 + "x": 7.208793539769761, + "y": 1.5483726341118162 }, "prevControl": { - "x": 4.783983565594652, - "y": 1.645754560785515 + "x": 6.829004025742336, + "y": 2.2397843134950803 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path index e2d7b32c..c46fe119 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.955600530418143, - "y": 3.5057493602531693 + "x": 7.0140296864223615, + "y": 3.3402000849078815 }, "prevControl": null, "nextControl": { - "x": 4.297073932226157, - "y": 4.51852139765964 + "x": 4.355503088230376, + "y": 4.352972122314352 }, "isLocked": false, "linkedName": null @@ -35,7 +35,19 @@ "rotateFast": false } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.8, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [ { "name": "New Event Marker", @@ -68,6 +80,9 @@ }, "reversed": false, "folder": null, - "previewStartingState": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path index b57f2955..980c26d6 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 3.7712115281881817, - "y": 3.0675306902215236 + "x": 4.131524656880868, + "y": 3.018839726884674 }, "prevControl": { - "x": 2.9921561147985893, - "y": 2.181355157490862 + "x": 3.3524692434912753, + "y": 2.1326641941540125 }, "nextControl": null, "isLocked": false, - "linkedName": "SourceSideBlueShoot" + "linkedName": null } ], "rotationTargets": [ From 6f28bd1c4320849a541a80b62cddc9db2838fe07 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 26 Apr 2024 22:59:27 -0500 Subject: [PATCH 24/37] more auto pose tuning --- src/main/deploy/pathplanner/paths/BlueAmpShoot1.path | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path index cea213b4..c2e9ad97 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.302473839515581, - "y": 6.852118395594816 + "x": 6.429738126380168, + "y": 7.206262573853738 }, "prevControl": null, "nextControl": { - "x": 4.530790556243034, - "y": 7.1380952251821475 + "x": 4.658054843107622, + "y": 7.492239403441069 }, "isLocked": false, "linkedName": null @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.85, + "waypointRelativePos": 0.75, "command": { "type": "parallel", "data": { From bb57905c99e8be7f901369957cee88516056455a Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 14:32:19 -0500 Subject: [PATCH 25/37] patch close note auto --- .../deploy/pathplanner/autos/(AP)StartMidScore3Close.auto | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto index 19d6d2f3..6aef7d9e 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto @@ -92,6 +92,12 @@ "name": "shootNote" } }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, { "type": "named", "data": { From cc57f9afe3929d7fdf7b55ac0e830d512db9a5c1 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 14:32:42 -0500 Subject: [PATCH 26/37] added HCPA field constants --- src/main/java/frc/robot/settings/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index b45f7508..303503f9 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -466,8 +466,8 @@ public final class Field{ public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263; - public static final double RED_SPEAKER_Y = 5.613;//home field: 5.613 - public static final double SHOOTER_RED_SPEAKER_X = 16.582;//home field: 16.582 + public static final double RED_SPEAKER_Y = 5.68;//home field: 5.613 HCPA: 5.68 + public static final double SHOOTER_RED_SPEAKER_X = 16.55;//home field: 16.582 HCPA: 16.55 public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in From 662232fb7ecf260c00dd2d08eed9b9afc20b8ccf Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 14:34:24 -0500 Subject: [PATCH 27/37] adjusted path of source side auot, added setpoint shot from far out --- .../autos/(AP)StartSourceScore3FarSource.auto | 10 ++++- .../paths/Blue4AutoPickupSourceSide.path | 18 ++++----- .../paths/Blue5AutoPickupFromStart.path | 8 ++-- .../paths/BlueSourceShoot4UnderStage.path | 12 +++--- .../pathplanner/paths/BlueSourceShoot5.path | 38 +++++-------------- src/main/java/frc/robot/RobotContainer.java | 14 +++++-- .../frc/robot/commands/AimRobotMoving.java | 9 ++++- 7 files changed, 56 insertions(+), 53 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto index 9292466a..539bb752 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto @@ -5,7 +5,7 @@ "x": 0.7523718013035084, "y": 4.450354048988052 }, - "rotation": 119.0 + "rotation": 118.0 }, "command": { "type": "sequential", @@ -73,10 +73,16 @@ ] } }, + { + "type": "named", + "data": { + "name": "sourceSideLongShot" + } + }, { "type": "path", "data": { - "pathName": "Blue4AutoPickupUnderStage" + "pathName": "Blue4AutoPickupSourceSide" } }, { diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path index 9b51001a..460dc70a 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.608696097581994, - "y": 1.8210420287981743 + "x": 5.981781263681151, + "y": 1.4217761294360087 }, "prevControl": null, "nextControl": { - "x": 5.942828493011672, - "y": 1.606801790116036 + "x": 6.682931135731786, + "y": 2.4735009375119583 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.994553301087623, - "y": 2.4929773228466994 + "x": 7.072458842426581, + "y": 2.8825050295414942 }, "prevControl": { - "x": 5.884399337007455, - "y": 2.054758652815053 + "x": 6.751098484403375, + "y": 2.619573827522507 }, "nextControl": null, "isLocked": false, @@ -44,7 +44,7 @@ "maxAngularAcceleration": 350.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": -165.06858282186255, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path index 2aca7816..53a4614f 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.7748820313773495, - "y": 3.7492041769374174 + "x": 2.622104793438532, + "y": 2.999363341549934 }, "prevControl": null, "nextControl": { - "x": 3.7127823721839626, - "y": 1.2075358907538694 + "x": 4.90084187760309, + "y": 1.071201193410692 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path index c46fe119..6750ed46 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.355503088230376, - "y": 4.352972122314352 + "x": 5.280631391630518, + "y": 5.424173315725042 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.267859354224047, - "y": 5.531293435066112 + "x": 4.102310078878758, + "y": 5.72605728841351 }, "prevControl": { - "x": 4.209430198219828, - "y": 4.674332480337559 + "x": 4.014666344872429, + "y": 4.245852002973283 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path index 980c26d6..8e000727 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.107551968190356, - "y": 1.3850240046159097 + "x": 7.734655943807735, + "y": 0.9056519180654026 }, "prevControl": null, "nextControl": { - "x": 4.415771287331697, - "y": 1.9027273823650077 + "x": 6.877694989079185, + "y": 1.1491067347496495 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.131524656880868, - "y": 3.018839726884674 + "x": 5.913613915009562, + "y": 1.460728900105488 }, "prevControl": { - "x": 3.3524692434912753, - "y": 2.1326641941540125 + "x": 6.390785355710689, + "y": 1.2367504687559792 }, "nextControl": null, "isLocked": false, @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.9, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shootNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -62,7 +44,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.8, + "velocity": 0.0, "rotation": 150.19409724626237, "rotateFast": false }, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 291eaa44..48486b9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -312,7 +312,8 @@ private void configureBindings() { FarStageAngleSup, SubwooferAngleSup, OverStagePassSup, - OppositeStageShotSup + OppositeStageShotSup, + falseSup )); if(Preferences.getBoolean("Detector Limelight", false)) { @@ -542,7 +543,7 @@ private void registerNamedCommands() { } if(intakeExists&&!indexerExists&&!angleShooterExists) { NamedCommands.registerCommand("groundIntake", new InstantCommand(()->intake.intakeYes(IntakeConstants.INTAKE_SPEED, IntakeConstants.INTAKE_SIDE_SPEED))); - NamedCommands.registerCommand("autoShootNote", new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup).withTimeout(1)); + NamedCommands.registerCommand("autoShootNote", new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, falseSup).withTimeout(1)); NamedCommands.registerCommand("autoPickup", new SequentialCommandGroup( new CollectNote(driveTrain, limelight), new DriveTimeCommand(-1, 0, 0, 1, driveTrain) @@ -554,6 +555,13 @@ private void registerNamedCommands() { } if(intakeExists&&indexerExists&&angleShooterExists) { NamedCommands.registerCommand("shootNote", new ShootNote(indexer, 0.2, 0, intake)); + NamedCommands.registerCommand("sourceSideLongShot", new SequentialCommandGroup( + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(25.3), angleShooterSubsystem), + new ParallelRaceGroup( + new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, ()->true), + new ShootNote(indexer, 0.75, 0.6, intake) + ) + )); } if(indexerExists) { // NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED), indexer)); @@ -571,7 +579,7 @@ private void registerNamedCommands() { //the following command will both aim the robot at the speaker (with the AimRobotMoving), and shoot a note while aiming the shooter (with shootNote). As a race group, it ends //when either command finishes. the AimRobotMoving command will never finish, but the shootNote finishes when shootTime is reached. NamedCommands.registerCommand("autoShootNote", new ParallelRaceGroup( - new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup), + new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, falseSup), new SequentialCommandGroup( new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle()), angleShooterSubsystem), new ShootNote(indexer, 1.5, 0.5,intake) diff --git a/src/main/java/frc/robot/commands/AimRobotMoving.java b/src/main/java/frc/robot/commands/AimRobotMoving.java index c8c2b27f..38742675 100644 --- a/src/main/java/frc/robot/commands/AimRobotMoving.java +++ b/src/main/java/frc/robot/commands/AimRobotMoving.java @@ -36,6 +36,7 @@ public class AimRobotMoving extends Command { BooleanSupplier SubwooferAngleSup; BooleanSupplier OverStagePassSup; BooleanSupplier OppositeStageShotSup; + BooleanSupplier AutonomousLongShotSup; /** * a command to automatically aim the robot at the speaker if odometry is correct. This command also controls aiming from setpoints incase the odometry isn't working. * @param drivetrain the swerve drive subsystem @@ -49,7 +50,7 @@ public class AimRobotMoving extends Command { * @param OverStagePassSup button to press to aim at the speaker from the opposite side of the stage (from the speaker) * @param OppositeStageShotSup button to press to aim at the speaker from the opposite side of the stage (from the speaker) */ - public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier PodiumAngleSup, BooleanSupplier FarStageAngleSup, BooleanSupplier SubwooferAngleSup, BooleanSupplier OverStagePassSup, BooleanSupplier OppositeStageShotSup){ + public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier PodiumAngleSup, BooleanSupplier FarStageAngleSup, BooleanSupplier SubwooferAngleSup, BooleanSupplier OverStagePassSup, BooleanSupplier OppositeStageShotSup, BooleanSupplier AutonomousLongShotSup){ m_drivetrain = drivetrain; speedController = new PIDController( AUTO_AIM_ROBOT_kP, @@ -66,6 +67,7 @@ public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSup this.OverStagePassSup = OverStagePassSup; this.OppositeStageShotSup = OppositeStageShotSup; this.run = run; + this.AutonomousLongShotSup = AutonomousLongShotSup; addRequirements(drivetrain); } @@ -84,16 +86,19 @@ public void execute() { double farStageRobotAngle; double OverStagePassAngle; double OppositeStageShotAngle; + double AutonomousLongShotAngle; if(DriverStation.getAlliance().get() == Alliance.Red) { podiumRobotAngle = Field.RED_PODIUM_ROBOT_ANGLE; farStageRobotAngle = Field.RED_FAR_STAGE_ROBOT_ANGLE; OverStagePassAngle = Field.RED_OVER_STAGE_PASS_ANGLE; OppositeStageShotAngle = Field.RED_OPPOSITE_STAGE_ROBOT_ANGLE; + AutonomousLongShotAngle = 33.7; } else { podiumRobotAngle = Field.BLUE_PODIUM_ROBOT_ANGLE; farStageRobotAngle = Field.BLUE_FAR_STAGE_ROBOT_ANGLE; OverStagePassAngle = Field.BLUE_OVER_STAGE_PASS_ANGLE; OppositeStageShotAngle = Field.BLUE_OPPOSITE_STAGE_ROBOT_ANGLE; + AutonomousLongShotAngle = 146.3; } if(PodiumAngleSup.getAsBoolean()) { speedController.setSetpoint(podiumRobotAngle); @@ -103,6 +108,8 @@ public void execute() { speedController.setSetpoint(OverStagePassAngle); } else if(OppositeStageShotSup.getAsBoolean()) { speedController.setSetpoint(OppositeStageShotAngle); + } else if (AutonomousLongShotSup.getAsBoolean()) { + speedController.setSetpoint(AutonomousLongShotAngle); } else { speedController.setSetpoint(desiredRobotAngle); } From a7ea106f2b677cca6d4c1c405301f1f60617a36b Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 14:34:35 -0500 Subject: [PATCH 28/37] more consistent pickups --- .../deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto | 2 +- src/main/deploy/pathplanner/paths/Blue2AutoPickup.path | 2 +- src/main/java/frc/robot/commands/GroundIntake.java | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index c009000a..77b79803 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -5,7 +5,7 @@ "x": 0.6842044526319189, "y": 6.690138362483131 }, - "rotation": -124.0 + "rotation": -116.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index fcbdccb1..2f4bd3f2 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -49,7 +49,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 2.0, + "velocity": 1.0, "rotation": 163.54, "rotateFast": false }, diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 1ce45630..72896d9e 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -65,7 +65,8 @@ public void end(boolean interrupted) { intake.intakeOff(); // indexer.magicRPS(0); if(DriverStation.isAutonomous()) { - indexer.forwardInches(-1); + // indexer.forwardInches(-1); + indexer.off(); } else { indexer.off(); } From 94dd86b41c65301b8390953936b243c1869f5e73 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:32:45 -0500 Subject: [PATCH 29/37] switched shooter and drivetrain to use velocityVoltage --- .../frc/robot/commands/NamedCommands/ShootNote.java | 2 +- src/main/java/frc/robot/settings/Constants.java | 10 +++++----- src/main/java/frc/robot/subsystems/SwerveModule.java | 8 ++++++-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java index 1cb53ab0..1e30e68d 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java +++ b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java @@ -43,7 +43,7 @@ public void initialize() { @Override public void execute() { if(timer.get()>revTime) { - indexer.set(IndexerConstants.INDEXER_SHOOTING_POWER); + indexer.setVoltage(12); } SmartDashboard.putNumber("auto timer", timer.get()); } diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 303503f9..22d76b37 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -406,9 +406,9 @@ public CTREConfigs() { driveMotorConfig.MotorOutput.Inverted = DriveConstants.DRIVETRAIN_DRIVE_INVERTED; driveMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.0; driveMotorConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = DriveConstants.DRIVE_MOTOR_RAMP; - driveMotorConfig.Slot0.kP = DriveConstants.k_DRIVE_P; - driveMotorConfig.Slot0.kI = DriveConstants.k_DRIVE_I; - driveMotorConfig.Slot0.kD = DriveConstants.k_DRIVE_D; + driveMotorConfig.Slot0.kP = DriveConstants.k_DRIVE_P*12; + driveMotorConfig.Slot0.kI = DriveConstants.k_DRIVE_I*12; + driveMotorConfig.Slot0.kD = DriveConstants.k_DRIVE_D*12; driveMotorConfig.Slot0.kS = DriveConstants.k_DRIVE_FF_S; driveMotorConfig.Slot0.kV = DriveConstants.k_DRIVE_FF_V; driveMotorConfig.Voltage.PeakForwardVoltage = 12; @@ -466,8 +466,8 @@ public final class Field{ public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263; - public static final double RED_SPEAKER_Y = 5.68;//home field: 5.613 HCPA: 5.68 - public static final double SHOOTER_RED_SPEAKER_X = 16.55;//home field: 16.582 HCPA: 16.55 + public static final double RED_SPEAKER_Y = 5.613;//home field: 5.613 HCPA: 5.68 + public static final double SHOOTER_RED_SPEAKER_X = 16.582;//home field: 16.582 HCPA: 16.55 public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 73b6e648..00947bf9 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -4,11 +4,15 @@ package frc.robot.subsystems; +import javax.swing.text.Position; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -37,7 +41,7 @@ public class SwerveModule { */ private double m_desiredDriveSpeed; - private VelocityDutyCycle m_driveControl = new VelocityDutyCycle(0); + private VelocityVoltage m_driveControl = new VelocityVoltage(0); private PositionDutyCycle m_steerControl = new PositionDutyCycle(0); private NeutralOut m_neutralControl = new NeutralOut(); @@ -167,7 +171,7 @@ public void setDesiredState(SwerveModuleState desiredState) { m_driveMotor.setControl(m_neutralControl); m_steerMotor.setControl(m_steerControl.withPosition(m_desiredSteerAngle)); } else { - m_driveMotor.setControl(m_driveControl.withVelocity(m_desiredDriveSpeed).withFeedForward(m_desiredDriveSpeed/DriveConstants.MAX_VELOCITY_RPS_EMPIRICAL));//TODO verify that this feedforward is effective + m_driveMotor.setControl(m_driveControl.withVelocity(m_desiredDriveSpeed).withFeedForward((m_desiredDriveSpeed/DriveConstants.MAX_VELOCITY_RPS_EMPIRICAL)*12));//TODO verify that this feedforward is effective m_steerMotor.setControl(m_steerControl.withPosition(m_desiredSteerAngle)); } } From 213c1d3fa9cb35469923191eadd607de3e8399bc Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:32:58 -0500 Subject: [PATCH 30/37] indexer velocityVoltage --- src/main/java/frc/robot/subsystems/IndexerSubsystem.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index f82207ff..430a7152 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -13,6 +13,7 @@ import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -94,6 +95,9 @@ public void reverse() { public void set(double speed) { m_IndexerMotor.set(speed); } + public void setVoltage(double voltage) { + m_IndexerMotor.setControl(new VoltageOut(voltage)); + } /** * uses the indexer motor's onboard Motion Magic control to move the indexer forward. To move backwards, use negative inches. * @param inches the inches to move forward. From bfeb6491c56ddb5e581a0876c56d23e753407145 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:34:47 -0500 Subject: [PATCH 31/37] switched to using setpoints instead of odometry shooting in some autos --- .../autos/(AP)StartAmpScore3FarAmp.auto | 4 ++-- .../autos/(AP)StartSourceScore3FarSource.auto | 2 +- src/main/java/frc/robot/RobotContainer.java | 14 ++++++++++++-- .../java/frc/robot/commands/AimRobotMoving.java | 2 +- 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index 77b79803..75b3a1f5 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -61,7 +61,7 @@ { "type": "named", "data": { - "name": "autoAimAtSpeaker" + "name": "midAmpSideShooterAngle" } }, { @@ -98,7 +98,7 @@ { "type": "named", "data": { - "name": "autoAimAtSpeaker" + "name": "midAmpSideShooterAngle" } }, { diff --git a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto index 539bb752..35fdf81e 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto @@ -104,7 +104,7 @@ { "type": "named", "data": { - "name": "autoAimAtSpeaker" + "name": "lowAmpSideShooterAngle" } }, { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48486b9e..a61373a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -556,12 +556,22 @@ private void registerNamedCommands() { if(intakeExists&&indexerExists&&angleShooterExists) { NamedCommands.registerCommand("shootNote", new ShootNote(indexer, 0.2, 0, intake)); NamedCommands.registerCommand("sourceSideLongShot", new SequentialCommandGroup( - new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(25.3), angleShooterSubsystem), + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(22.5), angleShooterSubsystem), new ParallelRaceGroup( new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, ()->true), - new ShootNote(indexer, 0.75, 0.6, intake) + new ShootNote(indexer, 0.5, 0.4, intake) ) )); + NamedCommands.registerCommand("midAmpSideShooterAngle", new OverrideCommand(angleShooterSubsystem) { + public void execute() { + angleShooterSubsystem.setDesiredShooterAngle(27); + }; + }); + NamedCommands.registerCommand("lowAmpSideShooterAngle", new OverrideCommand(angleShooterSubsystem) { + public void execute() { + angleShooterSubsystem.setDesiredShooterAngle(29.5); + }; + }); } if(indexerExists) { // NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED), indexer)); diff --git a/src/main/java/frc/robot/commands/AimRobotMoving.java b/src/main/java/frc/robot/commands/AimRobotMoving.java index 38742675..d22dbbca 100644 --- a/src/main/java/frc/robot/commands/AimRobotMoving.java +++ b/src/main/java/frc/robot/commands/AimRobotMoving.java @@ -92,7 +92,7 @@ public void execute() { farStageRobotAngle = Field.RED_FAR_STAGE_ROBOT_ANGLE; OverStagePassAngle = Field.RED_OVER_STAGE_PASS_ANGLE; OppositeStageShotAngle = Field.RED_OPPOSITE_STAGE_ROBOT_ANGLE; - AutonomousLongShotAngle = 33.7; + AutonomousLongShotAngle = 35; } else { podiumRobotAngle = Field.BLUE_PODIUM_ROBOT_ANGLE; farStageRobotAngle = Field.BLUE_FAR_STAGE_ROBOT_ANGLE; From 57b31b35051e9784dfd3f4957effed601d325104 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:35:03 -0500 Subject: [PATCH 32/37] adjust starting angle for home field --- src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto | 2 +- .../pathplanner/autos/(AP)StartSourceScore3FarSource.auto | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index 75b3a1f5..02fcb8d5 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -5,7 +5,7 @@ "x": 0.6842044526319189, "y": 6.690138362483131 }, - "rotation": -116.0 + "rotation": -125.5 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto index 35fdf81e..502986ae 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto @@ -5,7 +5,7 @@ "x": 0.7523718013035084, "y": 4.450354048988052 }, - "rotation": 118.0 + "rotation": 119.3 }, "command": { "type": "sequential", From 6237497b43aedef7bd92e49091a5bebf090fdfc8 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:35:15 -0500 Subject: [PATCH 33/37] debug shot-skipping problem --- .../pathplanner/autos/AmpShotPathTest.auto | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/AmpShotPathTest.auto diff --git a/src/main/deploy/pathplanner/autos/AmpShotPathTest.auto b/src/main/deploy/pathplanner/autos/AmpShotPathTest.auto new file mode 100644 index 00000000..9cbcab7b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AmpShotPathTest.auto @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 4.696339831588324, + "y": 7.478931968540094 + }, + "rotation": 178.8308606720925 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue1AutoPickupFromStart" + } + }, + { + "type": "named", + "data": { + "name": "autoPickup" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BlueAmpShoot1" + } + }, + { + "type": "named", + "data": { + "name": "autoAimAtSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "note isn't held" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Blue2AutoPickup" + } + }, + { + "type": "named", + "data": { + "name": "autoPickup" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file From cd291764ae3afcb319f695e1a4c595e22838a583 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:35:37 -0500 Subject: [PATCH 34/37] for tuning a setpoint --- src/main/java/frc/robot/RobotContainer.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a61373a1..a5ec38cd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -315,6 +315,15 @@ private void configureBindings() { OppositeStageShotSup, falseSup )); + // new Trigger(()->driverController.getL3Button()&&driverController.getR3Button()).onTrue( + // new SequentialCommandGroup( + // new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(22.5), angleShooterSubsystem), + // new ParallelRaceGroup( + // new AimRobotMoving(driveTrain, zeroSup, zeroSup, zeroSup, ()->true, falseSup, falseSup, falseSup, falseSup, falseSup, ()->true), + // new ShootNote(indexer, 0.65, 0.4, intake) + // ) + // ) + // ); if(Preferences.getBoolean("Detector Limelight", false)) { Command AutoGroundIntake = new SequentialCommandGroup( From 1e78739404527887acca8bbddb04bc747345094a Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:35:50 -0500 Subject: [PATCH 35/37] fix CollectNote sideways problem --- src/main/java/frc/robot/commands/CollectNote.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index eefd876e..4a29c587 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -50,7 +50,7 @@ public void initialize() { 0.6,//Vision.K_DETECTOR_TY_P, Vision.K_DETECTOR_TY_I, Vision.K_DETECTOR_TY_D); - tyLimiter = new SlewRateLimiter(2, -2, 0); + tyLimiter = new SlewRateLimiter(20, -20, 0); txController.setSetpoint(0); tyController.setSetpoint(0); txController.setTolerance(3.5, 0.25); @@ -114,7 +114,7 @@ public void execute() { } SmartDashboard.putNumber("CollectNote/calculated radians per second", txController.calculate(tx)); - SmartDashboard.putNumber("CollectNote/calculated forward meters per second", tyController.calculate(ty)); + SmartDashboard.putNumber("CollectNote/calculated forward meters per second", tyLimiter.calculate(-20/Math.abs(tx))); SmartDashboard.putBoolean("CollectNote/closeNote", closeNote); SmartDashboard.putNumber("CollectNote/runsInvalid", runsInvalid); // drives the robot forward faster if the object is higher up on the screen, and turns it more based on how far away the object is from x=0 From ab413e5fc3d4fcdf7445111db62bf62efbc105b9 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 21:36:04 -0500 Subject: [PATCH 36/37] adjust auto positions --- .../paths/Blue1AutoPickupFromStart.path | 4 +-- .../pathplanner/paths/Blue2AutoPickup.path | 32 ++++++++++++++----- .../paths/Blue3AutoPickupAmpSide.path | 8 ++--- .../paths/Blue3AutoUnderStageAmp.path | 8 ++--- .../paths/Blue4AutoPickupUnderStage.path | 8 ++--- .../paths/Blue5AutoPickupFromStart.path | 8 ++--- .../pathplanner/paths/BlueAmpShoot1.path | 10 +++--- .../pathplanner/paths/BlueAmpShoot2.path | 10 +++--- .../paths/BlueSourceShoot4UnderStage.path | 10 +++--- 9 files changed, 57 insertions(+), 41 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path index 6e2776fd..45841f1b 100644 --- a/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue1AutoPickupFromStart.path @@ -36,8 +36,8 @@ "y": 7.225738959188476 }, "prevControl": { - "x": 3.9951899595376896, - "y": 7.206262573853738 + "x": 5.212464042958929, + "y": 6.953069564502119 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index 2f4bd3f2..ac990131 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 6.215497887698029, - "y": 6.212966921782006 + "x": 5.475395244977917, + "y": 6.310348848455705 }, "prevControl": null, "nextControl": { - "x": 6.343956445841113, - "y": 6.176454482054112 + "x": 5.603853803121001, + "y": 6.273836408727811 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.897171374413923, - "y": 5.68710451774403 + "x": 6.19602150236329, + "y": 6.281134270453595 }, "prevControl": { - "x": 6.19602150236329, - "y": 5.67736632507666 + "x": 5.942217077535311, + "y": 6.352516764936465 + }, + "nextControl": { + "x": 6.507643667719127, + "y": 6.193490536447265 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.052982457091843, + "y": 5.72605728841351 + }, + "prevControl": { + "x": 6.449214511714907, + "y": 5.618937169072441 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path index 813f53bc..ab529493 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.608696097581994, + "y": 6.144799573110416 }, "prevControl": null, "nextControl": { - "x": 6.7450028218176925, - "y": 5.271930425780205 + "x": 7.04688679450616, + "y": 5.388788737788644 }, "isLocked": false, "linkedName": "SourceSideShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path index 7b298687..59740a4f 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.608696097581994, + "y": 6.144799573110416 }, "prevControl": null, "nextControl": { - "x": 1.3950925173499225, - "y": 2.8922432222088648 + "x": 1.6969764900383901, + "y": 3.009101534217304 }, "isLocked": false, "linkedName": "SourceSideShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path index af7b64ee..a542966a 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.608696097581994, + "y": 6.144799573110416 }, "prevControl": null, "nextControl": { - "x": 3.6446150235123724, - "y": 3.807633332941637 + "x": 3.94649899620084, + "y": 3.924491644950076 }, "isLocked": false, "linkedName": "SourceSideShoot" diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path index 53a4614f..7758b384 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.90084187760309, - "y": 1.071201193410692 + "x": 5.7578028323316435, + "y": 1.4120379367686373 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.5483726341118162 }, "prevControl": { - "x": 6.829004025742336, - "y": 2.2397843134950803 + "x": 6.225236080365399, + "y": 2.142402386821381 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path index c2e9ad97..163a87f0 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.429738126380168, - "y": 7.206262573853738 + "x": 7.9683725678246144, + "y": 7.459455583205354 }, "prevControl": null, "nextControl": { - "x": 4.658054843107622, - "y": 7.492239403441069 + "x": 4.326288510228267, + "y": 7.751601363226451 }, "isLocked": false, "linkedName": null @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.75, + "waypointRelativePos": 0.9, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path index 1e3e883d..cd206d61 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 4.3068121248935265, - "y": 6.027941261101977 + "x": 4.73, + "y": 6.144799573110416 }, "prevControl": { - "x": 3.94649899620084, - "y": 7.751601363226451 + "x": 4.369686871307314, + "y": 7.86845967523489 }, "nextControl": null, "isLocked": false, - "linkedName": "SourceSideShoot" + "linkedName": null } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path index 6750ed46..831841ba 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4UnderStage.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.102310078878758, - "y": 5.72605728841351 + "x": 4.043880922874538, + "y": 6.096108609773567 }, "prevControl": { - "x": 4.014666344872429, - "y": 4.245852002973283 + "x": 3.9172844181987303, + "y": 3.963444415619555 }, "nextControl": null, "isLocked": false, @@ -51,7 +51,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.85, + "waypointRelativePos": 0.8, "command": { "type": "parallel", "data": { From 6cbf980d6a09d73950c3b838baa4b061379ba04e Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Sat, 27 Apr 2024 22:21:05 -0500 Subject: [PATCH 37/37] quick rev down if we are going for a low speed --- src/main/java/frc/robot/RobotContainer.java | 5 +-- .../robot/subsystems/ShooterSubsystem.java | 44 ++++++++++++------- 2 files changed, 29 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5ec38cd..920f11a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -401,10 +401,9 @@ private void configureBindings() { new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter), new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), new InstantCommand(driveTrain::pointWheelsInward, driveTrain), - new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(15), angleShooterSubsystem), - new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)), + new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(50), angleShooterSubsystem), + new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<0.2)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<0.3)), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), - new WaitCommand(0.08), new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), new WaitCommand(0.5), new InstantCommand(()->intake.setNoteHeld(false)) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index edfaeb44..9a03cb0b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -41,8 +41,8 @@ public class ShooterSubsystem extends SubsystemBase { double m_DesiredShooterAngle; double targetVelocityL; double targetVelocityR; - boolean isRevingL; - boolean isRevingR; + int revStateL; + int revStateR; CurrentLimitsConfigs currentLimitConfigs; Slot0Configs PIDLeftconfigs; @@ -63,8 +63,8 @@ public class ShooterSubsystem extends SubsystemBase { /** Creates a new Shooter. */ public ShooterSubsystem(double runSpeed) { if(Preferences.getBoolean("CompBot", true)) { - PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.CompRightkP).withKV(ShooterConstants.CompRightkFF).withKI(0); - PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.CompLeftkP).withKV(ShooterConstants.CompLeftkFF).withKI(0); + PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.CompRightkP).withKV(ShooterConstants.CompRightkFF).withKI(0.004); + PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.CompLeftkP).withKV(ShooterConstants.CompLeftkFF).withKI(0.004); } else { PIDRightconfigs = new Slot0Configs().withKP(ShooterConstants.PracRightkP).withKV(ShooterConstants.PracRightkFF).withKI(0); PIDLeftconfigs = new Slot0Configs().withKP(ShooterConstants.PracLeftkP).withKV(ShooterConstants.PracLeftkFF).withKI(0); @@ -197,25 +197,33 @@ public void turnOff(){ * @param isReving Old isReving * @return Updated isReving */ - private static boolean updateIsReving(boolean isReving, double targetSpeed, double speed){ - double revUpEn = -6; + private static int updateIsReving(int revState, double targetSpeed, double speed){ + double revUpEn = -10; double revUpDis = -2; + double revDownEn = 10; + double revDownDis = 2; targetSpeed = Math.abs(targetSpeed); speed = Math.abs(speed); if (speed < targetSpeed + revUpEn){ - isReving = true; - } else if (speed > targetSpeed + revUpDis){ - isReving = false; - } - return isReving; + revState = 1; + } else if (speed > targetSpeed + revDownEn && targetSpeed<10){ + revState = -1; + } else if((revState == 1 && speed > targetSpeed + revUpDis) || (revState == -1 && speed < targetSpeed + revDownDis)) { + revState = 0; + } + return revState; } /** * If the motor is Reving, set speed to full, if not Reving, use PID Mode */ - private static void updateMotor(TalonFX shooterMotor, boolean revState, double targetSpeed){ + private static void updateMotor(TalonFX shooterMotor, int revState, double targetSpeed, CurrentLimitsConfigs currentLimits){ + currentLimits.StatorCurrentLimitEnable = revState != -1; + shooterMotor.getConfigurator().apply(currentLimits); if(targetSpeed == 0) { shooterMotor.set(0); - } else if (revState){ + } else if (revState == -1) { + shooterMotor.set(-Math.signum(targetSpeed)); + } else if(revState == 1) { shooterMotor.set(Math.signum(targetSpeed)); } else { shooterMotor.setControl(new VelocityDutyCycle(targetSpeed).withSlot(0)); @@ -225,10 +233,10 @@ private static void updateMotor(TalonFX shooterMotor, boolean revState, double t * Revs left and right motors while below target velocity, if above target velocity, the motors are put to PID Mode */ private void updateMotors(){ - isRevingL = updateIsReving(isRevingL, targetVelocityL, shooterL.getVelocity().getValueAsDouble()); - isRevingR = updateIsReving(isRevingR, targetVelocityR, shooterR.getVelocity().getValueAsDouble()); - updateMotor(shooterL, isRevingL, targetVelocityL); - updateMotor(shooterR, isRevingR, targetVelocityR); + revStateL = updateIsReving(revStateL, targetVelocityL, shooterL.getVelocity().getValueAsDouble()); + revStateR = updateIsReving(revStateR, targetVelocityR, shooterR.getVelocity().getValueAsDouble()); + updateMotor(shooterL, revStateL, targetVelocityL, currentLimitConfigs); + updateMotor(shooterR, revStateR, targetVelocityR, currentLimitConfigs); } public double getRSpeed() { return shooterR.getVelocity().getValueAsDouble(); @@ -249,6 +257,8 @@ public void periodic() { SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Right shooter speed", shooterR.getVelocity().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Left shooter speed", shooterL.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Shooter/Left shooter revState", revStateL); + SmartDashboard.putNumber("Shooter/right shooter revState", revStateR); SmartDashboard.putNumber("Shooter/right integral value", shooterR.getClosedLoopIntegratedOutput().getValueAsDouble()); double error = getSignedError(); RobotState.getInstance().ShooterError = error;