From 9d47987242e175a94699e77e31306335f5fab5a2 Mon Sep 17 00:00:00 2001
From: Wesley28w <wesleycwong@gmail.com>
Date: Sat, 24 Jan 2026 15:40:57 -0800
Subject: [PATCH] SOTM

---
 .../robot/commands/gpm/TurretAutoShoot.java   | 25 ++++++++++++++++---
 .../controls/PS5ControllerDriverConfig.java   | 15 ++++++++++-
 .../frc/robot/subsystems/turret/Turret.java   |  1 -
 3 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index 24feada..057e8da 100644
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -2,6 +2,7 @@ package frc.robot.commands.gpm;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -16,6 +17,8 @@ public class TurretAutoShoot extends Command {
     private Drivetrain drivetrain;
     private TurretVision turretVision;
 
+    public boolean SOTM;
+
     double fieldAngleRad;
     double turretSetpoint;
     double adjustedSetpoint;
@@ -24,23 +27,37 @@ public class TurretAutoShoot extends Command {
 
     private boolean turretVisionEnabled = false;
 
-    public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision){
+    public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision, boolean SOTM){
         this.turret = turret;
         this.drivetrain = drivetrain;
         this.turretVision = turretVision;
+        
+        this.SOTM = SOTM;
 
         addRequirements(turret);
     }
 
-    public TurretAutoShoot(Turret turret, Drivetrain drivetrain){
+    public TurretAutoShoot(Turret turret, Drivetrain drivetrain, boolean SOTM){
         this(turret, drivetrain, null);
+        this.SOTM = SOTM;
     }
 
     public void updateTurretSetpoint() {
         Translation2d drivepose = drivetrain.getPose().getTranslation();
         Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
-        double D_y = target.getY() - drivepose.getY();
-        double D_x = target.getX() - drivepose.getX();
+        double D_y;
+        double D_x;
+        if (SOTM) {
+            ChassisSpeeds chassisSpeed = drivetrain.getChassisSpeeds();
+            double xVel = chassisSpeed.vxMetersPerSecond;
+            double yVel = chassisSpeed.vyMetersPerSecond;
+            
+            D_y = target.getY() - drivepose.getY() + (Units.secondsToMilliseconds(yVel) * 20); // every periodic predict where the robot will be in one periodic into the future
+            D_x = target.getX() - drivepose.getX() + (Units.secondsToMilliseconds(xVel) * 20);
+        } else {
+            D_y = target.getY() - drivepose.getY();
+            D_x = target.getX() - drivepose.getX();
+        }
         fieldAngleRad = Math.atan2(D_y, D_x);
         double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Add 180 because drivetrain is backwards
         turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0,180.0);
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 7a06491..f35e45f 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -39,10 +39,13 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private Command turretAutoShoot;
     private TurretJoyStickAim turretJoyStickAim;
 
+    private boolean SOTM;
+
     public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) {
         super(drive);
         this.shooter = shooter;
         this.turret = turret;
+        SOTM = false;
     }
 
     public void configureControls() { 
@@ -51,6 +54,8 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
         )));
 
+        driver.get(PS5Button.MUTE).onTrue(new InstantCommand(() -> toggleSOTM()));
+
         // Cancel commands
         driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{
             getDrivetrain().setIsAlign(false);
@@ -83,7 +88,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                         if (turretAutoShoot != null && turretAutoShoot.isScheduled()){
                             turretAutoShoot.cancel();
                         } else{
-                            turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain());
+                            turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain(), SOTM);
                             CommandScheduler.getInstance().schedule(turretAutoShoot);
                         }
                     })
@@ -160,4 +165,12 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     public void endRumble(){
         driver.rumbleOff();
     }
+
+    public void toggleSOTM() {
+        if (SOTM) {
+            SOTM = false;
+        } else {
+            SOTM = true;
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 3d6e67a..95b386f 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -119,7 +119,6 @@ public class Turret extends SubsystemBase implements TurretIO{
     }
 
     public void setSetpoint(double setpointDegrees, double robotRotVel) {
-
         setpoint = MathUtil.clamp(setpointDegrees, TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE);
 
         if (infiniteRotation) {
-- 
2.39.5