feat(auto): add shoot in place option

FRC3636 · Sep 13, 2023 · ea242dd · ea242dd
1 parent bc421a6
commit ea242dd
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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":