From 29d5dc0c2e05bc016efee38acd951dbf6d048609 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sun, 29 Sep 2024 12:43:40 -0400 Subject: [PATCH] one piece autos --- .../paths/Blue Amp Side Mobility.path | 52 +++++++++++++++++++ .../pathplanner/paths/Blue Center to B.path | 2 +- .../paths/Blue Source Side Mobility.path | 52 +++++++++++++++++++ .../paths/Red Amp Side Mobility.path | 52 +++++++++++++++++++ .../pathplanner/paths/Red Center to B.path | 2 +- .../paths/Red Source Side Mobility.path | 52 +++++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 41 ++++++++------- .../auton/SideAutons/OnePieceAmpSide.java | 12 ++++- .../auton/SideAutons/OnePieceSourceSide.java | 3 ++ 9 files changed, 247 insertions(+), 21 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path create mode 100644 src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path create mode 100644 src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path create mode 100644 src/main/deploy/pathplanner/paths/Red Source Side Mobility.path diff --git a/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path new file mode 100644 index 00000000..9e7c37c3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7023114046228107, + "y": 6.739121939782341 + }, + "prevControl": null, + "nextControl": { + "x": 1.702311404622812, + "y": 6.739121939782341 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9026396514331507, + "y": 6.9956624528633675 + }, + "prevControl": { + "x": 1.9026396514331507, + "y": 6.9956624528633675 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": 60.94539590092292, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Center to B.path b/src/main/deploy/pathplanner/paths/Blue Center to B.path index 23c3a136..e2497fd3 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to B.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to B.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path new file mode 100644 index 00000000..20c43361 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7319122330552369, + "y": 4.351321779566637 + }, + "prevControl": null, + "nextControl": { + "x": 1.5903362575955933, + "y": 2.8614134151345225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.563724819757333, + "y": 1.4405736503780713 + }, + "prevControl": { + "x": 2.563724819757333, + "y": 1.4405736503780713 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": -58.91249467652, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path new file mode 100644 index 00000000..45101df0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7023114046228107, + "y": 6.739121939782341 + }, + "prevControl": null, + "nextControl": { + "x": 1.702311404622812, + "y": 6.739121939782341 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9026396514331507, + "y": 6.9956624528633675 + }, + "prevControl": { + "x": 1.9026396514331507, + "y": 6.9956624528633675 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": 60.94539590092292, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Center to B.path b/src/main/deploy/pathplanner/paths/Red Center to B.path index 5709f4d3..85c17027 100644 --- a/src/main/deploy/pathplanner/paths/Red Center to B.path +++ b/src/main/deploy/pathplanner/paths/Red Center to B.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path new file mode 100644 index 00000000..7b9b8f2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7319122330552369, + "y": 4.351321779566637 + }, + "prevControl": null, + "nextControl": { + "x": 1.5903362575955933, + "y": 2.8614134151345225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.563724819757333, + "y": 1.4405736503780713 + }, + "prevControl": { + "x": 2.563724819757333, + "y": 1.4405736503780713 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Misc", + "previewStartingState": { + "rotation": -58.91249467652, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f2f9a3ac..e876faca 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,9 +1,6 @@ package com.stuypulse.robot; -import java.util.concurrent.atomic.AtomicBoolean; - import com.ctre.phoenix6.Utils; -import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.arm.ArmToAmp; import com.stuypulse.robot.commands.arm.ArmToClimbing; @@ -19,7 +16,6 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; -import com.stuypulse.robot.commands.auton.BCA.AltFourPieceBCA; import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF; import com.stuypulse.robot.commands.auton.SideAutons.OnePieceAmpSide; @@ -28,40 +24,32 @@ import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.leds.LEDDefaultMode; -import com.stuypulse.robot.commands.leds.LEDReset; import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.shooter.ShooterFeederAcquire; import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; 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.driveAligned.SwerveDriveDriveAlignedAmp; +import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeaker; import com.stuypulse.robot.commands.vision.VisionChangeWhiteList; import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; -import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.constants.LEDInstructions; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.swerve.Telemetry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; -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 com.stuypulse.robot.subsystems.leds.LEDController; -import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction; -import com.stuypulse.robot.subsystems.leds.instructions.LEDPulseColor; -import com.stuypulse.robot.subsystems.leds.instructions.LEDRainbow; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -306,6 +294,23 @@ public void configureAutons() { AutonConfig ADEF_RED = new AutonConfig("5 ADEF", FivePieceADEF::new, "Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F"); + // AutonConfig AMP_SIDE_ONE_PIECE_BLUE = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, + // "Blue Amp Side Mobility"); + // AutonConfig AMP_SIDE_ONE_PIECE_RED = new AutonConfig("Amp Side One Piece", OnePieceAmpSide::new, + // "Red Amp Side Mobility"); + + AutonConfig SOURCE_SIDE_ONE_PIECE_BLUE = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, + "Blue Source Side Mobility"); + + AutonConfig SOURCE_SIDE_ONE_PIECE_RED = new AutonConfig("Source Side One Piece", OnePieceSourceSide::new, + "Red Source Side Mobility"); + + // AMP_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser); + // AMP_SIDE_ONE_PIECE_RED.registerRed(autonChooser); + + SOURCE_SIDE_ONE_PIECE_BLUE.registerBlue(autonChooser); + SOURCE_SIDE_ONE_PIECE_RED.registerRed(autonChooser); + MOBILITY_BLUE.registerBlue(autonChooser); MOBILITY_RED.registerRed(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java index 5f8102bb..6b9dcebc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java @@ -3,9 +3,14 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class OnePieceAmpSide extends SequentialCommandGroup { @@ -15,8 +20,13 @@ public OnePieceAmpSide(PathPlannerPath... paths) { ShootRoutine.fromSubwoofer(), new ArmToFeed(), + new WaitCommand(10), + + new IntakeSetAcquire(), // Mobility - SwerveDrive.getInstance().followPathCommand(paths[0]) + SwerveDrive.getInstance().followPathCommand(paths[0]), + new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).andThen(ShootRoutine.fromAnywhere()) + .onlyIf(() -> Intake.getInstance().hasNote() || Shooter.getInstance().hasNote()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java index 405651c3..56695c90 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java @@ -6,6 +6,7 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class OnePieceSourceSide extends SequentialCommandGroup { @@ -15,6 +16,8 @@ public OnePieceSourceSide(PathPlannerPath... paths) { ShootRoutine.fromSubwoofer(), new ArmToFeed(), + new WaitCommand(10), + // Mobility SwerveDrive.getInstance().followPathCommand(paths[0]) );