Add cube shooting to auto routine

.gitignore

@@ -160,3 +160,5 @@ bin/
 
 # Simulation GUI and other tools window save file
 simgui*
+networktables.json
+*.wpilog

src/main/java/frc/robot/Constants.java

@@ -315,6 +315,8 @@ public static final class AutoConstants {
 
         };
 
+        public static final Pose2d CUBE_SHOOT_POSITION = new Pose2d(new Translation2d(3.76, 4.86), Rotation2d.fromRotations(0.5)); // leave it
+
         public static final double INTAKE_CONTROL_HANDLE = 3;
     }
 

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()
                                 );

src/main/java/frc/robot/commands/autonomous/AutoShoot.java

@@ -1,27 +1,69 @@
 package frc.robot.commands.autonomous;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint;
 import edu.wpi.first.wpilibj2.command.*;
 import frc.robot.Constants;
-import frc.robot.commands.alignment.AlignToSelectedNode;
+import frc.robot.RobotContainer;
+import frc.robot.commands.pathgeneration.FollowTrajectoryToPose;
 import frc.robot.commands.pathgeneration.FollowTrajectoryToState;
 import frc.robot.poseestimation.PoseEstimation;
 import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.Rollers;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.utils.AllianceUtils;
-import frc.robot.utils.Node;
+import frc.robot.utils.GenerateCommand;
 
-import java.util.function.Supplier;
+import java.time.Instant;
+import java.util.Set;
 
 import com.pathplanner.lib.PathPoint;
 
-public class AutoShoot extends SequentialCommandGroup {
-        public AutoShoot(Drivetrain drivetrain, Arm arm, PoseEstimation poseEstimation) {
-                this.addCommands(
-                        new FollowTrajectoryToState(drivetrain, poseEstimation,new PathPoint(new Translation2d(3.76, 4.86), new Rotation2d(0), new Rotation2d(180), 4), null)
-
+public class AutoShoot extends GenerateCommand {
+        public AutoShoot(Drivetrain drivetrain, boolean shouldShootInPlace) {
+                super(
+                        () -> { 
+                                if (!shouldShootInPlace) {
+                                        Pose2d shootPose = AllianceUtils.allianceToField(Constants.AutoConstants.CUBE_SHOOT_POSITION);
+                                        FollowTrajectoryToState driveCommand = new FollowTrajectoryToState(
+                                                drivetrain,
+                                                RobotContainer.poseEstimation,
+                                                new PathPoint(
+                                                        shootPose.getTranslation(),
+                                                        shootPose.getRotation(),
+                                                        shootPose.getRotation(),
+                                                        2
+                                                ),
+                                                false);
+                                        return driveCommand.andThen(
+                                                new InstantCommand(() -> {
+                                                        RobotContainer.arm.setTarget(Arm.State.High);
+                                                }),
+                                                new InstantCommand(() -> {
+                                                        RobotContainer.arm.setRollerState(Rollers.State.Outtake);
+                                                }),
+                                                new WaitCommand(0.25),
+                                                new InstantCommand(() -> RobotContainer.arm.setRollerState(Rollers.State.Off)),
+                                                new InstantCommand(() -> RobotContainer.arm.setTarget(Arm.State.Stowed))
+                                        );
+                                } else {
+                                        return new SequentialCommandGroup(
+                                                new InstantCommand(() -> {
+                                                        RobotContainer.arm.setTarget(Arm.State.High);
+                                                }),
+                                                new InstantCommand(() -> {
+                                                        RobotContainer.arm.setRollerState(Rollers.State.Outtake);
+                                                }),
+                                                new WaitCommand(0.25),
+                                                new InstantCommand(() -> RobotContainer.arm.setRollerState(Rollers.State.Off)),
+                                                new InstantCommand(() -> RobotContainer.arm.setTarget(Arm.State.Stowed))  
+                                        );
+                                }
+
+                        }, 
+                        Set.of(drivetrain)
                 );
         }
 }

src/main/java/frc/robot/utils/AutoLanguage.java

@@ -12,6 +12,7 @@
 import frc.robot.commands.autonomous.AutoBalance;
 import frc.robot.commands.autonomous.AutoIntake;
 import frc.robot.commands.autonomous.AutoScore;
+import frc.robot.commands.autonomous.AutoShoot;
 import frc.robot.commands.autonomous.DropGamePiece;
 import frc.robot.commands.pathgeneration.FollowTrajectoryToPose;
 import frc.robot.commands.pathgeneration.FollowTrajectoryToState;
@@ -54,8 +55,9 @@ private static Command compileStatement(String source) {
                 Node node = new Node(scorePiece, level, column, grid);
                 return new InstantCommand(() -> RobotContainer.arm.setGamePiece(scorePiece)).
                         andThen(new AutoScore(RobotContainer.drivetrain, RobotContainer.arm, RobotContainer.poseEstimation, () -> node));
-            //case "shoot":
-            //    return new AutoShoot(RobotContainer.drivetrain, RobotContainer.poseEstimation);
+            case "shoot":
+                Boolean shouldShootInPlace = tokens.length > 1 ? Boolean.parseBoolean(tokens[1]) : false;
+               return new AutoShoot(RobotContainer.drivetrain, shouldShootInPlace);
             case "balance":
                 return new AutoBalance(RobotContainer.drivetrain, RobotContainer.poseEstimation);
             case "leave_community":