From 1ae0d48a83cb255dfff7a22eb3706985f31f767d Mon Sep 17 00:00:00 2001
From: mixxlto <maxtan0626@gmail.com>
Date: Wed, 21 Jan 2026 16:46:52 -0800
Subject: [PATCH] fda

---
 .../frc/robot/commands/gpm/TurretJoyStickAim.java   |  4 ++--
 .../robot/controls/PS5ControllerDriverConfig.java   | 13 +++++++++++++
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java b/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
index a383128..8be9b99 100644
--- a/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretJoyStickAim.java
@@ -1,10 +1,10 @@
 package frc.robot.commands.gpm;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.controls.BaseDriverConfig;
-import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.turret.Turret;
 
 public class TurretJoyStickAim extends Command{
@@ -16,7 +16,7 @@ public class TurretJoyStickAim extends Command{
     }
 
     Rotation2d rotation2d = new Rotation2d(driver.getRawSideTranslation(), driver.getRawForwardTranslation());
-    double angle = Units.radiansToDegrees(rotation2d.getDegrees());
+    double angle = Units.radiansToDegrees(MathUtil.angleModulus(rotation2d.getDegrees()));
 
     @Override
     public void execute() {
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index e7af91c..8c15939 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Robot;
 import frc.robot.commands.gpm.TurretAutoShoot;
+import frc.robot.commands.gpm.TurretJoyStickAim;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
@@ -37,6 +38,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
 
     private Pose2d alignmentPose = null;
     private Command turretAutoShoot;
+    private TurretJoyStickAim turretJoyStickAim;
 
     public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) {
         super(drive);
@@ -87,6 +89,17 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                         }
                     })
         );
+
+        driver.get(PS5Button.CROSS).onTrue(
+            new InstantCommand(()->{
+                        turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain());
+                        CommandScheduler.getInstance().schedule(turretAutoShoot);
+                    })
+        ).onFalse(
+            new InstantCommand(()->{
+                turretAutoShoot.cancel();
+            })
+        );
     }
     
     public void setAlignmentPose(){
-- 
2.39.5