diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 888387c..f341205 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -7,6 +7,7 @@ "GKC Amp", "GKC Source", "Middle Race", + "Source_5-4", "amp_side", "lame", "source_side", diff --git a/src/main/deploy/pathplanner/autos/Middle Race Cleanup.auto b/src/main/deploy/pathplanner/autos/Middle Race Cleanup.auto index d930f0e..6bbea32 100644 --- a/src/main/deploy/pathplanner/autos/Middle Race Cleanup.auto +++ b/src/main/deploy/pathplanner/autos/Middle Race Cleanup.auto @@ -5,6 +5,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "ShootSubwooferFirstHalf" + } + }, { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/autos/Source Race 5-4.auto b/src/main/deploy/pathplanner/autos/Source Race 5-4.auto new file mode 100644 index 0000000..d63a251 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source Race 5-4.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.39, + "y": 1.57 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source_5-4_a" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Source_5-4_b" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "PathFindToSourceShot" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Source_5-4_c" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "PathFindToSourceShot" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + }, + "folder": "IRI", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Race 2 Success.path b/src/main/deploy/pathplanner/paths/Middle Race 2 Success.path index dae7f1b..ac1cd9b 100644 --- a/src/main/deploy/pathplanner/paths/Middle Race 2 Success.path +++ b/src/main/deploy/pathplanner/paths/Middle Race 2 Success.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.534074173710932, - "y": 4.508819045102521 + "x": 4.6120192584713875, + "y": 4.728852252843395 }, "isLocked": false, "linkedName": "middle_race_1_end" @@ -20,12 +20,12 @@ "y": 5.545689939904186 }, "prevControl": { - "x": 3.7794417368188835, - "y": 5.079999454927962 + "x": 3.8341330839924415, + "y": 5.357907257770535 }, "nextControl": { - "x": 2.2307833206823586, - "y": 5.894957803636354 + "x": 2.159036728303594, + "y": 5.692667822170061 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Source_5-4_a.path b/src/main/deploy/pathplanner/paths/Source_5-4_a.path new file mode 100644 index 0000000..68cefbc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source_5-4_a.path @@ -0,0 +1,185 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.39, + "y": 1.57 + }, + "prevControl": null, + "nextControl": { + "x": 2.3559258262890683, + "y": 1.3111809548974793 + }, + "isLocked": false, + "linkedName": "source_race_start" + }, + { + "anchor": { + "x": 7.9, + "y": 0.75 + }, + "prevControl": { + "x": 6.9, + "y": 0.75 + }, + "nextControl": { + "x": 8.9, + "y": 0.75 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.294906624680149, + "y": 1.75 + }, + "prevControl": { + "x": 8.294906624680149, + "y": 1.0696015291969474 + }, + "nextControl": { + "x": 8.294906624680149, + "y": 3.25 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.337328917134686, + "y": 1.8947383787418037 + }, + "prevControl": { + "x": 5.997467039336302, + "y": 1.6544677517560307 + }, + "nextControl": { + "x": 4.6325594515452515, + "y": 2.1512534862360564 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5745727825005997, + "y": 2.9853194908936116 + }, + "prevControl": { + "x": 4.393724826789591, + "y": 2.411743054542565 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "source_5-4_a_end" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -35.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 90.0, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": -21.497331108184007, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Slow Poop 5", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.65, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Poop Prep", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + } + ] + } + } + }, + { + "name": "Poop Preload", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + } + ] + } + } + }, + { + "name": "Intake 4", + "waypointRelativePos": 1.65, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.626989153286004, + "rotateFast": false + }, + "reversed": false, + "folder": "Source_5-4", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source_5-4_b.path b/src/main/deploy/pathplanner/paths/Source_5-4_b.path new file mode 100644 index 0000000..4c91bd4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source_5-4_b.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5745727825005997, + "y": 2.9853194908936116 + }, + "prevControl": null, + "nextControl": { + "x": 4.336325427714513, + "y": 2.2644766509151077 + }, + "isLocked": false, + "linkedName": "source_5-4_a_end" + }, + { + "anchor": { + "x": 5.1371265490650195, + "y": 1.8184062559531378 + }, + "prevControl": { + "x": 4.294084321457229, + "y": 2.3051369130007173 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -30.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source_5-4", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source_5-4_c.path b/src/main/deploy/pathplanner/paths/Source_5-4_c.path new file mode 100644 index 0000000..8a9fd3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source_5-4_c.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5745727825005997, + "y": 2.9853194908936116 + }, + "prevControl": null, + "nextControl": { + "x": 2.2121480814142185, + "y": 3.117461794457733 + }, + "isLocked": false, + "linkedName": "source_5-4_a_end" + }, + { + "anchor": { + "x": 2.346802158592743, + "y": 1.8885030190488628 + }, + "prevControl": { + "x": 1.3997681661993824, + "y": 2.414172605471947 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -28.58825946325851, + "rotateFast": false + }, + "reversed": false, + "folder": "Source_5-4", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ 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 253f1f7..89d9923 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -88,13 +89,13 @@ public class RobotContainer { public final ShuffleboardTab SHOOTER_TAB = Shuffleboard.getTab("SHOOTER"); public final ShuffleboardTab PROTO_TAB = Shuffleboard.getTab("PROTO"); public static final Vision INTAKE_PHOTON = new Vision("Arducam_OV9782_USB_Camera", 0.651830, 60); - public final CommandSwerveDrivetrain DRIVETRAIN = DriveConstants.DriveTrain; // My drivetrain + public static final CommandSwerveDrivetrain DRIVETRAIN = DriveConstants.DriveTrain; // My drivetrain public final Candles CANDLES = new Candles(Constants.LEFT_CANDLE, Constants.RIGHT_CANDLE); public final Intake INTAKE = new Intake(Constants.INTAKE_TOP_ID, Constants.INTAKE_BOTTOM_ID); public final Shooter SHOOTER = new Shooter( Constants.SHOOTER_LEADER_ID, Constants.SHOOTER_FOLLOWER_ID, Constants.SHOOTER_FEEDER_ID); - public final Wrist WRIST = new Wrist(Constants.WRIST_ID); + public static final Wrist WRIST = new Wrist(Constants.WRIST_ID); public final Elevator ELEVATOR = new Elevator(Constants.ELEVATOR_LEADER_ID, Constants.ELEVATOR_FOLLOWER_ID); public final AmpSensors AMP_SENSORS = new AmpSensors(); @@ -527,6 +528,7 @@ public void configureShuffleboard() { // addAuto("AGKC-Amp-1-2Red"); // addAuto("GKC-Amp-Skip-1-2"); addAuto("Middle Race Cleanup"); + addAuto("Source Race 5-4"); addAuto("lame"); // AUTO_CHOOSER.addOption( // "Middle Race Cleanup", @@ -686,6 +688,8 @@ public void configureNamedCommands() { NamedCommands.registerCommand("AutoAimAndShoot", new AutoAimAndShoot(DRIVETRAIN, SHOOTER)); NamedCommands.registerCommand("EnableWristLockDown", new InstantCommand(() -> WRIST.enableWristLockdown())); NamedCommands.registerCommand("DisableWristLockDown", new InstantCommand(() -> WRIST.disableWristLockdown())); + NamedCommands.registerCommand("PathFindToSourceShot", Util.PathFindToAutoSourceShot()); + NamedCommands.registerCommand("ShootSubwooferFirstHalf", new ShootSubwooferFirstHalf(ELEVATOR, WRIST, SHOOTER)); } public void addAuto(String autoName) { @@ -699,8 +703,7 @@ public void addAuto(String autoName) { AUTO_CHOOSER.addOption(autoName, new ShootSubwoofer(ELEVATOR, WRIST, SHOOTER).andThen(auto)); return; } else if (autoName == "GKC Source 5-4-3" - || autoName == "GKC Source 4-3-2" - || autoName == "Middle Race Cleanup") { + || autoName == "GKC Source 4-3-2") { AUTO_CHOOSER.addOption( autoName, new ShootSubwooferFirstHalf(ELEVATOR, WRIST, SHOOTER).andThen(auto)); return; diff --git a/src/main/java/frc/robot/commands/control/Climb.java b/src/main/java/frc/robot/commands/control/Climb.java index d7350b9..e169c2d 100644 --- a/src/main/java/frc/robot/commands/control/Climb.java +++ b/src/main/java/frc/robot/commands/control/Climb.java @@ -26,10 +26,10 @@ public Climb(Elevator Elevator, Wrist Wrist, Shooter Shooter) { new InstantCommand( () -> Elevator.setLengthInchesSlot1(Constants.Climb.CLIMB_PULLDOWN_HEIGHT)), new WaitUntilCommand(Elevator::isAtSetpoint), - new WaitCommand(0.9), + new WaitCommand(0.6), // 0.9 new InstantCommand(() -> Elevator.setLengthInches(Constants.Climb.CLIMB_LEVEL_HEIGHT)), new WaitUntilCommand(Elevator::isAtSetpoint), - new WaitCommand(2.0), + new WaitCommand(0.6), // 2.0 new InstantCommand(Wrist::goHome, Wrist), new InstantCommand(Wrist::stop, Wrist), new InstantCommand(() -> Wrist.setDegrees(Constants.Wrist.INITIAL_ANGLE_DEGREES), Wrist), diff --git a/src/main/java/frc/robot/commands/control/auto/AutoCollectNote.java b/src/main/java/frc/robot/commands/control/auto/AutoCollectNote.java index 9aa72a8..598a45e 100644 --- a/src/main/java/frc/robot/commands/control/auto/AutoCollectNote.java +++ b/src/main/java/frc/robot/commands/control/auto/AutoCollectNote.java @@ -4,7 +4,10 @@ package frc.robot.commands.control.auto; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotContainer; import frc.robot.commands.control.note.IntakeNoteSequenceAuto; import frc.robot.commands.movement.DriveToNote2; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -29,6 +32,11 @@ public AutoCollectNote( // addCommands(). super(new IntakeNoteSequenceAuto(shooter, intake, elevator)); // addCommands(new FooCommand(), new BarCommand()); - addCommands(new DriveToNote2(drivetrain, vision, initialVelocity)); + addCommands( + new InstantCommand(() -> RobotContainer.WRIST.enableWristLockdown()) + .andThen(new DriveToNote2(drivetrain, vision, initialVelocity)) + .finallyDo(() -> RobotContainer.WRIST.disableWristLockdown()) + ); + // addCommands(new DriveToNote2(drivetrain, vision, initialVelocity)); } } diff --git a/src/main/java/frc/robot/commands/movement/DriveToNote2.java b/src/main/java/frc/robot/commands/movement/DriveToNote2.java index 1bed4b2..f78cb95 100644 --- a/src/main/java/frc/robot/commands/movement/DriveToNote2.java +++ b/src/main/java/frc/robot/commands/movement/DriveToNote2.java @@ -20,7 +20,7 @@ public class DriveToNote2 extends PIDCommand { private static final SwerveRequest.ApplyChassisSpeeds swerveRequest = new SwerveRequest.ApplyChassisSpeeds(); - private static final Debouncer canSeeNoteDebouncer = new Debouncer(0.8); + private static final Debouncer canSeeNoteDebouncer = new Debouncer(0.6, DebounceType.kFalling); /** Creates a new DriveToNote2. */ public DriveToNote2( diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index d142f5e..e70474b 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -64,7 +64,7 @@ public static class Trap { public static final double TRAP_WRIST_DEGREES = 119.0; public static final double TRAP_WRIST_DEGREES_MIDWAY = 85.0; public static final double TRAP_RPM_SPEED = - 375; // suggesting but not doing 350 at GKC // 425, 475 worked but touched the top + 225; //375; // suggesting but not doing 350 at GKC // 425, 475 worked but touched the top } public static class Wrist { diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java index 9be9e19..0b0c8c2 100644 --- a/src/main/java/frc/robot/util/Util.java +++ b/src/main/java/frc/robot/util/Util.java @@ -9,6 +9,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; @@ -20,7 +22,9 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -98,6 +102,16 @@ public static Rotation2d getRotationToAllianceLob(Pose2d opose) { // : new Rotation2d(Math.atan2(pose.getY(), pose.getX()) - 90.0); } + public static final Pose2d BLUE_AUTO_SOURCE_SHOOTING_POSE = new Pose2d(3.57, 2.99, Rotation2d.fromDegrees(-35.0)); + public static final Pose2d RED_AUTO_SOURCE_SHOOTING_POSE = new Pose2d(13.02, 2.99, Rotation2d.fromDegrees(-145.0)); + public static Command PathFindToAutoSourceShot() { + return new ConditionalCommand( + Util.pathfindToPose(BLUE_AUTO_SOURCE_SHOOTING_POSE), + Util.pathfindToPose(RED_AUTO_SOURCE_SHOOTING_POSE), + () -> RobotContainer.DRIVETRAIN.getAlliance().equals(Alliance.Blue) + ); + } + public static boolean isWithinTolerance( double currentValue, double targetValue, double tolerance) { return Math.abs(currentValue - targetValue) <= tolerance;