From 0c1eda6f3d21ca540442d1448ca847c9e5fc395b Mon Sep 17 00:00:00 2001 From: Lucas Han Date: Mon, 23 Sep 2024 20:58:22 -0700 Subject: [PATCH] add middle FU --- .../deploy/pathplanner/autos/middle_FU.auto | 79 +++++++++++++++++++ .../pathplanner/paths/middle-note1.1.path | 4 +- .../pathplanner/paths/middle-note1.2.path | 2 +- .../pathplanner/paths/middle-note1.3.path | 22 +----- .../pathplanner/paths/note1.2-middle.path | 52 ++++++++++++ .../{New Path.path => note1.3-middle.path} | 22 +++--- .../frc/team3128/autonomous/Trajectories.java | 48 +++-------- .../frc/team3128/commands/CmdManager.java | 38 ++++++--- 8 files changed, 183 insertions(+), 84 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/middle_FU.auto create mode 100644 src/main/deploy/pathplanner/paths/note1.2-middle.path rename src/main/deploy/pathplanner/paths/{New Path.path => note1.3-middle.path} (72%) diff --git a/src/main/deploy/pathplanner/autos/middle_FU.auto b/src/main/deploy/pathplanner/autos/middle_FU.auto new file mode 100644 index 0000000..f726b25 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/middle_FU.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ramShootNoStop" + } + }, + { + "type": "path", + "data": { + "pathName": "middle-note1.3" + } + }, + { + "type": "path", + "data": { + "pathName": "note1.3-middle" + } + }, + { + "type": "named", + "data": { + "name": "ramShootNoStop" + } + }, + { + "type": "path", + "data": { + "pathName": "middle-note1.2" + } + }, + { + "type": "path", + "data": { + "pathName": "note1.2-middle" + } + }, + { + "type": "named", + "data": { + "name": "ramShootNoStop" + } + }, + { + "type": "path", + "data": { + "pathName": "middle-note1.1" + } + }, + { + "type": "path", + "data": { + "pathName": "note1.1-middle" + } + }, + { + "type": "named", + "data": { + "name": "ramShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middle-note1.1.path b/src/main/deploy/pathplanner/paths/middle-note1.1.path index 80f9397..f8d3b1c 100644 --- a/src/main/deploy/pathplanner/paths/middle-note1.1.path +++ b/src/main/deploy/pathplanner/paths/middle-note1.1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "Intake", + "name": "intakeAndStop", "waypointRelativePos": 0, "command": { "type": "parallel", @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "intakeAndStop" } } ] diff --git a/src/main/deploy/pathplanner/paths/middle-note1.2.path b/src/main/deploy/pathplanner/paths/middle-note1.2.path index 1cc3d8b..9b3a60e 100644 --- a/src/main/deploy/pathplanner/paths/middle-note1.2.path +++ b/src/main/deploy/pathplanner/paths/middle-note1.2.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "intakeAndStop" } } ] diff --git a/src/main/deploy/pathplanner/paths/middle-note1.3.path b/src/main/deploy/pathplanner/paths/middle-note1.3.path index 80c6d63..278b5f8 100644 --- a/src/main/deploy/pathplanner/paths/middle-note1.3.path +++ b/src/main/deploy/pathplanner/paths/middle-note1.3.path @@ -23,32 +23,16 @@ "x": 2.5792893218813453, "y": 4.470710678118655 }, - "nextControl": { - "x": 2.911629509039022, - "y": 4.138370490960978 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.2, - "y": 4.78 - }, - "prevControl": { - "x": 2.1292893218813456, - "y": 4.850710678118655 - }, "nextControl": null, "isLocked": false, - "linkedName": "note1.3-diag" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [ { - "name": "Intake", + "name": "intakeAndStop", "waypointRelativePos": 0, "command": { "type": "parallel", @@ -57,7 +41,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "intakeAndStop" } } ] diff --git a/src/main/deploy/pathplanner/paths/note1.2-middle.path b/src/main/deploy/pathplanner/paths/note1.2-middle.path new file mode 100644 index 0000000..cdca137 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/note1.2-middle.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.55, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.4694476010769155, + "y": 5.537726865962991 + }, + "isLocked": false, + "linkedName": "note1.2" + }, + { + "anchor": { + "x": 1.4, + "y": 5.55 + }, + "prevControl": { + "x": 1.5166108734633559, + "y": 5.53796254771329 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.6, + "maxAcceleration": 6.0, + "maxAngularVelocity": 720.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": true + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/note1.3-middle.path similarity index 72% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/note1.3-middle.path index 1bd7218..cd939fb 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/note1.3-middle.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.65, + "y": 4.4 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 2.5769342825867363, + "y": 4.404664289380663 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 1.4, + "y": 5.55 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 1.4791690231166925, + "y": 5.471893715633497 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "middle" } ], "rotationTargets": [], @@ -39,8 +39,8 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 180.0, + "rotateFast": true }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index e376ceb..b9fe40e 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -35,11 +35,13 @@ import frc.team3128.Constants.ShooterConstants; import frc.team3128.Robot; import frc.team3128.RobotContainer; +import frc.team3128.commands.CmdManager; import frc.team3128.commands.CmdSwerveDrive; import java.util.function.DoubleSupplier; import frc.team3128.subsystems.Swerve; +import frc.team3128.subsystems.Intake; /** * Store trajectories for autonomous. Edit points here. @@ -47,22 +49,6 @@ */ public class Trajectories { - //USED FOR HARDCODED SHOTS BITCHES - public enum ShootPosition { - // find values - WING(5.09), //Change this for top - BOTTOM(7.5), //Change this for bottom auto - RAM(24.5); - - private final double height; - ShootPosition(double height) { - this.height = height; - } - public double getHeight() { - return height; - } - } - private static final Swerve swerve = Swerve.getInstance(); private static double vx = 0, vy = 0; private static boolean turning = false; @@ -71,20 +57,9 @@ public static void initTrajectories() { Pathfinding.setPathfinder(new LocalADStar()); // TODO: add commands - // NamedCommands.registerCommand("Intake", intake.intakeAuto()); - // NamedCommands.registerCommand("Shoot", autoShoot(0.75)); - // NamedCommands.registerCommand("RamShoot", ramShotAuto()); - // NamedCommands.registerCommand("BottomShoot", autoShootPreset(ShootPosition.BOTTOM)); - // NamedCommands.registerCommand("WingRamp", rampUpAuto(ShootPosition.WING)); - // NamedCommands.registerCommand("Align", align()); - // NamedCommands.registerCommand("AlignCCW", alignSearch(true)); - // NamedCommands.registerCommand("AlignCW", alignSearch(false)); - // NamedCommands.registerCommand("Outtake", outtakeAuto()); - // NamedCommands.registerCommand("Retract", intake.retractAuto()); - // NamedCommands.registerCommand("Neutral", neutralAuto()); - // NamedCommands.registerCommand("AlignPreload", alignPreload(false)); - // NamedCommands.registerCommand("Drop", shooter.shoot(MAX_RPM)); - // NamedCommands.registerCommand("RamShootMax", autoShootPreset(ShootPosition.RAM)); + NamedCommands.registerCommand("ramShoot", CmdManager.ramShoot(true)); + NamedCommands.registerCommand("ramShootNoStop", CmdManager.ramShootNoStop(true)); + NamedCommands.registerCommand("intakeAndStop", CmdManager.intakeAndStop(Intake.Setpoint.GROUND)); AutoBuilder.configureHolonomic( swerve::getPose, @@ -108,7 +83,6 @@ public static Command resetAuto() { runOnce(()-> swerve.zeroGyro(Robot.getAlliance() == Alliance.Red ? 0 : 180)), runOnce(()-> swerve.resetEncoders()) ); - } public static void drive(ChassisSpeeds velocity) { @@ -130,7 +104,6 @@ public static Command turnDegrees(boolean counterClockwise, double angle) { setpoint, //setpoint (double output) -> { Swerve.getInstance().drive(new ChassisSpeeds(vx, vy, Units.degreesToRadians(output))); - NAR_Shuffleboard.addData("HElp", "help", output, 0, 0); } ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn())); } @@ -146,7 +119,6 @@ public static Command turnInPlace() { setpoint, //setpoint (double output) -> { Swerve.getInstance().drive(new ChassisSpeeds(vx, vy, Units.degreesToRadians(output))); - NAR_Shuffleboard.addData("HElp", "help", output, 0, 0); } ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn())); } @@ -157,11 +129,11 @@ public static Command getPathPlannerAuto(String name) { public static Command goToPoint(Pose2d pose) { return AutoBuilder.pathfindToPose( - pose, - AutoConstants.constraints, - 0.0, // Goal end velocity in meters/sec - 0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. - ); + pose, + AutoConstants.constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. + ); } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index b04c6b9..ed2f5d0 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -68,23 +68,29 @@ public static Command kick(boolean once) { public static Command ramShoot(boolean once) { return sequence( - rampUpShoot(), - waitUntil(() -> shooter.atSetpoint()), - kick(once), + ramShootNoStop(once), shooter.stopMotors() ); } - public static Command autoShoot(boolean once) { - return deadline( + public static Command ramShootNoStop(boolean once) { + return sequence( rampUpShoot(), - repeatingSequence( - runOnce(()-> CmdSwerveDrive.setTurnSetpoint(swerve.getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue))), - waitSeconds(0.1) - ) - ).andThen(ramShoot(once)).andThen(runOnce(() -> CmdSwerveDrive.disableTurn())); + waitUntil(()->shooter.atSetpoint()), + kick(once) + ); } + // public static Command autoShoot(boolean once) { + // return deadline( + // rampUpShoot(), + // repeatingSequence( + // runOnce(()-> CmdSwerveDrive.setTurnSetpoint(swerve.getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue))), + // waitSeconds(0.1) + // ) + // ).andThen(ramShoot(once)).andThen(runOnce(() -> CmdSwerveDrive.disableTurn())); + // } + public static Command rampUpAmp() { return sequence( shooter.shoot(AMP_RPM), @@ -107,14 +113,13 @@ public static Command amp(boolean once) { // } - public static Command intake(Setpoint setpoint) { + public static Command intake(Intake.Setpoint setpoint) { return sequence( intake.pivotTo(setpoint), intake.runIntakeRollers(), hopper.runManipulator(HOPPER_INTAKE_POWER), waitUntil(() -> shooter.noteInKick()), - waitUntil(() -> hopper.noteInFront()), - stopIntake() + waitUntil(() -> hopper.noteInFront()) ); } @@ -126,6 +131,13 @@ public static Command stopIntake() { ); } + public static Command intakeAndStop(Intake.Setpoint setpoint) { + return sequence( + intake(setpoint), + stopIntake() + ); + } + public static Command feed(double rpm, double angle, boolean once) { return sequence( parallel(