diff --git a/src/main/deploy/pathplanner/autos/Left2CoralSpeedy.auto b/src/main/deploy/pathplanner/autos/Left2CoralSpeedy.auto new file mode 100644 index 0000000..ff1ca97 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left2CoralSpeedy.auto @@ -0,0 +1,125 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftCoralPre1" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RAISE_ELEVATOR_L4" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCoral1" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "OUTTAKE_CORAL" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftCoralIntakePre2" + } + }, + { + "type": "named", + "data": { + "name": "LOWER_ELEVATOR" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCoralIntake2" + } + }, + { + "type": "named", + "data": { + "name": "GET_CORAL" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCoralPre2" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RAISE_ELEVATOR_L4" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftCoral2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "OUTTAKE_CORAL" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftCoral2Reverse" + } + }, + { + "type": "named", + "data": { + "name": "LOWER_ELEVATOR" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "LeftAutons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftCoral2Reverse.path b/src/main/deploy/pathplanner/paths/LeftCoral2Reverse.path index 7228c0a..29bb4f6 100644 --- a/src/main/deploy/pathplanner/paths/LeftCoral2Reverse.path +++ b/src/main/deploy/pathplanner/paths/LeftCoral2Reverse.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 3.4038829108808333, - "y": 5.5789732859277015 + "x": 2.8895057362806864, + "y": 5.708562332187294 }, "isLocked": false, "linkedName": "Left Coral Post 2" }, { "anchor": { - "x": 3.408333333333333, - "y": 5.569949494949494 + "x": 1.2535353535353533, + "y": 7.003093434343434 }, "prevControl": { - "x": 3.6808130069872926, - "y": 5.096884123936249 + "x": 1.820257574166762, + "y": 6.543201602037102 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Coral Pre 2" + "linkedName": "Left Source" } ], "rotationTargets": [], @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": -52.2243156940453 }, "reversed": false, "folder": "LeftPaths", diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 50cf43c..7f1913f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -214,7 +214,7 @@ public static final class ElevatorConstants { public static final double kElevatorSensorMaxTrustDistance = 10; public static final double kL1Height = 0.2; - public static final double kL2Height = 3; + public static final double kL2Height = 3.25; public static final double kL3Height = 10.5; public static final double kL4Height = 20; diff --git a/src/main/java/frc/robot/commands/scoring/coral/CoralL3Command.java b/src/main/java/frc/robot/commands/scoring/coral/CoralL3Command.java index 42b6215..4479726 100644 --- a/src/main/java/frc/robot/commands/scoring/coral/CoralL3Command.java +++ b/src/main/java/frc/robot/commands/scoring/coral/CoralL3Command.java @@ -15,7 +15,7 @@ public CoralL3Command(EndEffectorSubsystem endEffector, ElevatorSubsystem elevat new ParallelCommandGroup( new PivotCommand(endEffector, 0.36), new ElevatorNoPivotCommand(ElevatorConstants.kL3Height, elevator)), - new PivotCommand(endEffector, 1.035) + new PivotCommand(endEffector, 0.9) ); } diff --git a/src/main/java/frc/robot/utils/FindNearest.java b/src/main/java/frc/robot/utils/FindNearest.java index d5f5387..3c5e23e 100644 --- a/src/main/java/frc/robot/utils/FindNearest.java +++ b/src/main/java/frc/robot/utils/FindNearest.java @@ -14,18 +14,18 @@ public class FindNearest { // Scoring locations for the blue alliance public static final Pose2d[] blueScoringLocations = { - new Pose2d(new Translation2d(5.759, 3.952), Rotation2d.fromDegrees(180)), - new Pose2d(new Translation2d(5.36, 3.03), Rotation2d.fromDegrees(120)), - new Pose2d(new Translation2d(5.042, 2.88), Rotation2d.fromDegrees(120)), - new Pose2d(new Translation2d(4.035, 2.794), Rotation2d.fromDegrees(60)), - new Pose2d(new Translation2d(3.80, 2.95), Rotation2d.fromDegrees(60)), - new Pose2d(new Translation2d(3.222, 3.738), Rotation2d.fromDegrees(0)), - new Pose2d(new Translation2d(3.218, 4.015), Rotation2d.fromDegrees(0)), - new Pose2d(new Translation2d(3.638, 5.004), Rotation2d.fromDegrees(-60)), - new Pose2d(new Translation2d(3.94, 5.14), Rotation2d.fromDegrees(-60)), - new Pose2d(new Translation2d(4.95, 5.227), Rotation2d.fromDegrees(-120)), - new Pose2d(new Translation2d(5.247, 4.993), Rotation2d.fromDegrees(-120)), - new Pose2d(new Translation2d(5.759, 4.269), Rotation2d.fromDegrees(180)) + new Pose2d(new Translation2d(5.778, 3.861), Rotation2d.fromDegrees(180)), + new Pose2d(new Translation2d(5.276, 2.993), Rotation2d.fromDegrees(120)), + new Pose2d(new Translation2d(4.99, 2.828), Rotation2d.fromDegrees(120)), + new Pose2d(new Translation2d(3.988, 2.828), Rotation2d.fromDegrees(60)), + new Pose2d(new Translation2d(3.702, 2.993), Rotation2d.fromDegrees(60)), + new Pose2d(new Translation2d(3.200, 3.861), Rotation2d.fromDegrees(0)), + new Pose2d(new Translation2d(3.200, 4.191), Rotation2d.fromDegrees(0)), + new Pose2d(new Translation2d(3.702, 5.059), Rotation2d.fromDegrees(-60)), + new Pose2d(new Translation2d(3.988, 5.224), Rotation2d.fromDegrees(-60)), + new Pose2d(new Translation2d(4.99, 5.224), Rotation2d.fromDegrees(-120)), + new Pose2d(new Translation2d(5.276, 5.059), Rotation2d.fromDegrees(-120)), + new Pose2d(new Translation2d(5.778, 4.191), Rotation2d.fromDegrees(180)) }; // Scoring locations for the red alliance