From c1913a9a1ba6b52f44d187bdd43c3f6baacdc0c6 Mon Sep 17 00:00:00 2001 From: Sana Shah <54493565+sanashah007@users.noreply.github.com> Date: Tue, 4 Apr 2023 16:05:13 -0700 Subject: [PATCH] feat: BottomGrid2_5TaxiConeVision slay --- .../BottomGrid2_5PieceTaxiConeVision.path | 99 +++++++++++++++++++ .../team1540/robot2023/RobotContainer.java | 2 +- .../AutoBottomGrid2_5PieceTaxiConeVision.java | 82 +++++++++++++++ 3 files changed, 182 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/BottomGrid2_5PieceTaxiConeVision.path create mode 100644 src/main/java/org/team1540/robot2023/commands/auto/AutoBottomGrid2_5PieceTaxiConeVision.java diff --git a/src/main/deploy/pathplanner/BottomGrid2_5PieceTaxiConeVision.path b/src/main/deploy/pathplanner/BottomGrid2_5PieceTaxiConeVision.path new file mode 100644 index 0000000..2efa535 --- /dev/null +++ b/src/main/deploy/pathplanner/BottomGrid2_5PieceTaxiConeVision.path @@ -0,0 +1,99 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8710528497445884, + "y": 0.4647735250331603 + }, + "prevControl": null, + "nextControl": { + "x": 2.4412241867777134, + "y": 0.10510098638034204 + }, + "holonomicAngle": 179.52224912952565, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.21035572686416, + "y": 0.8017943092965504 + }, + "prevControl": { + "x": 5.941248284591866, + "y": 0.8017943092965504 + }, + "nextControl": { + "x": 5.941248284591866, + "y": 0.8017943092965504 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.691366753254689, + "y": 1.0457402965965006 + }, + "prevControl": { + "x": 2.3305118957485313, + "y": 1.0856858486827012 + }, + "nextControl": { + "x": 2.3305118957485313, + "y": 1.0856858486827012 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.547541871955501, + "y": 1.4577746279287969 + }, + "prevControl": { + "x": 5.767665810946437, + "y": -0.2123554796901983 + }, + "nextControl": null, + "holonomicAngle": -123.87016329772545, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/org/team1540/robot2023/RobotContainer.java b/src/main/java/org/team1540/robot2023/RobotContainer.java index 4c5ce9d..21d0105 100644 --- a/src/main/java/org/team1540/robot2023/RobotContainer.java +++ b/src/main/java/org/team1540/robot2023/RobotContainer.java @@ -207,7 +207,7 @@ private void initAutos() { manager.addAuto(new AutoTopGrid2PieceVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight)); manager.addAuto(new AutoBottomGrid2PieceTaxiVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight)); manager.addAuto(new AutoBottomGrid2_5PieceTaxiVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight)); - + manager.addAuto(new AutoBottomGrid2_5PieceTaxiConeVision(drivetrain, arm, intake, LimelightManager.getInstance().rearLimelight)); //manager.addAuto(new Auto2PieceTaxiConeVision(drivetrain, arm, intake, ScoringGridLocation.TOP_GRID)); // manager.addAuto(new Auto2PieceTaxi(drivetrain, arm, intake, ScoringGridLocation.BOTTOM_GRID)); // manager.addAuto("MiddleGrid1PieceSideBalance", new Auto1PieceSideBalance(drivetrain, arm, intake)); diff --git a/src/main/java/org/team1540/robot2023/commands/auto/AutoBottomGrid2_5PieceTaxiConeVision.java b/src/main/java/org/team1540/robot2023/commands/auto/AutoBottomGrid2_5PieceTaxiConeVision.java new file mode 100644 index 0000000..1f10a0e --- /dev/null +++ b/src/main/java/org/team1540/robot2023/commands/auto/AutoBottomGrid2_5PieceTaxiConeVision.java @@ -0,0 +1,82 @@ +package org.team1540.robot2023.commands.auto; + +import com.pathplanner.lib.PathConstraints; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ProxyCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import org.team1540.robot2023.Constants; +import org.team1540.robot2023.commands.arm.Arm; +import org.team1540.robot2023.commands.arm.PivotCommand; +import org.team1540.robot2023.commands.arm.ResetArmPositionCommand; +import org.team1540.robot2023.commands.arm.SetArmPosition; +import org.team1540.robot2023.commands.drivetrain.Drivetrain; +import org.team1540.robot2023.commands.grabber.GrabberIntakeCommand; +import org.team1540.robot2023.commands.grabber.GrabberOuttakeCommand; +import org.team1540.robot2023.commands.grabber.WheeledGrabber; +import org.team1540.robot2023.commands.vision.DriveToGamePiece; +import org.team1540.robot2023.commands.vision.TurnToGamePiece; +import org.team1540.robot2023.utils.AutoCommand; +import org.team1540.robot2023.utils.Limelight; +import org.team1540.robot2023.utils.PolePosition; + +import java.util.List; + +public class AutoBottomGrid2_5PieceTaxiConeVision extends AutoCommand { + public AutoBottomGrid2_5PieceTaxiConeVision(Drivetrain drivetrain, Arm arm, WheeledGrabber intake, Limelight limelight) { + List pathCommands = getPathPlannerDriveCommandGroup(drivetrain,"BottomGrid2_5PieceTaxiConeVision", new PathConstraints[]{ + new PathConstraints(2,1), + new PathConstraints(4,2) + }, true); + limelight.setPipeline(Limelight.Pipeline.GAME_PIECE); + setName("BottomGrid2.5PieceTaxiConeVision"); + addCommands( + Commands.deadline( + new SetArmPosition(arm, Constants.Auto.highCube.approach), + Commands.sequence( + new ProxyCommand(() -> new WaitCommand((arm.timeToState(Constants.Auto.highCube.approach)-150)/1000)), + new GrabberOuttakeCommand(intake, 0.5) + ) + ), + Commands.parallel( + new GrabberIntakeCommand(intake), + Commands.sequence( + Commands.parallel( + pathCommands.get(0), + Commands.sequence( + new ResetArmPositionCommand(arm), + new PivotCommand(arm, Constants.Auto.armDownBackwards) + ) + ), + Commands.sequence( + new TurnToGamePiece(drivetrain, null, TurnToGamePiece.GamePiece.CUBE ), + new DriveToGamePiece(drivetrain, () -> Constants.Auto.autoDriveDistance) + ), + Commands.parallel( + new SetArmPosition(arm, Constants.Auto.midCube.approach), + pathCommands.get(1) + ) + ) + ), + new GrabberOuttakeCommand(intake,1).withTimeout(0.1), + Commands.parallel( + Commands.sequence( + new ResetArmPositionCommand(arm), + new PivotCommand(arm, Constants.Auto.armDownBackwards) + ), + pathCommands.get(2) + ), + Commands.parallel( + new GrabberIntakeCommand(intake), + Commands.parallel( + new TurnToGamePiece(drivetrain, null, TurnToGamePiece.GamePiece.CONE ), + new DriveToGamePiece(drivetrain, () -> Constants.Auto.autoDriveDistance) + ) + ) +// new AutoGridScore(drivetrain, arm, Constants.Auto.midCube.withPolePosition(PolePosition.CENTER), intake, null, false) + + ); + } + + +} \ No newline at end of file