From ea242ddac4d28c0ac2874b73f81813b5232cdb93 Mon Sep 17 00:00:00 2001 From: Craftzman7 Date: Wed, 13 Sep 2023 09:36:00 -0700 Subject: [PATCH] feat(auto): add shoot in place option --- .../frc/robot/commands/autonomous/AutoBalance.java | 2 +- .../java/frc/robot/commands/autonomous/AutoShoot.java | 10 ++++++++-- src/main/java/frc/robot/utils/AutoLanguage.java | 3 ++- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/autonomous/AutoBalance.java b/src/main/java/frc/robot/commands/autonomous/AutoBalance.java index c7d49d8..d8469f9 100644 --- a/src/main/java/frc/robot/commands/autonomous/AutoBalance.java +++ b/src/main/java/frc/robot/commands/autonomous/AutoBalance.java @@ -43,7 +43,7 @@ public AutoBalance (Drivetrain drivetrain, PoseEstimation poseEstimation) { new GenerateCommand( () -> { try { - Pose2d targetXPosition = new Pose2d(AllianceUtils.allianceToField(Constants.AutoConstants.BALANCE_X_POSITION), poseEstimation.getEstimatedPose().getY(), poseEstimation.getEstimatedPose().getRotation()); + Pose2d targetXPosition = new Pose2d(AllianceUtils.allianceToField(Constants.AutoConstants.BALANCE_X_POSITION), poseEstimation.getEstimatedPose().getY(), Rotation2d.fromDegrees(0)); PathPoint targetPoint = Constants.AutoConstants.BALANCING_OUTER_PARTITION.queryWaypoint(poseEstimation.getEstimatedPose(), targetXPosition).orElseGet( () -> Constants.AutoConstants.BALANCING_INNER_PARTITION.queryWaypoint(poseEstimation.getEstimatedPose(), targetXPosition).orElseThrow() ); diff --git a/src/main/java/frc/robot/commands/autonomous/AutoShoot.java b/src/main/java/frc/robot/commands/autonomous/AutoShoot.java index 1282aa7..cb223e7 100644 --- a/src/main/java/frc/robot/commands/autonomous/AutoShoot.java +++ b/src/main/java/frc/robot/commands/autonomous/AutoShoot.java @@ -17,11 +17,17 @@ import com.pathplanner.lib.PathPoint; public class AutoShoot extends SequentialCommandGroup { - public AutoShoot(Drivetrain drivetrain, Arm arm, PoseEstimation poseEstimation) { + public AutoShoot(Drivetrain drivetrain, Arm arm, PoseEstimation poseEstimation, boolean shouldShootInPlace) { Pose2d shootPose = AllianceUtils.allianceToField(Constants.AutoConstants.CUBE_SHOOT_POSITION); + // I'm probably doing this wrong. Someone please tell me how to do this right. + if (!shouldShootInPlace) { + this.addCommands( + new FollowTrajectoryToState(drivetrain, poseEstimation, new PathPoint(shootPose.getTranslation(), shootPose.getRotation(), shootPose.getRotation(), 2), true) + ); + } this.addCommands( - // AllianceUtils.allianceToField(new Pose2d(new Translation2d(3.76, 4.86), new Rotation2d(180))) new FollowTrajectoryToState(drivetrain, poseEstimation, new PathPoint(shootPose.getTranslation(), shootPose.getRotation(), shootPose.getRotation(), 2), true), + // AllianceUtils.allianceToField(new Pose2d(new Translation2d(3.76, 4.86), new Rotation2d(180))) new InstantCommand(() -> arm.setTarget(Arm.State.High)), new WaitCommand(0.25), new InstantCommand(() -> arm.setRollerState(Rollers.State.Outtake)), diff --git a/src/main/java/frc/robot/utils/AutoLanguage.java b/src/main/java/frc/robot/utils/AutoLanguage.java index e2be6f3..bd6f4d1 100644 --- a/src/main/java/frc/robot/utils/AutoLanguage.java +++ b/src/main/java/frc/robot/utils/AutoLanguage.java @@ -56,8 +56,9 @@ private static Command compileStatement(String source) { return new InstantCommand(() -> RobotContainer.arm.setGamePiece(scorePiece)). andThen(new AutoScore(RobotContainer.drivetrain, RobotContainer.arm, RobotContainer.poseEstimation, () -> node)); case "shoot": + Boolean shouldShootInPlace = tokens.length > 1 ? Boolean.parseBoolean(tokens[1]) : false; return new GenerateCommand( - () -> new AutoShoot(RobotContainer.drivetrain, RobotContainer.arm, RobotContainer.poseEstimation), + () -> new AutoShoot(RobotContainer.drivetrain, RobotContainer.arm, RobotContainer.poseEstimation, shouldShootInPlace), Set.of(RobotContainer.drivetrain) ); case "balance":