From 98bb19379a7ec112b78a0c998a37e2ce6d78fc15 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Thu, 1 Aug 2024 18:12:14 -0400 Subject: [PATCH 1/9] readded everything from og autons branch b/c rebasing is real annoying --- .../auton/FollowPathAlignAndShoot.java | 44 +++++++++++++++++++ .../auton/FollowPathAlignAndShootFast.java | 18 ++++++++ .../commands/auton/FollowPathAndIntake.java | 31 +++++++++++++ .../commands/auton/FollowPathWithShoot.java | 23 ++++++++++ .../auton/FollowPathWithShootAndIntake.java | 35 +++++++++++++++ .../commands/auton/HGF/FourPieceHGF.java | 39 ++++++++++++++++ .../robot/commands/auton/Mobility.java | 12 +++++ .../robot/commands/auton/UntilNoteShot.java | 21 +++++++++ .../commands/swerve/SwerveDriveStop.java | 15 +++++++ 9 files changed, 238 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/Mobility.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java new file mode 100644 index 00000000..9fd382d5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java @@ -0,0 +1,44 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathAlignAndShoot extends SequentialCommandGroup { + + public double getPathTime(PathPlannerPath path) { + return path.getTrajectory(new ChassisSpeeds(), path.getStartingDifferentialPose().getRotation()) + .getTotalTimeSeconds(); + } + + public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand) { + this(path, alignCommand, false); + } + + public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand, boolean noteShot) { + addCommands( + new ParallelCommandGroup( + SwerveDrive.getInstance().followPathCommand(path), + new WaitCommand(getPathTime(path) - Auton.SHOOTER_START_PRE) + .andThen(new ShooterScoreSpeaker()) + ), + alignCommand, + new ShooterWaitForTarget() + .withTimeout(0.5) + ); + + if (noteShot) + addCommands(new UntilNoteShot(0.75)); + else + addCommands(new ShooterScoreSpeaker()); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java new file mode 100644 index 00000000..4510f8c0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FollowPathAlignAndShootFast extends SequentialCommandGroup { + + public FollowPathAlignAndShootFast(PathPlannerPath path, FastAlignShootSpeakerRelative alignCommand) { + addCommands( + SwerveDrive.getInstance().followPathCommand(path), + alignCommand, + new IntakeStop() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java new file mode 100644 index 00000000..5149e604 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java @@ -0,0 +1,31 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.swerve.SwerveDriveStop; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathAndIntake extends SequentialCommandGroup { + + public FollowPathAndIntake(PathPlannerPath path) { + this(path, Auton.DEFAULT_INTAKE_TIMEOUT); + } + + public FollowPathAndIntake(PathPlannerPath path, double intakeTimeout) { + addCommands( + new ParallelRaceGroup( + new IntakeAcquire(), + + SwerveDrive.getInstance().followPathCommand(path) + .andThen(new WaitCommand(intakeTimeout)) + ), + + new SwerveDriveStop() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java new file mode 100644 index 00000000..84370dfa --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java @@ -0,0 +1,23 @@ + +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathWithShoot extends ParallelCommandGroup { + + public FollowPathWithShoot(PathPlannerPath path, double shootTime) { + addCommands( + SwerveDrive.getInstance().followPathCommand(path), + new WaitCommand(shootTime) + .andThen(new ShooterWaitForTarget()) + .andThen(new ShooterScoreSpeaker()) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java new file mode 100644 index 00000000..77c6afd1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java @@ -0,0 +1,35 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathWithShootAndIntake extends SequentialCommandGroup { + + public FollowPathWithShootAndIntake(PathPlannerPath path, double shootTime) { + this(path, shootTime, Auton.DEFAULT_INTAKE_TIMEOUT); + } + + public FollowPathWithShootAndIntake(PathPlannerPath path, double shootTime, double intakeTimeout) { + addCommands( + new ParallelCommandGroup( + SwerveDrive.getInstance().followPathCommand(path) + .until(() -> Intake.getInstance().hasNote()), + new SequentialCommandGroup( + new WaitCommand(shootTime), + new ShooterWaitForTarget(), + new ShooterScoreSpeaker(), + new IntakeAcquire() + ) + ) + ); + } +} \ No newline at end of file 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 new file mode 100644 index 00000000..7b6e765d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -0,0 +1,39 @@ +package com.stuypulse.robot.commands.auton.HGF; + +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.auton.UntilNoteShot; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FourPieceHGF { + + public FourPieceHGF(PathPlannerPath... paths) { + addCommands( + new ParallelCommandGroup( + new WaitCommand(0.25) + .andThen(new ShooterScoreSpeaker()), + + SwerveDriveToPose.speakerRelative(-55) + .withTolerance(0.1, 0.1, 2) + ), + + new ShooterWaitForTarget(), + new UntilNoteShot(0.75), + + new FollowPathAndIntake(paths[0]), + new FollowPathAlignAndShoot(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), + new FollowPathAlignAndShoot(paths[1], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAndIntake(paths[2]), + new FollowPathAlignAndShoot(paths[3], new FastAlignShootSpeakerRelative(-45)), + new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45)), + new FollowPathAndIntake(paths[4]), + new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)) + ); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java new file mode 100644 index 00000000..d17c1a58 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.auton; + +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class Mobility extends SequentialCommandGroup { + + public Mobility() { + addCommands( + SwerveDrive.getInstance().followPathCommand("Mobility")); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java new file mode 100644 index 00000000..9c214d21 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.auton; + +import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class UntilNoteShot extends SequentialCommandGroup { + + public UntilNoteShot(double timeout) { + addCommands( + new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), + new WaitUntilCommand(Shooter.getInstance()::hasNote) + .withTimeout(timeout), + new IntakeStop() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java new file mode 100644 index 00000000..ed7fc495 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java @@ -0,0 +1,15 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SwerveDriveStop extends InstantCommand { + + public SwerveDriveStop() { + super(() -> SwerveDrive.getInstance().stop()); + + addRequirements(SwerveDrive.getInstance()); + } + +} \ No newline at end of file From 44061359b45d60350b061114cc62be5f8db12a95 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 2 Aug 2024 18:33:05 -0400 Subject: [PATCH 2/9] paths --- src/main/deploy/pathplanner/paths/A to C.path | 58 ++++++++++++++ src/main/deploy/pathplanner/paths/A to D.path | 52 ++++++++++++ .../deploy/pathplanner/paths/Amp to A.path | 52 ++++++++++++ src/main/deploy/pathplanner/paths/B to A.path | 49 ++++++++++++ src/main/deploy/pathplanner/paths/B to F.path | 55 +++++++++++++ src/main/deploy/pathplanner/paths/C to A.path | 58 ++++++++++++++ src/main/deploy/pathplanner/paths/C to B.path | 58 ++++++++++++++ .../deploy/pathplanner/paths/Center to B.path | 49 ++++++++++++ .../deploy/pathplanner/paths/Center to C.path | 49 ++++++++++++ .../pathplanner/paths/D Shoot to E.path | 58 ++++++++++++++ .../deploy/pathplanner/paths/D to Shoot.path | 55 +++++++++++++ .../pathplanner/paths/E Shoot to D.path | 52 ++++++++++++ .../pathplanner/paths/E Shoot to F.path | 58 ++++++++++++++ .../deploy/pathplanner/paths/E to Shoot.path | 58 ++++++++++++++ .../pathplanner/paths/F Shoot ALT to E.path | 52 ++++++++++++ .../pathplanner/paths/F Shoot to A.path | 52 ++++++++++++ .../pathplanner/paths/F Shoot to C.path | 58 ++++++++++++++ .../pathplanner/paths/F Shoot to E.path | 58 ++++++++++++++ .../pathplanner/paths/F Shoot to G.path | 52 ++++++++++++ .../pathplanner/paths/F to Shoot ALT.path | 58 ++++++++++++++ .../deploy/pathplanner/paths/F to Shoot.path | 52 ++++++++++++ .../pathplanner/paths/G Shoot to F.path | 58 ++++++++++++++ .../pathplanner/paths/G Shoot to H.path | 58 ++++++++++++++ .../deploy/pathplanner/paths/G to Shoot.path | 52 ++++++++++++ .../pathplanner/paths/H Shoot to G.path | 58 ++++++++++++++ .../deploy/pathplanner/paths/H to Shoot.path | 52 ++++++++++++ .../deploy/pathplanner/paths/Mobility.path | 49 ++++++++++++ .../deploy/pathplanner/paths/Source to H.path | 79 +++++++++++++++++++ 28 files changed, 1549 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/A to C.path create mode 100644 src/main/deploy/pathplanner/paths/A to D.path create mode 100644 src/main/deploy/pathplanner/paths/Amp to A.path create mode 100644 src/main/deploy/pathplanner/paths/B to A.path create mode 100644 src/main/deploy/pathplanner/paths/B to F.path create mode 100644 src/main/deploy/pathplanner/paths/C to A.path create mode 100644 src/main/deploy/pathplanner/paths/C to B.path create mode 100644 src/main/deploy/pathplanner/paths/Center to B.path create mode 100644 src/main/deploy/pathplanner/paths/Center to C.path create mode 100644 src/main/deploy/pathplanner/paths/D Shoot to E.path create mode 100644 src/main/deploy/pathplanner/paths/D to Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/E Shoot to D.path create mode 100644 src/main/deploy/pathplanner/paths/E Shoot to F.path create mode 100644 src/main/deploy/pathplanner/paths/E to Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/F Shoot ALT to E.path create mode 100644 src/main/deploy/pathplanner/paths/F Shoot to A.path create mode 100644 src/main/deploy/pathplanner/paths/F Shoot to C.path create mode 100644 src/main/deploy/pathplanner/paths/F Shoot to E.path create mode 100644 src/main/deploy/pathplanner/paths/F Shoot to G.path create mode 100644 src/main/deploy/pathplanner/paths/F to Shoot ALT.path create mode 100644 src/main/deploy/pathplanner/paths/F to Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/G Shoot to F.path create mode 100644 src/main/deploy/pathplanner/paths/G Shoot to H.path create mode 100644 src/main/deploy/pathplanner/paths/G to Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/H Shoot to G.path create mode 100644 src/main/deploy/pathplanner/paths/H to Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/Mobility.path create mode 100644 src/main/deploy/pathplanner/paths/Source to H.path diff --git a/src/main/deploy/pathplanner/paths/A to C.path b/src/main/deploy/pathplanner/paths/A to C.path new file mode 100644 index 00000000..8ab12e3c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A to C.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.49, + "y": 6.82 + }, + "prevControl": null, + "nextControl": { + "x": 1.8063404583877622, + "y": 5.684733839028691 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5041716843106308, + "y": 4.352132867212163 + }, + "prevControl": { + "x": 1.7061923065438394, + "y": 5.606093593139567 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -35.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.01025016685515, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 40.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path new file mode 100644 index 00000000..e37f2b30 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5387368492370914, + "y": 6.712803832043677 + }, + "prevControl": null, + "nextControl": { + "x": 3.679387291810298, + "y": 6.898225029708988 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.80980431566518, + "y": 7.431413130125368 + }, + "prevControl": { + "x": 5.426682819511473, + "y": 7.094030199035101 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 41.18784481523856, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp to A.path b/src/main/deploy/pathplanner/paths/Amp to A.path new file mode 100644 index 00000000..769f6912 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp to A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.435487934219916, + "y": 6.595791855088175 + }, + "prevControl": null, + "nextControl": { + "x": 2.26274434319702, + "y": 6.615980477868694 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5146518038571792, + "y": 6.712803832043677 + }, + "prevControl": { + "x": 1.7508529994738564, + "y": 6.559928693704114 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.85599579668576, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path new file mode 100644 index 00000000..3920a744 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.403841365532434, + "y": 5.547443382833194 + }, + "prevControl": null, + "nextControl": { + "x": 2.1124651977726576, + "y": 6.184182756200334 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.505085089225238, + "y": 6.749916915942463 + }, + "prevControl": { + "x": 1.9565133270891542, + "y": 6.234996913818232 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.7991322249468, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path new file mode 100644 index 00000000..d0cd21c6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B to F.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.54, + "y": 5.53 + }, + "prevControl": null, + "nextControl": { + "x": 3.5538181530232116, + "y": 5.137757241660317 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": { + "x": 5.998955035484476, + "y": 3.6317929071870045 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": -24.38902882105597, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -19.61556919738271, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to A.path b/src/main/deploy/pathplanner/paths/C to A.path new file mode 100644 index 00000000..3ae25545 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C to A.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.52, + "y": 4.38 + }, + "prevControl": null, + "nextControl": { + "x": 1.9086797539294484, + "y": 5.314909721151288 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.47, + "y": 6.77 + }, + "prevControl": { + "x": 1.9818002210409187, + "y": 5.977080210298814 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 1.0388554318784187, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 38.18757752913914, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path new file mode 100644 index 00000000..073ac455 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.51, + "y": 4.29 + }, + "prevControl": null, + "nextControl": { + "x": 2.0727080470157966, + "y": 4.981565001262443 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.425282344054272, + "y": 5.545520425118232 + }, + "prevControl": { + "x": 1.6559550362404298, + "y": 5.38476116014732 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": -32.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to B.path b/src/main/deploy/pathplanner/paths/Center to B.path new file mode 100644 index 00000000..ea98b40c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center to B.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4053936459807443, + "y": 5.5301848373414 + }, + "prevControl": null, + "nextControl": { + "x": 2.179287978367624, + "y": 5.533261569685343 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.537823444322484, + "y": 5.5301848373414 + }, + "prevControl": { + "x": 2.0945336170806272, + "y": 5.562923192438648 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to C.path b/src/main/deploy/pathplanner/paths/Center to C.path new file mode 100644 index 00000000..190fa1d3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center to C.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4365936349060198, + "y": 5.5179740558513855 + }, + "prevControl": null, + "nextControl": { + "x": 1.9887709427576594, + "y": 4.858543781447682 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.509507891969653, + "y": 4.287761857703775 + }, + "prevControl": { + "x": 2.1220319124045988, + "y": 4.703745685393156 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -25.389005943465087, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D Shoot to E.path b/src/main/deploy/pathplanner/paths/D Shoot to E.path new file mode 100644 index 00000000..a2699513 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D Shoot to E.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 6.510461787664648, + "y": 6.264802758400161 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.834129730759462, + "y": 5.769064259482685 + }, + "prevControl": { + "x": 6.47604084456681, + "y": 6.294320159324842 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 16.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D to Shoot.path b/src/main/deploy/pathplanner/paths/D to Shoot.path new file mode 100644 index 00000000..63adf4ed --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D to Shoot.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.7855750484566455, + "y": 7.436124376527027 + }, + "prevControl": null, + "nextControl": { + "x": 6.609157592385126, + "y": 7.043167967474329 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.329525380962965, + "y": 6.789433696984953 + }, + "prevControl": { + "x": 6.628819835020623, + "y": 7.036774133072075 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 16.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 17.438531624411638, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E Shoot to D.path b/src/main/deploy/pathplanner/paths/E Shoot to D.path new file mode 100644 index 00000000..71074dab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E Shoot to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 6.421717289119103, + "y": 7.099702924294243 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.79, + "y": 7.44 + }, + "prevControl": { + "x": 6.876162419906684, + "y": 7.238618253300495 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 5.550205795548943, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 25.626758135321538, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E Shoot to F.path b/src/main/deploy/pathplanner/paths/E Shoot to F.path new file mode 100644 index 00000000..95aa5b23 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E Shoot to F.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 6.704247851390048, + "y": 5.800312322350837 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.8917703882654795, + "y": 4.342229634980103 + }, + "prevControl": { + "x": 6.716218263165693, + "y": 5.79459152314882 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -39.907792341252474, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.96161156944142, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 11.872095567230494, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path new file mode 100644 index 00000000..a2fda6be --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": null, + "nextControl": { + "x": 6.464262728562661, + "y": 6.586753953827849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": { + "x": 6.487145925370721, + "y": 6.506326247399518 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 16.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 17.14111484001432, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 39.01725127228337, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path new file mode 100644 index 00000000..0c8920d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 6.282639372384399, + "y": 6.412389763023573 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": { + "x": 6.77751254033014, + "y": 6.22081510067038 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": 17.734765504348854, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to A.path b/src/main/deploy/pathplanner/paths/F Shoot to A.path new file mode 100644 index 00000000..0a42c162 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 2.9511160941071886, + "y": 5.54126539972574 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.487778469790571, + "y": 6.823421974596926 + }, + "prevControl": { + "x": 2.094966282566494, + "y": 6.342153732484548 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.162913907056854, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -16.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to C.path b/src/main/deploy/pathplanner/paths/F Shoot to C.path new file mode 100644 index 00000000..05a7e763 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to C.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 3.0066405352304755, + "y": 5.122613949654983 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5184015714013572, + "y": 4.376602504135068 + }, + "prevControl": { + "x": 1.7198933802629512, + "y": 5.131796072743931 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -3.9091747518495517, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.82130002339912, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -3.382897056838466, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to E.path b/src/main/deploy/pathplanner/paths/F Shoot to E.path new file mode 100644 index 00000000..32aabad5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to E.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 5.607152401060755, + "y": 3.1411502462141825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": { + "x": 7.661789273883607, + "y": 5.857831093630937 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 17.078402120790262, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 7.5783021431903075, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -3.5408335937068656, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to G.path b/src/main/deploy/pathplanner/paths/F Shoot to G.path new file mode 100644 index 00000000..1e30f609 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to G.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 5.415973831144092, + "y": 4.249032236330842 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": { + "x": 6.730603199234672, + "y": 3.645327872322412 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -38.92997350795309, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -6.680432083088365, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to Shoot ALT.path b/src/main/deploy/pathplanner/paths/F to Shoot ALT.path new file mode 100644 index 00000000..1d6e5751 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to Shoot ALT.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 7.037832940993264, + "y": 5.5022538765315625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": { + "x": 6.695690689558464, + "y": 5.875163451405772 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -53.74657859666923, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 13.23836155662926, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -27.35687342191869, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to Shoot.path b/src/main/deploy/pathplanner/paths/F to Shoot.path new file mode 100644 index 00000000..baab248b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 5.625660869067274, + "y": 4.243149238716632 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": { + "x": 5.723635564644642, + "y": 4.265840139753196 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -8.268297961765734, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -27.729825561933676, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G Shoot to F.path b/src/main/deploy/pathplanner/paths/G Shoot to F.path new file mode 100644 index 00000000..7aca94cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G Shoot to F.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 5.614603862206237, + "y": 4.242043538030528 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": { + "x": 6.6666540280625215, + "y": 4.085370558203914 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -12.184647650336021, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.573918256423003, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -9.481237644879682, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G Shoot to H.path b/src/main/deploy/pathplanner/paths/G Shoot to H.path new file mode 100644 index 00000000..91d1fa90 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G Shoot to H.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 5.64412126313092, + "y": 4.370449039447186 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.018108710138552, + "y": 1.1315632861920477 + }, + "prevControl": { + "x": 7.387955466945161, + "y": 2.4966709680443175 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -49.02018512209823, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -49.24596024004399, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -8.909069649342515, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G to Shoot.path b/src/main/deploy/pathplanner/paths/G to Shoot.path new file mode 100644 index 00000000..a9ff6b30 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": null, + "nextControl": { + "x": 6.794920886590286, + "y": 3.474898154111784 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": { + "x": 5.047523632063633, + "y": 4.447367348045708 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -7.757990482222401, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -15.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H Shoot to G.path b/src/main/deploy/pathplanner/paths/H Shoot to G.path new file mode 100644 index 00000000..4129899c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H Shoot to G.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 5.583645205891603, + "y": 4.669973259075983 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.929219989764386, + "y": 2.728964260011853 + }, + "prevControl": { + "x": 6.772224406613993, + "y": 3.2354713221331206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -25.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -40.317362875171, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -5.742284345235014, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H to Shoot.path b/src/main/deploy/pathplanner/paths/H to Shoot.path new file mode 100644 index 00000000..eb515e1c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.03, + "y": 1.13 + }, + "prevControl": null, + "nextControl": { + "x": 7.760720819990748, + "y": 2.1380393542037086 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.439099811049325, + "y": 4.919501541012011 + }, + "prevControl": { + "x": 6.113755611067771, + "y": 4.501017868293177 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -8.797211565554276, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -57.0617176721166, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path new file mode 100644 index 00000000..2bcb647a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.897224241249078, + "y": 5.567009477582943 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.15, + "y": 5.55 + }, + "prevControl": { + "x": 2.896214688448722, + "y": 5.56249052695278 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source to H.path b/src/main/deploy/pathplanner/paths/Source to H.path new file mode 100644 index 00000000..d0d01520 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source to H.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7437038962619593, + "y": 4.320740582515391 + }, + "prevControl": null, + "nextControl": { + "x": 1.5747100727832408, + "y": 3.3032555815855678 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.19535290219204, + "y": 2.5364461465297556 + }, + "prevControl": { + "x": 3.256787052187917, + "y": 3.2931577463451562 + }, + "nextControl": { + "x": 5.156266890844794, + "y": 1.76171652938544 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.03, + "y": 1.13 + }, + "prevControl": { + "x": 6.683160416439879, + "y": 1.5490605600333218 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": 2.3059547563747755, + "rotateFast": false + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": -23.99917974539861, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.9402672501694, + "rotateFast": false + }, + "reversed": false, + "folder": "FGH", + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From 50bd894c2e4573faf0f3fddbbfe7e714aae13d49 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Sat, 3 Aug 2024 12:56:21 -0400 Subject: [PATCH 3/9] readded pathplanner stuff, worked a bit on cba, added swervedrivetoshoot, planning to make followpathcmd some time soon --- .pathplanner/settings.json | 15 +++++ src/main/deploy/pathplanner/autos/ADEF.auto | 61 ++++++++++++++++++ src/main/deploy/pathplanner/autos/BFAC.auto | 49 +++++++++++++++ src/main/deploy/pathplanner/autos/BFCA.auto | 49 +++++++++++++++ src/main/deploy/pathplanner/autos/BFED.auto | 61 ++++++++++++++++++ src/main/deploy/pathplanner/autos/BFGH.auto | 61 ++++++++++++++++++ src/main/deploy/pathplanner/autos/CBA.auto | 37 +++++++++++ src/main/deploy/pathplanner/autos/HGF.auto | 55 ++++++++++++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/F to A Shoot.path | 52 +++++++++++++++ .../pathplanner/paths/F to C Shoot.path | 63 +++++++++++++++++++ .../pathplanner/paths/FA Shoot to A.path | 52 +++++++++++++++ .../pathplanner/paths/FC Shoot to C.path | 52 +++++++++++++++ .../commands/auton/CBA/FourPieceCBA.java | 43 +++++++++++++ .../commands/shooter/SwerveDriveToShoot.java | 29 +++++++++ 15 files changed, 680 insertions(+) create mode 100644 .pathplanner/settings.json create mode 100644 src/main/deploy/pathplanner/autos/ADEF.auto create mode 100644 src/main/deploy/pathplanner/autos/BFAC.auto create mode 100644 src/main/deploy/pathplanner/autos/BFCA.auto create mode 100644 src/main/deploy/pathplanner/autos/BFED.auto create mode 100644 src/main/deploy/pathplanner/autos/BFGH.auto create mode 100644 src/main/deploy/pathplanner/autos/CBA.auto create mode 100644 src/main/deploy/pathplanner/autos/HGF.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/F to A Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/F to C Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/FA Shoot to A.path create mode 100644 src/main/deploy/pathplanner/paths/FC Shoot to C.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 00000000..71dc1d9a --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,15 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [ + "ABCDE", + "HGF" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ADEF.auto b/src/main/deploy/pathplanner/autos/ADEF.auto new file mode 100644 index 00000000..5788e66b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ADEF.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.435487934219916, + "y": 6.595791855088175 + }, + "rotation": 1.3048503501930353 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Amp to A" + } + }, + { + "type": "path", + "data": { + "pathName": "A to D" + } + }, + { + "type": "path", + "data": { + "pathName": "D to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "D Shoot to E" + } + }, + { + "type": "path", + "data": { + "pathName": "E to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "E Shoot to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFAC.auto b/src/main/deploy/pathplanner/autos/BFAC.auto new file mode 100644 index 00000000..2293e32a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFAC.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4053936459807443, + "y": 5.5301848373414 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to A Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FA Shoot to A" + } + }, + { + "type": "path", + "data": { + "pathName": "A to C" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFCA.auto b/src/main/deploy/pathplanner/autos/BFCA.auto new file mode 100644 index 00000000..56528954 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFCA.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4053936459807443, + "y": 5.5301848373414 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to C Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "FC Shoot to C" + } + }, + { + "type": "path", + "data": { + "pathName": "C to A" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFED.auto b/src/main/deploy/pathplanner/autos/BFED.auto new file mode 100644 index 00000000..47d69b92 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFED.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4053936459807443, + "y": 5.5301848373414 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot ALT" + } + }, + { + "type": "path", + "data": { + "pathName": "F Shoot ALT to E" + } + }, + { + "type": "path", + "data": { + "pathName": "E to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "E Shoot to D" + } + }, + { + "type": "path", + "data": { + "pathName": "D to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFGH.auto b/src/main/deploy/pathplanner/autos/BFGH.auto new file mode 100644 index 00000000..14f26960 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFGH.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4053936459807443, + "y": 5.5301848373414 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "F Shoot to G" + } + }, + { + "type": "path", + "data": { + "pathName": "G to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "G Shoot to H" + } + }, + { + "type": "path", + "data": { + "pathName": "H to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/CBA.auto b/src/main/deploy/pathplanner/autos/CBA.auto new file mode 100644 index 00000000..699f42d1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CBA.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4365936349060198, + "y": 5.5179740558513855 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center to C" + } + }, + { + "type": "path", + "data": { + "pathName": "C to B" + } + }, + { + "type": "path", + "data": { + "pathName": "B to A" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HGF.auto b/src/main/deploy/pathplanner/autos/HGF.auto new file mode 100644 index 00000000..dcb5cb07 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HGF.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7437038962619593, + "y": 4.320740582515391 + }, + "rotation": -59.682052822906385 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source to H" + } + }, + { + "type": "path", + "data": { + "pathName": "H to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "H Shoot to G" + } + }, + { + "type": "path", + "data": { + "pathName": "G to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "G Shoot to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 00000000..bab0da93 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to A Shoot.path b/src/main/deploy/pathplanner/paths/F to A Shoot.path new file mode 100644 index 00000000..2d63eaa8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to A Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 4.9685862178644005, + "y": 4.268436132668396 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2530814806793305, + "y": 6.434696072517145 + }, + "prevControl": { + "x": 4.892244796580366, + "y": 4.086860850433009 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.67316277210924, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -28.880848497857865, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to C Shoot.path b/src/main/deploy/pathplanner/paths/F to C Shoot.path new file mode 100644 index 00000000..03d2921b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to C Shoot.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 5.6385927597004, + "y": 3.2728247757462774 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2317366500432407, + "y": 4.6325001020537755 + }, + "prevControl": { + "x": 3.0671656293089375, + "y": 6.355710584375037 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -42.94374675747001, + "rotateFast": true + }, + { + "waypointRelativePos": 0.75, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.108347979019236, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -27.412130290314096, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FA Shoot to A.path b/src/main/deploy/pathplanner/paths/FA Shoot to A.path new file mode 100644 index 00000000..a52fbdf2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FA Shoot to A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.25, + "y": 6.43 + }, + "prevControl": null, + "nextControl": { + "x": 2.356103940255067, + "y": 6.680882733977868 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.49, + "y": 6.82 + }, + "prevControl": { + "x": 2.395476499468936, + "y": 6.670691058088565 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 37.107670410433414, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": 40.36814255534238, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FC Shoot to C.path b/src/main/deploy/pathplanner/paths/FC Shoot to C.path new file mode 100644 index 00000000..accb8635 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FC Shoot to C.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.23, + "y": 4.63 + }, + "prevControl": null, + "nextControl": { + "x": 2.371295306206684, + "y": 4.45587643593442 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.52, + "y": 4.35 + }, + "prevControl": { + "x": 2.368075591798505, + "y": 4.424436077294773 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -28.277746130553208, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -32.54514019016325, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java new file mode 100644 index 00000000..0f80ae58 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java @@ -0,0 +1,43 @@ +package com.stuypulse.robot.commands.auton.CBA; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.UntilNoteShot; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.constants.Settings.Auton; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FourPieceCBA { + + public FourPieceCBA(PathPlannerPath... path) { + addCommands( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterScoreSpeaker()), + + SwerveDriveToPose.speakerRelative(-15) + .withTolerance(0.03, 0.03, 3) + ), + + new UntilNoteShot(0.7), + + new FollowPathAndIntake(path[0]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + + new FollowPathAndIntake(path[1]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + + new FollowPathAndIntake(path[2]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(0.7) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java new file mode 100644 index 00000000..45fe180f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import de.erichseifert.vectorgraphics2d.intermediate.commands.Command; + +public class SwerveDriveToShoot extends Command { + + private final SwerveDrive swerve; + + private final Number targetDistance; + + public SwerveDriveToShoot() { + this(Alignment.PODIUM_SHOT_DISTANCE); + } + + public SwerveDriveToShoot(Number targetDistance) { + this(targetDistance, Alignment.DEBOUNCE_TIME); + } + + public SwerveDriveToShoot(Number targetDistance, double debounce) { + this.targetDistance = targetDistance; + + swerve = SwerveDrive.getInstance(); + + } + +} From 9b6af67947a5d3d2180bfdcffb23c36294ecd8b2 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 9 Aug 2024 17:54:59 -0400 Subject: [PATCH 4/9] basic paths all done, will add rerouting stuff at future date --- .../pathplanner/paths/F Shoot ALT to E.path | 2 +- .../pathplanner/paths/F Shoot to A.path | 2 +- .../pathplanner/paths/F Shoot to C.path | 2 +- .../pathplanner/paths/F Shoot to E.path | 2 +- .../pathplanner/paths/F Shoot to G.path | 2 +- .../pathplanner/paths/F to Shoot ALT.path | 2 +- .../deploy/pathplanner/paths/F to Shoot.path | 2 +- .../pathplanner/paths/G Shoot to F.path | 2 +- .../pathplanner/paths/G Shoot to H.path | 2 +- .../deploy/pathplanner/paths/G to Shoot.path | 2 +- .../pathplanner/paths/H Shoot to G.path | 2 +- .../deploy/pathplanner/paths/H to Shoot.path | 2 +- .../pathplanner/paths/Point Locations.path | 161 +++++++++++++++ .../deploy/pathplanner/paths/Source to H.path | 2 +- .../commands/auton/ADEF/FivePieceADEF.java | 34 ++++ .../auton/BF_Series/FivePieceBFAC.java | 36 ++++ .../auton/BF_Series/FivePieceBFCA.java | 36 ++++ .../auton/BF_Series/FivePieceBFED.java | 36 ++++ .../auton/BF_Series/FivePieceBFGH.java | 36 ++++ .../commands/auton/CBA/FourPieceCBA.java | 5 +- .../auton/FollowPathAlignAndShootFast.java | 18 -- .../commands/auton/HGF/FourPieceHGF.java | 35 ++-- .../robot/commands/auton/UntilNoteShot.java | 5 + .../commands/shooter/SwerveDriveToShoot.java | 128 +++++++++++- .../commands/swerve/SwerveDriveToPose.java | 183 ++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 28 +++ .../robot/subsystems/swerve/SwerveDrive.java | 48 +++++ .../robot/util/MirrorRotation2d.java | 29 +++ 28 files changed, 798 insertions(+), 46 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Point Locations.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java create mode 100644 src/main/java/com/stuypulse/robot/util/MirrorRotation2d.java diff --git a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path index 0c8920d1..b0d2f337 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path +++ b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": 17.734765504348854, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/F Shoot to A.path b/src/main/deploy/pathplanner/paths/F Shoot to A.path index 0a42c162..227629f6 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to A.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to A.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -16.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/F Shoot to C.path b/src/main/deploy/pathplanner/paths/F Shoot to C.path index 05a7e763..aeef9976 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to C.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -3.382897056838466, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/F Shoot to E.path b/src/main/deploy/pathplanner/paths/F Shoot to E.path index 32aabad5..1ae9e0dc 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to E.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -3.5408335937068656, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/F Shoot to G.path b/src/main/deploy/pathplanner/paths/F Shoot to G.path index 1e30f609..6dd5ba45 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to G.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -6.680432083088365, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/F to Shoot ALT.path b/src/main/deploy/pathplanner/paths/F to Shoot ALT.path index 1d6e5751..e1195418 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot ALT.path +++ b/src/main/deploy/pathplanner/paths/F to Shoot ALT.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -27.35687342191869, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/F to Shoot.path b/src/main/deploy/pathplanner/paths/F to Shoot.path index baab248b..164c2fe6 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/F to Shoot.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -27.729825561933676, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/G Shoot to F.path b/src/main/deploy/pathplanner/paths/G Shoot to F.path index 7aca94cb..65685f5c 100644 --- a/src/main/deploy/pathplanner/paths/G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/G Shoot to F.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -9.481237644879682, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/G Shoot to H.path b/src/main/deploy/pathplanner/paths/G Shoot to H.path index 91d1fa90..79b34d8e 100644 --- a/src/main/deploy/pathplanner/paths/G Shoot to H.path +++ b/src/main/deploy/pathplanner/paths/G Shoot to H.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -8.909069649342515, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/G to Shoot.path b/src/main/deploy/pathplanner/paths/G to Shoot.path index a9ff6b30..25198788 100644 --- a/src/main/deploy/pathplanner/paths/G to Shoot.path +++ b/src/main/deploy/pathplanner/paths/G to Shoot.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -15.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/H Shoot to G.path b/src/main/deploy/pathplanner/paths/H Shoot to G.path index 4129899c..3ddeed4f 100644 --- a/src/main/deploy/pathplanner/paths/H Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/H Shoot to G.path @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -5.742284345235014, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/H to Shoot.path b/src/main/deploy/pathplanner/paths/H to Shoot.path index eb515e1c..5c284ece 100644 --- a/src/main/deploy/pathplanner/paths/H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/H to Shoot.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -57.0617176721166, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Point Locations.path b/src/main/deploy/pathplanner/paths/Point Locations.path new file mode 100644 index 00000000..9ef48822 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Point Locations.path @@ -0,0 +1,161 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.8606755147290057, + "y": 6.160145784763296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.25, + "y": 5.55 + }, + "prevControl": { + "x": 0.23853011040910355, + "y": 5.685869940164359 + }, + "nextControl": { + "x": 0.26146988959089645, + "y": 5.4141300598356406 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": { + "x": 2.89563193569979, + "y": 5.358936627062242 + }, + "nextControl": { + "x": 2.90436806430021, + "y": 5.741063372937758 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 4.1 + }, + "prevControl": { + "x": 2.910352370185796, + "y": 4.231098373366762 + }, + "nextControl": { + "x": 2.889647629814204, + "y": 3.9689016266332375 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 7.44 + }, + "prevControl": { + "x": 7.180548926469876, + "y": 6.917676610673163 + }, + "nextControl": { + "x": 8.69419010200492, + "y": 7.63677199205437 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 5.77 + }, + "prevControl": { + "x": 8.28838041697661, + "y": 6.311456818590719 + }, + "nextControl": { + "x": 8.269224286337115, + "y": 5.073783611933209 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 4.1 + }, + "prevControl": { + "x": 8.286182106596115, + "y": 4.544857124277373 + }, + "nextControl": { + "x": 8.271362241199185, + "y": 3.4784370294187017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 2.44 + }, + "prevControl": { + "x": 8.274967786914742, + "y": 3.4775236245040944 + }, + "nextControl": { + "x": 8.282900374354867, + "y": 1.842011230029793 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 0.77 + }, + "prevControl": { + "x": 8.266410625083155, + "y": 1.8410385351276657 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source to H.path b/src/main/deploy/pathplanner/paths/Source to H.path index d0d01520..57f2fdad 100644 --- a/src/main/deploy/pathplanner/paths/Source to H.path +++ b/src/main/deploy/pathplanner/paths/Source to H.path @@ -70,7 +70,7 @@ "rotateFast": false }, "reversed": false, - "folder": "FGH", + "folder": "HGF", "previewStartingState": { "rotation": -60.0, "velocity": 0 diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java new file mode 100644 index 00000000..dc8315c8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -0,0 +1,34 @@ +package com.stuypulse.robot.commands.auton.ADEF; + +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.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceADEF extends SequentialCommandGroup { + + public FivePieceADEF(PathPlannerPath... paths) { + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java new file mode 100644 index 00000000..9eac5362 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +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.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFAC extends SequentialCommandGroup { + + public FivePieceBFAC(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java new file mode 100644 index 00000000..46740f52 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +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.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFCA extends SequentialCommandGroup { + + public FivePieceBFCA(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java new file mode 100644 index 00000000..08c6acc1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +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.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFED extends SequentialCommandGroup { + + public FivePieceBFED(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java new file mode 100644 index 00000000..6474c310 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +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.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFGH extends SequentialCommandGroup { + + public FivePieceBFGH(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java index 0f80ae58..a25756a0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java @@ -4,12 +4,15 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.UntilNoteShot; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.constants.Settings.Auton; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -public class FourPieceCBA { +public class FourPieceCBA extends SequentialCommandGroup { public FourPieceCBA(PathPlannerPath... path) { addCommands( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java deleted file mode 100644 index 4510f8c0..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShootFast.java +++ /dev/null @@ -1,18 +0,0 @@ -package com.stuypulse.robot.commands.auton; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.intake.IntakeStop; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class FollowPathAlignAndShootFast extends SequentialCommandGroup { - - public FollowPathAlignAndShootFast(PathPlannerPath path, FastAlignShootSpeakerRelative alignCommand) { - addCommands( - SwerveDrive.getInstance().followPathCommand(path), - alignCommand, - new IntakeStop() - ); - } -} \ No newline at end of file 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 7b6e765d..e14f55a2 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 @@ -6,33 +6,42 @@ import com.stuypulse.robot.commands.auton.UntilNoteShot; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.constants.Settings.Auton; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -public class FourPieceHGF { +public class FourPieceHGF extends SequentialCommandGroup { public FourPieceHGF(PathPlannerPath... paths) { addCommands( new ParallelCommandGroup( - new WaitCommand(0.25) + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) .andThen(new ShooterScoreSpeaker()), - - SwerveDriveToPose.speakerRelative(-55) - .withTolerance(0.1, 0.1, 2) + + SwerveDriveToPose.speakerRelative(-15) + .withTolerance(0.03, 0.03, 3) ), - new ShooterWaitForTarget(), - new UntilNoteShot(0.75), + new UntilNoteShot(0.7), new FollowPathAndIntake(paths[0]), - new FollowPathAlignAndShoot(paths[1], new FastAlignShootSpeakerRelative(-45, 1.0)), - new FollowPathAlignAndShoot(paths[1], SwerveDriveToPose.speakerRelative(-45)), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + + new FollowPathAndIntake(paths[1]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + new FollowPathAndIntake(paths[2]), - new FollowPathAlignAndShoot(paths[3], new FastAlignShootSpeakerRelative(-45)), - new FollowPathAlignAndShoot(paths[3], SwerveDriveToPose.speakerRelative(-45)), - new FollowPathAndIntake(paths[4]), - new FollowPathAlignAndShoot(paths[5], SwerveDriveToPose.speakerRelative(-45)) + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(0.7) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java index 9c214d21..2fa0eb24 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.auton; import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -18,4 +19,8 @@ public UntilNoteShot(double timeout) { new IntakeStop() ); } + + public UntilNoteShot() { + this(Settings.Auton.SHOOT_WAIT_DELAY.get()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java index 45fe180f..34b0bb9c 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java @@ -1,15 +1,47 @@ package com.stuypulse.robot.commands.shooter; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Shoot; +import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.Derivative; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; -import de.erichseifert.vectorgraphics2d.intermediate.commands.Command; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveToShoot extends Command { private final SwerveDrive swerve; + private final Controller distanceController; + private final AnglePIDController angleController; + private final IStream velocityError; + + private final IStream distanceToSpeaker; + private final BStream isAligned; + private final Number targetDistance; + + private double distanceTolerance; + private double angleTolerance; + private double velocityTolerance; + public SwerveDriveToShoot() { this(Alignment.PODIUM_SHOT_DISTANCE); @@ -24,6 +56,100 @@ public SwerveDriveToShoot(Number targetDistance, double debounce) { swerve = SwerveDrive.getInstance(); + distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) + .setOutputFilter( + x -> -x, + x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) + ); + + angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); + + velocityError = IStream.create(distanceController::getError) + .filtered(new Derivative()) + .filtered(new LowPassFilter(0.05)) + .filtered(x -> Math.abs(x)); + + distanceToSpeaker = IStream.create(() -> getTranslationToSpeaker().getNorm()) + .filtered(new LowPassFilter(0.05)); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Rising(debounce)); + + distanceTolerance = 0.05; + angleTolerance = Alignment.ANGLE_TOLERANCE.get(); + velocityTolerance = 0.1; + + addRequirements(swerve); + + } + + private double getTargetDistance() { + return SLMath.clamp(targetDistance.doubleValue(), 1, 5); + } + + public SwerveDriveToShoot withTolerance(double distanceTolerance, double angleTolerance) { + this.distanceTolerance = distanceTolerance; + this.angleTolerance = angleTolerance; + return this; + } + + public SwerveDriveToShoot withRotationConstants(double p, double i, double d) { + angleController.setPID(p, i, d); + return this; } + private boolean isAligned() { + return distanceController.isDone(distanceTolerance) + && angleController.isDoneDegrees(angleTolerance) + && velocityError.get() < velocityTolerance; + } + + private Translation2d getTranslationToSpeaker() { + return Field.getAllianceSpeakerPose().getTranslation().minus(swerve.getPose().getTranslation()); + } + + @Override + public void initialize() { + SmartDashboard.putBoolean("AutonAlignment", true); + } + + @Override + public void execute() { + Rotation2d toSpeaker = getTranslationToSpeaker().getAngle(); + + double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); + double rotation = angleController.update( + Angle.fromRotation2d(toSpeaker).add(Angle.k180deg), + Angle.fromRotation2d(swerve.getPose().getRotation())); + + Translation2d speeds = new Translation2d( + speed, + toSpeaker); + + if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + swerve.setFieldRelativeSpeeds( + new ChassisSpeeds( + speeds.getX(), + speeds.getY(), + rotation)); + + SmartDashboard.putNumber("Alignment/Velocity Error", velocityError.get()); + SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.plus(Rotation2d.fromDegrees(180)).getDegrees()); + } + + @Override + public boolean isFinished() { + return isAligned.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + SmartDashboard.putBoolean("AutonAlignment", false); + } + + // NEED TO CHANGE SO THAT ALLOWED SHOOT DISTANCE IS ALL THE WAY FROM ALLIANCE WING LINE AND NOT AROUND PDOIUM + } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java new file mode 100644 index 00000000..68bf9bd7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -0,0 +1,183 @@ +package com.stuypulse.robot.commands.swerve; + +import java.util.function.Supplier; + +import com.pathplanner.lib.util.PIDConstants; +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.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.robot.util.MirrorRotation2d; +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 com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveToPose extends Command { + + public static SwerveDriveToPose speakerRelative(double angleToSpeaker, double distanceToSpeaker) { + MirrorRotation2d angle = MirrorRotation2d.fromBlue( + Rotation2d.fromDegrees(SLMath.clamp( + angleToSpeaker, 80))); + + double distance = SLMath.clamp(distanceToSpeaker, 1, 5); + + return new SwerveDriveToPose(() -> { + return new Pose2d( + Field.getAllianceSpeakerPose().getTranslation() + .plus(new Translation2d(distance, angle.get())), + angle.get()); + } + ); + } + + public static SwerveDriveToPose speakerRelative(double angleToSpeaker) { + return speakerRelative(angleToSpeaker, Alignment.PODIUM_SHOT_DISTANCE.get()); + } + + private final SwerveDrive swerve; + + private final HolonomicController controller; + private final Supplier poseSupplier; + private final BStream isAligned; + private final IStream velocityError; + + private final FieldObject2d targetPose2d; + + private double xTolerance; + private double yTolerance; + private double thetaTolerance; + private double velocityTolerance; + private double maxSpeed; + + private Pose2d targetPose; + + public SwerveDriveToPose(Pose2d targetPose) { + this(() -> targetPose); + } + + public SwerveDriveToPose(Supplier poseSupplier) { + swerve = SwerveDrive.getInstance(); + + this.poseSupplier = poseSupplier; + + targetPose2d = swerve.getField().getObject("Target Pose"); + maxSpeed = Swerve.MAX_MODULE_SPEED; + + 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)); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Both(Alignment.DEBOUNCE_TIME)); + + velocityError = IStream.create(() -> { + ChassisSpeeds speeds = controller.getError(); + + return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond).getNorm(); + }) + .filtered(new LowPassFilter(0.05)) + .filtered(x -> Math.abs(x)); + + xTolerance = Alignment.X_TOLERANCE.get(); + yTolerance = Alignment.Y_TOLERANCE.get(); + thetaTolerance = Alignment.ANGLE_TOLERANCE.get(); + velocityTolerance = 0.15; + + addRequirements(swerve); + } + + public SwerveDriveToPose withTranslationConstants(PIDConstants pid) { + controller.setTranslationConstants(pid.kP, pid.kI, pid.kD); + return this; + } + + public SwerveDriveToPose withRotationConstants(PIDConstants pid) { + controller.setRotationConstants(pid.kP, pid.kI, pid.kD); + return this; + } + + public SwerveDriveToPose withTranslationConstants(double p, double i, double d) { + controller.setTranslationConstants(p, i, d); + return this; + } + + public SwerveDriveToPose withRotationConstants(double p, double i, double d) { + controller.setRotationConstants(p, i, d); + return this; + } + + public SwerveDriveToPose withTolerance(Number x, Number y, Number theta) { + xTolerance = x.doubleValue(); + yTolerance = y.doubleValue(); + thetaTolerance = theta.doubleValue(); + return this; + } + + public SwerveDriveToPose withMaxSpeed(double speed) { + maxSpeed = speed; + return this; + } + + @Override + public void initialize() { + targetPose = poseSupplier.get(); + SmartDashboard.putBoolean("AutonAlignment", true); + } + + private boolean isAligned() { + return controller.isDone(xTolerance, yTolerance, thetaTolerance) + && velocityError.get() < velocityTolerance; + } + + @Override + public void execute() { + targetPose2d.setPose(targetPose); + controller.update(targetPose, swerve.getPose()); + + Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond) + .clamp(maxSpeed); + double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get()); + + SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance()); + + if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + ChassisSpeeds clamped = new ChassisSpeeds( + speed.x, speed.y, rotation); + + swerve.setChassisSpeeds(clamped); + } + + @Override + public boolean isFinished() { + return isAligned.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + Field.clearFieldObject(targetPose2d); + SmartDashboard.putBoolean("AutonAlignment", false); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d3d6556a..f58040cd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -169,6 +169,8 @@ public interface Swerve { String CAN_BUS_NAME = "swerve"; + SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); + // The stator current at which the wheels start to slip; double SLIP_CURRENT = 150.0; @@ -290,6 +292,8 @@ public interface Simulation { public interface Alignment { double DEBOUNCE_TIME = 0.05; + SmartNumber PODIUM_SHOT_DISTANCE = new SmartNumber("Shooter/Podium Distance", 2.85); + SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); @@ -312,6 +316,20 @@ public interface Rotation { SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Alignment/Rotation/Omega Deadband", 0.05); } + + public interface Shoot { + public interface Translation { + SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); + SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); + SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.7); + } + + public interface Rotation { + SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); + SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); + SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); + } + } } public interface Driver { @@ -374,4 +392,14 @@ public interface Buzz { double BUZZ_DURATION = 0.2; double BUZZ_INTENSITY = 1; } + + public interface Auton { + double MAX_SHOT_DISTANCE = 3.1; + + SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/Shoot Wait Delay", 0.45); + + double SHOOTER_STARTUP_DELAY = 0.5; + double DEFAULT_INTAKE_TIMEOUT = 0.75; + double SHOOTER_START_PRE = 1.0; + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index ba77864a..7e5d9904 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -6,17 +6,25 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.util.vision.VisionData; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; @@ -113,6 +121,46 @@ public Field2d getField() { return field; } + public void stop() { + stop(); + } + + public Command followPathCommand(String pathName) { + return followPathCommand(PathPlannerPath.fromPathFile(pathName)); + } + + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + setControl(drive.withVelocityX(chassisSpeeds.vxMetersPerSecond) + .withVelocityY(chassisSpeeds.vyMetersPerSecond) + .withRotationalRate(chassisSpeeds.omegaRadiansPerSecond) + ); + } + + public Command followPathCommand(PathPlannerPath path) { + return new FollowPathHolonomic( + path, + () -> getPose(), + () -> m_kinematics.toChassisSpeeds(m_moduleStates), + this::setChassisSpeeds, + new HolonomicPathFollowerConfig( + Motion.XY, + Motion.THETA, + Settings.Swerve.MAX_MODULE_SPEED, + Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH), + new ReplanningConfig(false, false) + ), + () -> false, + this + ); + } + + public void setFieldRelativeSpeeds(ChassisSpeeds chassisSpeeds) { + setChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( + chassisSpeeds, + getPose().getRotation())); + } + public void initFieldObject() { String[] ids = {"Front Left", "Front Right", "Back Left", "Back Right"}; for (int i = 0; i < Modules.length; i++) { diff --git a/src/main/java/com/stuypulse/robot/util/MirrorRotation2d.java b/src/main/java/com/stuypulse/robot/util/MirrorRotation2d.java new file mode 100644 index 00000000..6b167aa9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/MirrorRotation2d.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.robot.Robot; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class MirrorRotation2d { + + public static MirrorRotation2d fromBlue(Rotation2d blue) { + return new MirrorRotation2d(blue); + } + + private final Rotation2d red; + private final Rotation2d blue; + + private MirrorRotation2d(Rotation2d blue) { + this.blue = blue; + this.red = blue.times(-1); + } + + public Rotation2d get() { + if (Robot.isBlue()){ + return blue; + } + + return red; + } + +} From 6350a7d66c519914e404573dc8589a6a1b31b0e7 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 9 Aug 2024 19:01:42 -0400 Subject: [PATCH 5/9] annoying 4.9 max module speed compile bug >:( --- .../com/stuypulse/robot/subsystems/swerve/SwerveDrive.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 7e5d9904..ce025d78 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -146,7 +146,7 @@ public Command followPathCommand(PathPlannerPath path) { new HolonomicPathFollowerConfig( Motion.XY, Motion.THETA, - Settings.Swerve.MAX_MODULE_SPEED, + 4.9, Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH), new ReplanningConfig(false, false) ), @@ -269,5 +269,6 @@ public void periodic() { if (Settings.Vision.IS_ACTIVE.get() && outputs.size() > 0) { updateEstimatorWithVisionData(outputs); } + } } \ No newline at end of file From c3f9a401a9430da6834e78f780554efa303498bc Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 9 Aug 2024 19:06:34 -0400 Subject: [PATCH 6/9] reverted sob --- .../com/stuypulse/robot/subsystems/swerve/SwerveDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index ce025d78..ab276efc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -146,7 +146,7 @@ public Command followPathCommand(PathPlannerPath path) { new HolonomicPathFollowerConfig( Motion.XY, Motion.THETA, - 4.9, + Settings.Swerve.MAX_MODULE_SPEED, Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH), new ReplanningConfig(false, false) ), @@ -269,6 +269,6 @@ public void periodic() { if (Settings.Vision.IS_ACTIVE.get() && outputs.size() > 0) { updateEstimatorWithVisionData(outputs); } - + } } \ No newline at end of file From d5afe0b8001956f1698f571f39f69c3575887184 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Sat, 10 Aug 2024 07:59:09 -0400 Subject: [PATCH 7/9] changed max module speed so it will compile maybe. --- .../com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java | 2 +- .../java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java index 68bf9bd7..ffce554b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -79,7 +79,7 @@ public SwerveDriveToPose(Supplier poseSupplier) { this.poseSupplier = poseSupplier; targetPose2d = swerve.getField().getObject("Target Pose"); - maxSpeed = Swerve.MAX_MODULE_SPEED; + maxSpeed = 4.9; controller = new HolonomicController( new PIDController(Translation.kP, Translation.kI, Translation.kD), diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index ab276efc..9dcfb706 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -146,7 +146,7 @@ public Command followPathCommand(PathPlannerPath path) { new HolonomicPathFollowerConfig( Motion.XY, Motion.THETA, - Settings.Swerve.MAX_MODULE_SPEED, + 4.9, Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH), new ReplanningConfig(false, false) ), From f7facddbca751b047a05e57c34c189eb791161a9 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Mon, 12 Aug 2024 16:55:59 -0400 Subject: [PATCH 8/9] tweaked some numbers in settings, also clean up some uneeded imports so it look all pretty and stuff --- .../java/com/stuypulse/robot/RobotContainer.java | 14 -------------- .../robot/commands/auton/HGF/FourPieceHGF.java | 2 -- .../robot/commands/shooter/ShooterFerry.java | 1 - .../commands/shooter/ShooterLobFerryManual.java | 3 --- .../commands/shooter/ShooterLowFerryManual.java | 3 --- .../commands/shooter/ShooterScoreSpeaker.java | 1 - .../robot/commands/shooter/SwerveDriveToShoot.java | 1 - .../commands/swerve/SwerveDriveDriveToClimb.java | 2 -- .../robot/commands/swerve/SwerveDriveToPose.java | 1 - .../SwerveDriveDriveAlignedLobFerry.java | 2 -- .../SwerveDriveDriveAlignedLowFerry.java | 2 -- .../SwerveDriveDriveAlignedManualLobFerry.java | 3 --- .../SwerveDriveDriveAlignedManualLowFerry.java | 3 --- .../SwerveDriveDriveAndLobFerryManual.java | 8 -------- .../driveAndShoot/SwerveDriveDriveAndLowFerry.java | 4 ---- .../SwerveDriveDriveAndLowFerryManual.java | 6 ------ .../SwerveDriveDriveAndScoreSpeaker.java | 3 --- .../driveAndShoot/SwerveDriveDriveAndShoot.java | 3 --- .../com/stuypulse/robot/constants/Settings.java | 4 ++-- .../stuypulse/robot/subsystems/arm/ArmImpl.java | 1 - .../stuypulse/robot/subsystems/intake/Intake.java | 2 -- .../robot/subsystems/swerve/SwerveDrive.java | 3 --- 22 files changed, 2 insertions(+), 70 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0665d85d..1d132913 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -9,14 +9,12 @@ import com.stuypulse.robot.commands.arm.ArmToLowFerry; import com.stuypulse.robot.commands.arm.ArmToPreClimb; import com.stuypulse.robot.commands.arm.ArmToSpeaker; -import com.stuypulse.robot.commands.arm.ArmToStow; import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeStop; -import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; import com.stuypulse.robot.commands.shooter.ShooterScoreAmp; @@ -26,24 +24,16 @@ import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToClimb; -import com.stuypulse.robot.commands.swerve.SwerveDriveXMode; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLobFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry; -import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualLobFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeaker; -import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLobFerry; import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLobFerryManual; -import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLowFerry; import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndLowFerryManual; -import com.stuypulse.robot.commands.swerve.driveAndShoot.SwerveDriveDriveAndScoreSpeaker; import com.stuypulse.robot.commands.swerve.noteAlignment.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Driver; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.robot.subsystems.shooter.Shooter; @@ -51,19 +41,15 @@ import com.stuypulse.robot.subsystems.swerve.Telemetry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.subsystems.vision.NoteVision; -import com.stuypulse.robot.util.ShooterLobFerryInterpolation; -import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.Intake; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; 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 e14f55a2..eacb6a37 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 @@ -1,11 +1,9 @@ package com.stuypulse.robot.commands.auton.HGF; 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.auton.UntilNoteShot; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.constants.Settings.Auton; diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java index cd93ee2b..453a7599 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java @@ -1,6 +1,5 @@ package com.stuypulse.robot.commands.shooter; -import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLobFerryManual.java index 86db34ec..4b7a0d54 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLobFerryManual.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLobFerryManual.java @@ -1,13 +1,10 @@ package com.stuypulse.robot.commands.shooter; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLowFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLowFerryManual.java index 773c5be7..f0d5ac79 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLowFerryManual.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterLowFerryManual.java @@ -1,13 +1,10 @@ package com.stuypulse.robot.commands.shooter; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.ShooterLowFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java index c08fd2fa..76b2d342 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java @@ -1,6 +1,5 @@ package com.stuypulse.robot.commands.shooter; -import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java index 34b0bb9c..d500dbac 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java @@ -21,7 +21,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java index b4ce4e79..46f47ab5 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToClimb.java @@ -3,11 +3,9 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; 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.swerve.SwerveDrive; import com.stuypulse.robot.util.HolonomicController; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java index ffce554b..436bb9fd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -25,7 +25,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java index 682e79bd..6b928604 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java @@ -1,11 +1,9 @@ package com.stuypulse.robot.commands.swerve.driveAligned; -import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; public class SwerveDriveDriveAlignedLobFerry extends SwerveDriveDriveAligned { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java index f2795c00..70d14458 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java @@ -1,11 +1,9 @@ package com.stuypulse.robot.commands.swerve.driveAligned; -import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; public class SwerveDriveDriveAlignedLowFerry extends SwerveDriveDriveAligned { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java index d4c29d67..614d976d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java @@ -1,11 +1,8 @@ package com.stuypulse.robot.commands.swerve.driveAligned; -import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; public class SwerveDriveDriveAlignedManualLobFerry extends SwerveDriveDriveAligned { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLowFerry.java index dae7b713..a4a71e8c 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLowFerry.java @@ -1,11 +1,8 @@ package com.stuypulse.robot.commands.swerve.driveAligned; -import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; public class SwerveDriveDriveAlignedManualLowFerry extends SwerveDriveDriveAligned { diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLobFerryManual.java index 71f9259f..ac457caa 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLobFerryManual.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLobFerryManual.java @@ -1,21 +1,13 @@ package com.stuypulse.robot.commands.swerve.driveAndShoot; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.commands.shooter.ShooterFerry; -import com.stuypulse.robot.commands.shooter.ShooterLobFerryManual; -import com.stuypulse.robot.commands.shooter.ShooterLowFerryManual; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.util.ShooterLobFerryInterpolation; -import com.stuypulse.robot.util.ShooterLowFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.CommandScheduler; public class SwerveDriveDriveAndLobFerryManual extends SwerveDriveDriveAndShoot{ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerry.java index 81de6124..75df9619 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerry.java @@ -1,7 +1,5 @@ package com.stuypulse.robot.commands.swerve.driveAndShoot; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.commands.shooter.ShooterFerry; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -9,8 +7,6 @@ import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandScheduler; public class SwerveDriveDriveAndLowFerry extends SwerveDriveDriveAndShoot{ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerryManual.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerryManual.java index 8bbecf7e..0d7e19dd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerryManual.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndLowFerryManual.java @@ -1,19 +1,13 @@ package com.stuypulse.robot.commands.swerve.driveAndShoot; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.commands.shooter.ShooterFerry; -import com.stuypulse.robot.commands.shooter.ShooterLowFerryManual; import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.util.ShooterLowFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.input.Gamepad; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.CommandScheduler; public class SwerveDriveDriveAndLowFerryManual extends SwerveDriveDriveAndShoot{ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java index d510b652..3c36435c 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndScoreSpeaker.java @@ -1,7 +1,5 @@ package com.stuypulse.robot.commands.swerve.driveAndShoot; -import com.stuypulse.robot.commands.shooter.ShooterFerry; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; @@ -11,7 +9,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.CommandScheduler; public class SwerveDriveDriveAndScoreSpeaker extends SwerveDriveDriveAndShoot{ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java index 25e76b0c..97a4602f 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAndShoot/SwerveDriveDriveAndShoot.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Driver.Drive; @@ -26,9 +25,7 @@ import com.stuypulse.stuylib.util.AngleVelocity; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; public abstract class SwerveDriveDriveAndShoot extends Command { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f58040cd..511d43d5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -158,8 +158,8 @@ public interface FF { } public interface Swerve { - double WIDTH = Units.inchesToMeters(27); // intake side - double LENGTH = Units.inchesToMeters(19.25); + double WIDTH = Units.inchesToMeters(36); // intake side + double LENGTH = Units.inchesToMeters(32); double MAX_MODULE_SPEED = 4.9; double MAX_MODULE_ACCEL = 15.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index b03c6446..bf01d29a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index cd9538c3..ef6e729d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -1,7 +1,5 @@ package com.stuypulse.robot.subsystems.intake; -import com.stuypulse.robot.Robot; - import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 9dcfb706..e3fb2a34 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -13,7 +13,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; -import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Swerve.Motion; @@ -24,10 +23,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; From 0b65065388a547c0e152cdcdc1c14e4ea02055bd Mon Sep 17 00:00:00 2001 From: jopy-man Date: Mon, 12 Aug 2024 17:37:20 -0400 Subject: [PATCH 9/9] reroute paths from pathplanner + max module speed trying to fix --- .pathplanner/settings.json | 3 +- src/main/deploy/pathplanner/paths/D to E.path | 52 +++++++++++++++++ src/main/deploy/pathplanner/paths/E to D.path | 52 +++++++++++++++++ src/main/deploy/pathplanner/paths/E to F.path | 52 +++++++++++++++++ .../deploy/pathplanner/paths/E to Shoot.path | 2 +- src/main/deploy/pathplanner/paths/F to E.path | 52 +++++++++++++++++ src/main/deploy/pathplanner/paths/F to G.path | 52 +++++++++++++++++ src/main/deploy/pathplanner/paths/G to F.path | 58 +++++++++++++++++++ src/main/deploy/pathplanner/paths/G to H.path | 58 +++++++++++++++++++ src/main/deploy/pathplanner/paths/H to G.path | 58 +++++++++++++++++++ 10 files changed, 437 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/D to E.path create mode 100644 src/main/deploy/pathplanner/paths/E to D.path create mode 100644 src/main/deploy/pathplanner/paths/E to F.path create mode 100644 src/main/deploy/pathplanner/paths/F to E.path create mode 100644 src/main/deploy/pathplanner/paths/F to G.path create mode 100644 src/main/deploy/pathplanner/paths/G to F.path create mode 100644 src/main/deploy/pathplanner/paths/G to H.path create mode 100644 src/main/deploy/pathplanner/paths/H to G.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 71dc1d9a..75a4120e 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,7 +4,8 @@ "holonomicMode": true, "pathFolders": [ "ABCDE", - "HGF" + "HGF", + "Reroutes" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/deploy/pathplanner/paths/D to E.path b/src/main/deploy/pathplanner/paths/D to E.path new file mode 100644 index 00000000..9e2d9a50 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D to E.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.79, + "y": 7.44 + }, + "prevControl": null, + "nextControl": { + "x": 7.583520266556904, + "y": 6.532141954722897 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": { + "x": 7.066677306717709, + "y": 6.05322933581135 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to D.path b/src/main/deploy/pathplanner/paths/E to D.path new file mode 100644 index 00000000..11739768 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": null, + "nextControl": { + "x": 7.677216381218478, + "y": 6.4247447663422115 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.79, + "y": 7.44 + }, + "prevControl": { + "x": 7.058408588543368, + "y": 7.234165742513032 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to F.path b/src/main/deploy/pathplanner/paths/E to F.path new file mode 100644 index 00000000..0611a22d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E to F.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": null, + "nextControl": { + "x": 8.037386361231057, + "y": 5.1801103853323776 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": { + "x": 7.447759451880511, + "y": 4.608703500331107 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -28.367522393402137, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index a2fda6be..9902b876 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -51,7 +51,7 @@ "reversed": false, "folder": "ABCDE", "previewStartingState": { - "rotation": 39.01725127228337, + "rotation": -0.49534996263591174, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/F to E.path b/src/main/deploy/pathplanner/paths/F to E.path new file mode 100644 index 00000000..586f73ae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to E.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 7.538234612369523, + "y": 5.378126956030698 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": { + "x": 7.287673222109838, + "y": 5.7519018618766395 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": -29.424188910993948, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to G.path b/src/main/deploy/pathplanner/paths/F to G.path new file mode 100644 index 00000000..0dba821b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to G.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 7.929652655250251, + "y": 3.767938313406389 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": { + "x": 7.565636759807746, + "y": 2.9737087031837754 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.8764980084921, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": -28.532662399377827, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G to F.path b/src/main/deploy/pathplanner/paths/G to F.path new file mode 100644 index 00000000..58f3cbd5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G to F.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": null, + "nextControl": { + "x": 7.497323686983685, + "y": 3.4047396749927423 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": { + "x": 7.064994718717117, + "y": 4.620818133935375 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -26.210890099444256, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": -35.86786412528748, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G to H.path b/src/main/deploy/pathplanner/paths/G to H.path new file mode 100644 index 00000000..b7848fb0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G to H.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": null, + "nextControl": { + "x": 8.028733051513724, + "y": 2.080446770640565 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.03, + "y": 1.13 + }, + "prevControl": { + "x": 7.876867465974517, + "y": 1.4229875278946982 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -62.33749298525765, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -51.63406355487065, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": -37.64093244998374, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H to G.path b/src/main/deploy/pathplanner/paths/H to G.path new file mode 100644 index 00000000..8949975d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H to G.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.03, + "y": 1.13 + }, + "prevControl": null, + "nextControl": { + "x": 7.191669558190307, + "y": 1.9568005895684404 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": { + "x": 7.173353385955284, + "y": 3.0555786278983272 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -40.39965604665211, + "rotateFast": false + }, + "reversed": false, + "folder": "Reroutes", + "previewStartingState": { + "rotation": -49.521729413623916, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file