From b7f743208ff877814739b60f1f78ea24a6bf2c2d Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Wed, 28 Jan 2026 17:07:26 -0800
Subject: [PATCH] aaa

---
 .../frc/robot/commands/gpm/DoSysidThings.java     |  4 ++--
 .../frc/robot/commands/gpm/TurretAutoShoot.java   |  7 ++++---
 .../java/frc/robot/subsystems/turret/Turret.java  | 15 +++++++++++----
 3 files changed, 17 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/commands/gpm/DoSysidThings.java b/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
index 7624265..e860a2a 100644
--- a/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
+++ b/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
@@ -22,8 +22,8 @@ public class DoSysidThings extends SequentialCommandGroup {
             routine.quasistatic(SysIdRoutine.Direction.kForward),
             routine.quasistatic(SysIdRoutine.Direction.kReverse),
             routine.dynamic(SysIdRoutine.Direction.kForward),
-            routine.dynamic(SysIdRoutine.Direction.kReverse),
-        )
+            routine.dynamic(SysIdRoutine.Direction.kReverse)
+        );
     }
 
     private void doLog(SysIdRoutineLog log) {
diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index ff78d09..2949fd1 100644
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -30,11 +30,12 @@ public class TurretAutoShoot extends Command {
 
     private boolean turretVisionEnabled = false;
     private boolean SOTM = true;
-    private Translation2d drivepose = drivetrain.getPose().getTranslation();
+    private Translation2d drivepose;
     public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision) {
         this.turret = turret;
         this.drivetrain = drivetrain;
         this.turretVision = turretVision;
+        drivepose  = drivetrain.getPose().getTranslation();
         
         addRequirements(turret);
     }
@@ -89,7 +90,7 @@ public class TurretAutoShoot extends Command {
         // Calculate turret setpoint (angle relative to robot heading)
         turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
 
-        System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
+        // System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
     }
 
     public void adjustWithTurretCam() {
@@ -131,7 +132,7 @@ public class TurretAutoShoot extends Command {
             adjustWithTurretCam();
             turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(adjustedSetpoint)), -drivetrain.getAngularRate(2));
         } else{
-            turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
+            turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) * 1.0);
         }
     }
 
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 94f331d..a668993 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -37,8 +37,8 @@ public class Turret extends SubsystemBase implements TurretIO{
     private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE);
     private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE);
 
-    private static double MAX_VEL_RAD_PER_SEC = 4*Units.degreesToRadians(360);
-    private static double MAX_ACCEL_RAD_PER_SEC2 = 4*Units.degreesToRadians(720);
+    private static double MAX_VEL_RAD_PER_SEC = 100;
+    private static double MAX_ACCEL_RAD_PER_SEC2 = 500000000;
 
     private static final double VERSA_RATIO = 5.0;
     private static final double TURRET_RATIO = 140.0 / 10.0;
@@ -76,14 +76,17 @@ public class Turret extends SubsystemBase implements TurretIO{
 
     /* ---------------- Tuning ---------------- */
 
-    private double kP = 15.0;
+    private double kP = 12.0;
     private double kD = 0.0;
 
     /* ---------------- Constructor ---------------- */
 
     public Turret() {
+		SmartDashboard.putNumber("FF Value", 0.1);
+
         motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN);
         motor.setNeutralMode(NeutralModeValue.Brake);
+		motor.setPosition(0);
 
         TalonFXConfiguration cfg = new TalonFXConfiguration();
         cfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
@@ -165,6 +168,7 @@ public class Turret extends SubsystemBase implements TurretIO{
 
     /* ---------------- Periodic ---------------- */
 
+	double FFValue;
     @Override
     public void periodic() {
         updateInputs();
@@ -202,10 +206,13 @@ public class Turret extends SubsystemBase implements TurretIO{
         double motorRot =
             Units.radiansToRotations(setpoint.position) * GEAR_RATIO;
 
-        motor.setControl(mmRequest.withPosition(motorRot));
+		FFValue = SmartDashboard.getNumber("FF Value", FFValue);
+        motor.setControl(mmRequest.withPosition(motorRot).withFeedForward(FFValue));
 
         ligament.setAngle(getPositionDeg());
 
+		FFValue = SmartDashboard.getNumber("FF Value", FFValue);
+
         SmartDashboard.putNumber("Turret Pos Deg", getPositionDeg());
         SmartDashboard.putNumber("Turret Goal Deg", Units.radiansToDegrees(best));
     }
-- 
2.39.5