diff --git a/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto b/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto index 67047d61..573b952c 100644 --- a/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto +++ b/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7523718013035084, - "y": 6.641447399146282 + "x": 1.3070140425995143, + "y": 5.673210203563265 }, "rotation": -120.96375653207352 }, @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "CloseLeftPickup" + "pathName": "CloseAmpPickup" } }, { @@ -44,7 +44,7 @@ { "type": "path", "data": { - "pathName": "CloseMiddlePickupFromLeft" + "pathName": "CloseMiddlePickupFromAmp" } }, { @@ -56,7 +56,7 @@ { "type": "path", "data": { - "pathName": "CloseRightPickupFromMiddle" + "pathName": "CloseSourcePickupFromMiddle" } } ] diff --git a/src/main/deploy/pathplanner/autos/StartMidScore3Close.auto b/src/main/deploy/pathplanner/autos/StartMidScore3Close.auto new file mode 100644 index 00000000..e232ff8e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/StartMidScore3Close.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7939330164374621, + "y": 4.441815740774339 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "initialShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.25 + } + }, + { + "type": "named", + "data": { + "name": "conditionlIndexer" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseAmpPickup" + } + }, + { + "type": "named", + "data": { + "name": "conditionlIndexer" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseMiddlePickupFromAmp" + } + }, + { + "type": "named", + "data": { + "name": "conditionlIndexer" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseSourcePickupFromMiddle" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/StartMRight3.auto b/src/main/deploy/pathplanner/autos/StartMidScore3FarSource.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/StartMRight3.auto rename to src/main/deploy/pathplanner/autos/StartMidScore3FarSource.auto diff --git a/src/main/deploy/pathplanner/autos/StartSourceScore3Close.auto b/src/main/deploy/pathplanner/autos/StartSourceScore3Close.auto new file mode 100644 index 00000000..a62c31ec --- /dev/null +++ b/src/main/deploy/pathplanner/autos/StartSourceScore3Close.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6766573533147072, + "y": 4.41249682499365 + }, + "rotation": 121.42956561483847 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "initialShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.25 + } + }, + { + "type": "named", + "data": { + "name": "conditionlIndexer" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseSourcePickup" + } + }, + { + "type": "named", + "data": { + "name": "conditionlIndexer" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseMiddlePickupFromSource" + } + }, + { + "type": "named", + "data": { + "name": "conditionlIndexer" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseAmpPickup" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSideFar3.auto b/src/main/deploy/pathplanner/autos/StartSourceScore3FarSource.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/SourceSideFar3.auto rename to src/main/deploy/pathplanner/autos/StartSourceScore3FarSource.auto diff --git a/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path b/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path index 8e4ffe3f..83adfc93 100644 --- a/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 8.665673221827673, - "y": 7.19589929449488 + "x": 8.240937624732393, + "y": 5.731848035124642 }, "isLocked": false, "linkedName": "SourceSideShoot" }, { "anchor": { - "x": 8.006386298486884, - "y": 4.485794114445372 + "x": 8.29957545629377, + "y": 4.089988751406074 }, "prevControl": { - "x": 7.4979276239118775, - "y": 5.280838587417197 + "x": 7.449326898653798, + "y": 4.764323814361915 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1, + "waypointRelativePos": 0.95, "rotationDegrees": 127.97, "rotateFast": true } diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path index 1b0e05d2..48ef5db0 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.196658160813356, - "y": 3.496011167585799 + "x": 2.4064733843753405, + "y": 3.1957618200950693 }, "isLocked": false, "linkedName": "SourceSideShoot" }, { "anchor": { - "x": 7.311335876233006, - "y": 3.0521743094768246 + "x": 7.595921477557241, + "y": 3.049167241191626 }, "prevControl": { - "x": 6.48105091081977, - "y": 4.02278912819934 + "x": 6.27657026742625, + "y": 4.64704815123916 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1, + "waypointRelativePos": 1.0, "rotationDegrees": 145.3, "rotateFast": true } diff --git a/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path index dcbd1494..c4277e87 100644 --- a/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.912961158238873, - "y": 0.24326353075685425 + "x": 7.346710693421388, + "y": 1.7444754889509784 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" }, { "anchor": { - "x": 7.8597917195834395, - "y": 2.2575565151130306 + "x": 8.270256540513081, + "y": 2.4041510940164743 }, "prevControl": { - "x": 6.657716172575203, - "y": 1.7884538626220112 + "x": 7.0681809935048445, + "y": 1.935048441525455 }, "nextControl": null, "isLocked": false, @@ -30,8 +30,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1, - "rotationDegrees": -157.01, + "waypointRelativePos": 0.35, + "rotationDegrees": -153.3479005035105, + "rotateFast": true + }, + { + "waypointRelativePos": 0.7, + "rotationDegrees": -159.58424098764888, "rotateFast": true } ], diff --git a/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path index 7a9e01f0..0b73d2b5 100644 --- a/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.713197140679996, - "y": 0.8649080155303174 + "x": 8.255597082622737, + "y": 0.7769512681882514 }, "prevControl": { - "x": 6.320548641097282, - "y": 1.2313944627889262 + "x": 6.862948583040024, + "y": 1.1434377154468605 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/CloseLeftPickup.path b/src/main/deploy/pathplanner/paths/CloseAmpPickup.path similarity index 90% rename from src/main/deploy/pathplanner/paths/CloseLeftPickup.path rename to src/main/deploy/pathplanner/paths/CloseAmpPickup.path index 21d276fb..ca0e6463 100644 --- a/src/main/deploy/pathplanner/paths/CloseLeftPickup.path +++ b/src/main/deploy/pathplanner/paths/CloseAmpPickup.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.6117057948372984, - "y": 6.7433506295548105 + "x": 2.890235494756704, + "y": 6.992561413694256 }, "prevControl": { - "x": 2.391813926482133, - "y": 6.684712797993434 + "x": 2.670343626401539, + "y": 6.93392358213288 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path b/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromAmp.path similarity index 100% rename from src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path rename to src/main/deploy/pathplanner/paths/CloseMiddlePickupFromAmp.path diff --git a/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromSource.path b/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromSource.path new file mode 100644 index 00000000..ca85fca1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromSource.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0546463950070764, + "y": 4.485794114445372 + }, + "prevControl": null, + "nextControl": { + "x": 1.6881599477484674, + "y": 5.336042672085345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8947741881248894, + "y": 5.531293435066112 + }, + "prevControl": { + "x": 1.9306931140552677, + "y": 5.404696930390302 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -175.78655513511495, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.37126181017194, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 154.29004621918864, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CloseSourcePickup.path b/src/main/deploy/pathplanner/paths/CloseSourcePickup.path new file mode 100644 index 00000000..9e537e4e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseSourcePickup.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.7467977793098448, + "y": 4.441815740774339 + }, + "prevControl": null, + "nextControl": { + "x": 2.171922058129831, + "y": 4.104648209296419 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.53840850538844, + "y": 4.133967125077108 + }, + "prevControl": { + "x": 2.318516637033275, + "y": 4.075329293515732 + }, + "nextControl": { + "x": 2.758300373743605, + "y": 4.192604956638484 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.333176094923619, + "y": 4.119307667186764 + }, + "prevControl": { + "x": 2.2782031278348276, + "y": 4.104648209296419 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 152.65012421993018, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 142.63754619139377, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path b/src/main/deploy/pathplanner/paths/CloseSourcePickupFromMiddle.path similarity index 100% rename from src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path rename to src/main/deploy/pathplanner/paths/CloseSourcePickupFromMiddle.path diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 17414c6f..d849b7a2 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -438,7 +438,7 @@ public final class Vision{ public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural"; public static final double APRILTAG_CLOSENESS = 0.5; - public static final double MAX_TAG_DISTANCE = 3.05; + public static final double MAX_TAG_DISTANCE = 2.3; public static final Translation2d fieldCorner = new Translation2d(16.54, 8.02); @@ -449,6 +449,13 @@ public final class Vision{ public static final double K_DETECTOR_TY_P = 0.1; public static final double K_DETECTOR_TY_I = 0; public static final double K_DETECTOR_TY_D = 0; + + // how many degrees back is your limelight rotated from perfectly vertical? + public static final double limelightMountAngleDegrees = 22.0; + // distance from the center of the Limelight lens to the floor + public static final double limelightLensHeightInches = 0.233; + //height of april tags from the floor + public static final double AprilTagHeight = 1.335; } public final class PathConstants{ //Welcome, to Pathconstantic Park diff --git a/src/main/java/frc/robot/settings/LimelightValues.java b/src/main/java/frc/robot/settings/LimelightValues.java index 85025e41..7d59d765 100644 --- a/src/main/java/frc/robot/settings/LimelightValues.java +++ b/src/main/java/frc/robot/settings/LimelightValues.java @@ -11,8 +11,13 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.LimelightHelpers; import frc.robot.LimelightHelpers.Results; + +import frc.robot.settings.Constants.Vision; import static frc.robot.settings.Constants.Vision.MAX_TAG_DISTANCE; import static frc.robot.settings.Constants.Vision.APRILTAG_CLOSENESS; +import static frc.robot.settings.Constants.Vision.limelightLensHeightInches; +import static frc.robot.settings.Constants.Vision.limelightMountAngleDegrees; +import static frc.robot.settings.Constants.Vision.AprilTagHeight; /** Add your docs here. */ public class LimelightValues { @@ -31,13 +36,14 @@ public class LimelightValues { public LimelightValues(Results llresults, boolean valid){ this.llresults = llresults; this.isResultValid = valid; + this.tagDistance = 30; if (isResultValid) { this.numTags = llresults.targets_Fiducials.length; for (int i = 0; i < numTags; i++) { this.tx[i] = llresults.targets_Fiducials[i].tx; this.ty[i] = llresults.targets_Fiducials[i].ty; this.ta[i] = llresults.targets_Fiducials[i].ta; - this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); + // this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm(); } this.botPoseRed = llresults.getBotPose2d_wpiRed(); this.botPoseBlue = llresults.getBotPose2d_wpiBlue(); @@ -60,14 +66,25 @@ public Pose2d getbotPose(){ public Pose2d getBotPoseBlue() { return botPoseBlue; } - public double getTagDistance() { - return tagDistance; + public double calculateTagDistance(String limelightName) { + if(isResultValid) { + double targetOffsetAngle_Vertical = NetworkTableInstance.getDefault().getTable(limelightName).getEntry("ty").getDouble(0.0); + + double angleToGoalRadians = Math.toRadians(limelightMountAngleDegrees + targetOffsetAngle_Vertical); + //calculate distance + double distanceFromLimelightToGoalInches = (Vision.AprilTagHeight - Vision.limelightLensHeightInches) / Math.tan(angleToGoalRadians); + return distanceFromLimelightToGoalInches; + } else { + return 30; + } } - public boolean isPoseTrustworthy(Pose2d robotPose){ + // public double getTagDistance() { + // return tagDistance; + // } + public boolean isPoseTrustworthy(double tagDist){ Pose2d poseEstimate = this.botPoseBlue; if ((poseEstimate.getX()