diff --git a/src/main/deploy/pathplanner/paths/A to E.path b/src/main/deploy/pathplanner/paths/A to E.path index 32e0f73c..bd9dae7c 100644 --- a/src/main/deploy/pathplanner/paths/A to E.path +++ b/src/main/deploy/pathplanner/paths/A to E.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.17, + "x": 8.07, "y": 5.84 }, "prevControl": { - "x": 7.01740860685425, + "x": 6.91740860685425, "y": 6.462229512977543 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/D to Ferry Shot.path b/src/main/deploy/pathplanner/paths/D to Ferry Shot.path index 6115c3d7..3fdde5f7 100644 --- a/src/main/deploy/pathplanner/paths/D to Ferry Shot.path +++ b/src/main/deploy/pathplanner/paths/D to Ferry Shot.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.0, + "x": 7.7, "y": 7.529999999999999 }, "prevControl": null, "nextControl": { - "x": 7.0823974120170465, + "x": 6.782397412017047, "y": 7.253301738161635 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/E to Ferry Shot.path b/src/main/deploy/pathplanner/paths/E to Ferry Shot.path index f1ee5c0a..aebabacf 100644 --- a/src/main/deploy/pathplanner/paths/E to Ferry Shot.path +++ b/src/main/deploy/pathplanner/paths/E to Ferry Shot.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 5.3, + "x": 5.15, "y": 6.563657142562062 }, "prevControl": { - "x": 5.567533940069292, + "x": 5.417533940069292, "y": 6.480854711709423 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index 14bfc8d3..d23db54f 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.17, + "x": 8.07, "y": 5.84 }, "prevControl": null, "nextControl": { - "x": 6.661102338686158, + "x": 6.561102338686158, "y": 6.648577691229455 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path index c33ca1af..daf28676 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 4.984199142187458, - "y": 5.437781815734572 + "y": 5.437781815734571 }, "isLocked": false, "linkedName": "F" diff --git a/src/main/deploy/pathplanner/paths/Ferry Shot to F.path b/src/main/deploy/pathplanner/paths/Ferry Shot to F.path index 31c3b520..8f0716cb 100644 --- a/src/main/deploy/pathplanner/paths/Ferry Shot to F.path +++ b/src/main/deploy/pathplanner/paths/Ferry Shot to F.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 5.3, + "x": 5.15, "y": 6.563657142562062 }, "prevControl": null, "nextControl": { - "x": 6.5806314292272345, + "x": 6.430631429227235, "y": 6.476519652113379 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/GShoot to F (HGF).path b/src/main/deploy/pathplanner/paths/GShoot to F (HGF).path index 24d5e4f3..21359936 100644 --- a/src/main/deploy/pathplanner/paths/GShoot to F (HGF).path +++ b/src/main/deploy/pathplanner/paths/GShoot to F (HGF).path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 5.819658184786454, - "y": 5.066378211460127 + "y": 5.066378211460126 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/NTF D To E.path b/src/main/deploy/pathplanner/paths/NTF D To E.path index d2872017..6623f9e8 100644 --- a/src/main/deploy/pathplanner/paths/NTF D To E.path +++ b/src/main/deploy/pathplanner/paths/NTF D To E.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 8.17, + "x": 8.07, "y": 5.84 }, "prevControl": { - "x": 6.920699524719652, + "x": 6.820699524719652, "y": 5.881812946132679 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/NTF E To F.path b/src/main/deploy/pathplanner/paths/NTF E To F.path index 03cb01d0..1c4e909d 100644 --- a/src/main/deploy/pathplanner/paths/NTF E To F.path +++ b/src/main/deploy/pathplanner/paths/NTF E To F.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.17, + "x": 8.07, "y": 5.84 }, "prevControl": null, "nextControl": { - "x": 6.621010550777611, + "x": 6.5210105507776115, "y": 6.341337932241838 }, "isLocked": false, @@ -37,7 +37,7 @@ }, "prevControl": { "x": 7.424255376791419, - "y": 4.810087379302906 + "y": 4.810087379302905 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/NTF Start To D.path b/src/main/deploy/pathplanner/paths/NTF Start To D.path index 3d1bd439..8c7f0df7 100644 --- a/src/main/deploy/pathplanner/paths/NTF Start To D.path +++ b/src/main/deploy/pathplanner/paths/NTF Start To D.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 8.0, + "x": 7.7, "y": 7.529999999999999 }, "prevControl": { - "x": 6.222230165687515, + "x": 5.922230165687515, "y": 7.685534706860702 }, "nextControl": null, @@ -67,7 +67,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, - "maxAcceleration": 4.0, + "maxAcceleration": 3.8, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -82,5 +82,5 @@ "rotation": 47.48005904226506, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rerouted D To E.path b/src/main/deploy/pathplanner/paths/Rerouted D To E.path new file mode 100644 index 00000000..da528cfb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rerouted D To E.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.2, + "y": 7.431149973519566 + }, + "prevControl": null, + "nextControl": { + "x": 8.2, + "y": 6.931149973519566 + }, + "isLocked": false, + "linkedName": "D" + }, + { + "anchor": { + "x": 8.07, + "y": 5.84 + }, + "prevControl": { + "x": 8.07, + "y": 6.34 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "E" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0, + "rotateFast": true + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rerouted E To D.path b/src/main/deploy/pathplanner/paths/Rerouted E To D.path new file mode 100644 index 00000000..698a3580 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rerouted E To D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.07, + "y": 5.84 + }, + "prevControl": null, + "nextControl": { + "x": 8.076569848142244, + "y": 6.3941167539916925 + }, + "isLocked": false, + "linkedName": "E" + }, + { + "anchor": { + "x": 8.2, + "y": 7.431149973519566 + }, + "prevControl": { + "x": 8.199991521233805, + "y": 6.827417706185608 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.13644188312996, + "rotateFast": true + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rerouted E To F.path b/src/main/deploy/pathplanner/paths/Rerouted E To F.path index ef77ff23..eb68e41d 100644 --- a/src/main/deploy/pathplanner/paths/Rerouted E To F.path +++ b/src/main/deploy/pathplanner/paths/Rerouted E To F.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.17, + "x": 8.07, "y": 5.84 }, "prevControl": null, "nextControl": { - "x": 8.17, - "y": 4.84 + "x": 8.07, + "y": 5.34 }, "isLocked": false, "linkedName": "E" @@ -21,7 +21,7 @@ }, "prevControl": { "x": 8.153314996088682, - "y": 4.800600193792981 + "y": 4.300600193792981 }, "nextControl": null, "isLocked": false, @@ -43,7 +43,7 @@ "rotateFast": true }, "reversed": false, - "folder": "ABCDE", + "folder": "Top Ferry", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Rerouted E to Ferry Shot.path b/src/main/deploy/pathplanner/paths/Rerouted E to Ferry Shot.path new file mode 100644 index 00000000..aebabacf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rerouted E to Ferry Shot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.37, + "y": 5.34 + }, + "prevControl": null, + "nextControl": { + "x": 8.101123246690936, + "y": 5.352924298441191 + }, + "isLocked": false, + "linkedName": "E Top Ferry" + }, + { + "anchor": { + "x": 5.15, + "y": 6.563657142562062 + }, + "prevControl": { + "x": 5.417533940069292, + "y": 6.480854711709423 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ferry Shot E" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -4.860211087167981, + "rotateFast": false + }, + "reversed": false, + "folder": "Top Ferry", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rerouted Ferry Shot to F.path b/src/main/deploy/pathplanner/paths/Rerouted Ferry Shot to F.path new file mode 100644 index 00000000..8f0716cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rerouted Ferry Shot to F.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.15, + "y": 6.563657142562062 + }, + "prevControl": null, + "nextControl": { + "x": 6.430631429227235, + "y": 6.476519652113379 + }, + "isLocked": false, + "linkedName": "Ferry Shot E" + }, + { + "anchor": { + "x": 8.16, + "y": 3.993395262111494 + }, + "prevControl": { + "x": 8.144797599224828, + "y": 3.9781928613363213 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F Top Ferry" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -47.12109639666155, + "rotateFast": false + }, + "reversed": false, + "folder": "Top Ferry", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to E.path b/src/main/deploy/pathplanner/paths/Shoot to E.path index 51e904f4..849f967c 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/Shoot to E.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.17, + "x": 8.07, "y": 5.84 }, "prevControl": { - "x": 6.389445717754282, + "x": 6.289445717754282, "y": 6.231006694375947 }, "nextControl": null, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 50300101..654e6e04 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -113,16 +113,6 @@ private void configureButtonBindings() { } private void configureDriverBindings() { - // intaking with swerve pointing at note - // driver.getRightTriggerButton() - // .whileTrue(new WaitCommand(Settings.Intake.TELEOP_DRIVE_STARTUP_DELAY) - // .andThen(new IntakeAcquire() - // .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE))) - // .andThen(new BuzzController(driver) - // .alongWith(new LEDSet(LEDInstructions.PICKUP) - // .withTimeout(3.0)))) - // .whileTrue(new SwerveDriveToNote()); - // intaking driver.getRightTriggerButton() .whileTrue(new IntakeAcquire() @@ -147,8 +137,13 @@ private void configureDriverBindings() { // then shoot driver.getRightBumper() .onTrue(new ShooterPodiumShot()) - .whileTrue(new SwerveDriveToShootWithoutStopping() - .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN))); + .whileTrue(new SwerveDriveToShoot() + .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN) + .andThen(new ShooterWaitForTarget() + .withTimeout(0.5))) + .andThen(new ConveyorShoot())) + .onFalse(new ConveyorStop()) + .onFalse(new IntakeStop()); // note to amper and align then score driver.getLeftBumper() @@ -327,9 +322,9 @@ public void configureAutons() { "Preload to C", "C to B Red", "B to A Red","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); AutonConfig ReroutableCBAED = new AutonConfig("5 CBAE", ReroutableSixPieceCBAED::new, - "Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot", "F to Shoot (HGF)", "Rerouted E To F"); + "Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot", "Rerouted E To D"); AutonConfig ReroutableCBAED_RED = new AutonConfig("5 CBAE", ReroutableSixPieceCBAED::new, - "Preload to C", "C to B Red", "B to A Red","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot", "F to Shoot (HGF)", "Rerouted E To F"); + "Preload to C", "C to B Red", "B to A Red","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot", "Rerouted E To D"); AutonConfig CBA = new AutonConfig("4 CBA", FourPieceCBA::new, "Preload to C", "C to B", "B to A"); @@ -345,8 +340,15 @@ public void configureAutons() { AutonConfig TopFerry = new AutonConfig("Top Ferry", TopFerry::new, "NTF Start To D", "D to Ferry Shot", "Ferry Shot to E", "E to Ferry Shot", "Ferry Shot to F", "F to Shoot (TopFerry)"); + AutonConfig ReroutableTopFerry = new AutonConfig("Top Ferry", ReroutableTopFerry::new, + "NTF Start To D", "D to Ferry Shot", "Ferry Shot to E", "E to Ferry Shot", "Ferry Shot to F", "F to Shoot (TopFerry)", "Rerouted D To E", "Rerouted E To F", "Rerouted E to Ferry Shot"); + // TODO: automatically choose red/blue - TopFerry + // TopFerry + // .registerBlue(autonChooser) + // .registerRed(autonChooser); + + ReroutableTopFerry .registerBlue(autonChooser) .registerRed(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java index b4bce321..5031ee1a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/ReroutableSixPieceCBAED.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.auton.CBADE; import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.DoNothingCommand; import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; @@ -16,10 +17,10 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; public class ReroutableSixPieceCBAED extends SequentialCommandGroup { - + public ReroutableSixPieceCBAED(PathPlannerPath... paths) { - PathPlannerPath E_TO_F = paths[8]; + PathPlannerPath E_TO_D = paths[7]; addCommands( new ParallelCommandGroup( @@ -30,36 +31,54 @@ public ReroutableSixPieceCBAED(PathPlannerPath... paths) { .withTolerance(0.03, 0.03, 3) ), + // shoot preload new ConveyorShootRoutine(0.55), + // intake C, shoot C new FollowPathAndIntake(paths[0]), new SwerveDriveToShoot() .withTolerance(0.03, 7), new ConveyorShootRoutine(), + // intake B, shoot B new FollowPathAndIntake(paths[1]), new SwerveDriveToShoot() .withTolerance(0.05, 7), new ConveyorShootRoutine(), + // intake A, shoot A new FollowPathAndIntake(paths[2]), new SwerveDriveToShoot() .withTolerance(0.05, 5), new ConveyorShootRoutine(0.6), + // intake E new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot() - .withTolerance(0.033, 7)), - - new FollowPathAndIntake(paths[5]), + new ConditionalCommand( - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot() - .withTolerance(0.033, 7)), + // shoot E, intake D, shoot D new SequentialCommandGroup( - new FollowPathAndIntake(E_TO_F), - new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot() - .withTolerance(0.033, 7)) - ), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot() + .withTolerance(0.033, 7)), + + new FollowPathAndIntake(paths[5]), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot() + .withTolerance(0.033, 7)), + new DoNothingCommand(), + Intake.getInstance()::hasNote)), + + // intake D, shoot D + new SequentialCommandGroup( + new FollowPathAndIntake(E_TO_D), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot() + .withTolerance(0.033, 7)), + new DoNothingCommand(), + Intake.getInstance()::hasNote) + ), Intake.getInstance()::hasNote) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerry.java b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerry.java new file mode 100644 index 00000000..61fd7893 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/Ferry/ReroutableTopFerry.java @@ -0,0 +1,154 @@ +package com.stuypulse.robot.commands.auton.Ferry; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetOdometry; +import com.stuypulse.robot.commands.swerve.SwerveDriveStop; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class ReroutableTopFerry extends SequentialCommandGroup { + + private Pose2d getPathStartPose(PathPlannerPath path) { + return path.getPreviewStartingHolonomicPose(); + } + + public ReroutableTopFerry(PathPlannerPath... paths) { + + PathPlannerPath D_TO_E = paths[6]; + PathPlannerPath E_TO_F = paths[7]; + PathPlannerPath E_FERRY_REROUTE = paths[8]; + + Command INTAKE_E = new FollowPathAndIntake(paths[2]); + + addCommands ( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterPodiumShot()), + + SwerveDriveToPose.speakerRelative(45) + .withTolerance(0.03, 0.03, 3) + ), + + new ConveyorShootRoutine(), + + new ShooterSetRPM(Settings.Shooter.WING_FERRY), + + // intake D + new FollowPathAndIntake(paths[0]), + + new ConditionalCommand( + // if we have a note, we can shoot D + new SequentialCommandGroup( + + // shoot D, intake E + SwerveDrive.getInstance().followPathCommand(paths[1]), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + INTAKE_E, + + new ConditionalCommand( + // if we have a note, we can shoot E + new SequentialCommandGroup( + + // shoot E, intake F + SwerveDrive.getInstance().followPathCommand(paths[3]), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + new ShooterPodiumShot(), + + new SwerveDriveResetOdometry(() -> getPathStartPose(paths[4])), + new FollowPathAndIntake(paths[4]), + + new ConditionalCommand( + // if we have a note, we can shoot F + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), + new InstantCommand(), + Intake.getInstance()::hasNote) + ), + // else intake E + new SequentialCommandGroup( + new FollowPathAndIntake(D_TO_E), + + new ConditionalCommand( + new SequentialCommandGroup( + // shoot E, intake F + SwerveDrive.getInstance().followPathCommand(E_FERRY_REROUTE), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + new ShooterPodiumShot(), + + new SwerveDriveResetOdometry(() -> getPathStartPose(paths[4])), + new FollowPathAndIntake(paths[4]), + + new ConditionalCommand( + // if we have a note, we can shoot F + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), + new InstantCommand(), + Intake.getInstance()::hasNote) + ), + new SequentialCommandGroup( + new FollowPathAndIntake(E_TO_F), + + new ConditionalCommand( + // if we have a note, we can shoot F + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), + new InstantCommand(), + Intake.getInstance()::hasNote) + ), + Intake.getInstance()::hasNote) + ), + Intake.getInstance()::hasNote) + ), + // else intake E + new SequentialCommandGroup( + new FollowPathAndIntake(D_TO_E), + + new ConditionalCommand( + new SequentialCommandGroup( + // shoot E, intake F + SwerveDrive.getInstance().followPathCommand(E_FERRY_REROUTE), + new SwerveDriveStop(), + new ConveyorShootRoutine(), + new ShooterPodiumShot(), + + new SwerveDriveResetOdometry(() -> getPathStartPose(paths[4])), + new FollowPathAndIntake(paths[4]), + + new ConditionalCommand( + // if we have a note, we can shoot F + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), + new InstantCommand(), + Intake.getInstance()::hasNote) + ), + new SequentialCommandGroup( + new FollowPathAndIntake(E_TO_F), + + new ConditionalCommand( + // if we have a note, we can shoot F + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()), + new InstantCommand(), + Intake.getInstance()::hasNote) + ), + Intake.getInstance()::hasNote) + ), + Intake.getInstance()::hasNote) + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index 3a7febb1..e86b2981 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -30,9 +30,11 @@ public FourPieceHGF(PathPlannerPath... paths) { ConveyorShootRoutine.untilNoteShot(0.75), new FollowPathAndIntake(paths[0]), - new FollowPathAlignAndShootFast(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), + // new FollowPathAlignAndShootFast(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), + new FollowPathAlignAndShoot(paths[1], SwerveDriveToPose.speakerRelative(-45)), new FollowPathAndIntake(paths[2]), - new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), + // new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45)), new FollowPathAndIntake(paths[4]), new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java index 2521e4bd..2c2d09de 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/ReroutableFourPieceHGF.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.auton.HGF; import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.DoNothingCommand; import com.stuypulse.robot.commands.FastAlignShootSpeakerRelative; import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAlignAndShootFast; @@ -40,19 +41,29 @@ public ReroutableFourPieceHGF(PathPlannerPath... paths) { // has H new SequentialCommandGroup( // shoot H, intake G - new FollowPathAlignAndShootFast(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), + // new FollowPathAlignAndShootFast(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), + new FollowPathAlignAndShoot(paths[1], SwerveDriveToPose.speakerRelative(-45)), new FollowPathAndIntake(paths[2]), new ConditionalCommand( // shoot G, intake F, shoot F new SequentialCommandGroup( - new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), + // new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45)), new FollowPathAndIntake(paths[4]), - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45))), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new DoNothingCommand(), + Intake.getInstance()::hasNote)), // intake F, shoot F new SequentialCommandGroup( new FollowPathAndIntake(G_TO_F), - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45))), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new DoNothingCommand(), + Intake.getInstance()::hasNote)), Intake.getInstance()::hasNote)), // no H new SequentialCommandGroup( @@ -61,14 +72,24 @@ public ReroutableFourPieceHGF(PathPlannerPath... paths) { new ConditionalCommand( // shoot G, intake F, shoot F new SequentialCommandGroup( - new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), + // new FollowPathAlignAndShootFast(paths[3], new FastAlignShootSpeakerRelative(-45)), + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45)), new FollowPathAndIntake(paths[4]), - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45))), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new DoNothingCommand(), + Intake.getInstance()::hasNote) + ), // intake F, shoot F new SequentialCommandGroup( new FollowPathAndIntake(G_TO_F), - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45))), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)), + new DoNothingCommand(), + Intake.getInstance()::hasNote)), Intake.getInstance()::hasNote)), diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index e2554870..96c9fa75 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -61,10 +61,6 @@ private LEDInstruction getInstruction() { if (intake.hasNote()) { return LEDInstructions.CONTAINS_NOTE; } - - if (odometry.getPose().getX() > Field.LENGTH / 2.0) { - return LEDInstructions.ATTENTION; - } return LEDInstructions.DEFAULT; } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetOdometry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetOdometry.java index efdc8e75..adf3d9e5 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetOdometry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetOdometry.java @@ -6,6 +6,8 @@ package com.stuypulse.robot.commands.swerve; +import java.util.function.Supplier; + import com.stuypulse.robot.subsystems.odometry.Odometry; import edu.wpi.first.math.geometry.Pose2d; @@ -13,6 +15,10 @@ public class SwerveDriveResetOdometry extends InstantCommand { + public SwerveDriveResetOdometry(Supplier supplier) { + super(() -> Odometry.getInstance().reset(supplier.get())); + } + public SwerveDriveResetOdometry(Pose2d pose) { super(() -> Odometry.getInstance().reset(pose)); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToClimb.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToClimb.java index 13f3532b..60818874 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToClimb.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToClimb.java @@ -2,32 +2,107 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; -public class SwerveDriveToClimb extends SwerveDriveToPose { +public class SwerveDriveToClimb extends Command { + private final SwerveDrive swerve; + private final Odometry odometry; + + private final HolonomicController controller; + + private final FieldObject2d targetPose2d; + + private Pose2d targetPose; + + private final double distance; + public SwerveDriveToClimb() { this(Alignment.TRAP_SETUP_DISTANCE.get()); } public SwerveDriveToClimb(double distance) { - super( - () -> { - Pose2d closestTrap = Field.getClosestAllianceTrapPose(Odometry.getInstance().getPose()); - Translation2d offsetTranslation = new Translation2d(distance, closestTrap.getRotation()); - - return new Pose2d(closestTrap.getTranslation().plus(offsetTranslation), closestTrap.getRotation()); - } - ); + swerve = SwerveDrive.getInstance(); + odometry = Odometry.getInstance(); + + this.distance = distance; + + targetPose2d = odometry.getField().getObject("Target Pose"); + + controller = new HolonomicController( + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new AnglePIDController(Rotation.kP, Rotation.kI, Rotation.kD)); + } + + private Pose2d getTargetPose() { + Pose2d closestTrap = Field.getClosestAllianceTrapPose(Odometry.getInstance().getPose()); + Translation2d offsetTranslation = new Translation2d(distance, closestTrap.getRotation()); + + return new Pose2d(closestTrap.getTranslation().plus(offsetTranslation), closestTrap.getRotation()); + } + + private boolean shouldSlow() { + double toTarget = getTargetPose() + .getTranslation() + .minus(Odometry.getInstance().getPose().getTranslation()) + .getNorm(); + + return toTarget < Units.inchesToMeters(14.0); + } + + @Override + public void initialize() { + targetPose = getTargetPose(); + } + + @Override + public void execute() { + targetPose2d.setPose(targetPose); + controller.update(targetPose, odometry.getPose()); + + double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get()); + if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; - withTolerance(3.0, 3.0, 3.0); + Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond) + .clamp(2.0); + + if (shouldSlow()) + speed = speed.clamp(0.3); + + SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance()); + + swerve.setChassisSpeeds(new ChassisSpeeds(speed.x, speed.y, rotation)); } @Override public boolean isFinished() { return false; } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + Field.clearFieldObject(targetPose2d); + } + }