Turret

src/main/java/com/stuypulse/robot/constants/Cameras.java

@@ -0,0 +1,45 @@
+
+package com.stuypulse.robot.constants;
+
+import com.stuypulse.stuylib.network.SmartBoolean;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+
+/** This interface stores information about each camera. */
+public interface Cameras {
+
+    public Camera[] LimelightCameras = new Camera[] {
+        //TODO: Set up camera pose
+        new Camera("limelight-turret", new Pose3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0))), Settings.EnabledSubsystems.LIMELIGHT_TURRET)
+    };
+
+    public static class Camera {
+        private String name;
+        private Pose3d location;
+        private SmartBoolean isEnabled;
+
+        public Camera(String name, Pose3d location, SmartBoolean isEnabled) {
+            this.name = name;
+            this.location = location;
+            this.isEnabled = isEnabled;
+        }
+
+        public String getName() {
+            return name;
+        }
+
+        public Pose3d getLocation() {
+            return location;
+        }
+
+        public boolean isEnabled() {
+            return isEnabled.get();
+        }
+
+        public void setEnabled(boolean enabled) {
+            this.isEnabled.set(enabled);
+        }
+    }
+}

src/main/java/com/stuypulse/robot/constants/Constants.java

@@ -0,0 +1,9 @@
+package com.stuypulse.robot.constants;
+
+public interface Constants {
+    public interface Turret {
+        final public int encoderTeeth18 = 18;
+        final public int encoderTeeth17 = 17;
+        final public int bigGearTeeth = 84;
+    }
+}

src/main/java/com/stuypulse/robot/constants/Field.java

@@ -1,4 +1,3 @@
-/************************ PROJECT KITBOT *************************/
 /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
 /* Use of this source code is governed by an MIT-style license */
 /* that can be found in the repository LICENSE file.           */
@@ -145,13 +144,12 @@ public static AprilTag getTag(int id) {
     public final int[] BLUE_TOWER_TAG_IDS = {31, 32};
     public final int[] RED_HP_TAG_IDS = {13, 14};
     public final int[] BLUE_HP_TAG_IDS = {29, 30};
-
+  
     public final Pose2d blueHubCenter = new Pose2d(Units.inchesToMeters(158.60), Units.inchesToMeters(WIDTH / 2.0), new Rotation2d());
 
     public static Pose2d getAllianceHubPose() {
         return (Robot.isBlue() ? blueHubCenter : transformToOppositeAlliance(blueHubCenter));
     }
-
     /* TRANSFORM FUNCTIONS */
 
     public static Pose3d transformToOppositeAlliance(Pose3d pose) {

src/main/java/com/stuypulse/robot/constants/Gains.java

@@ -3,7 +3,16 @@
 import com.pathplanner.lib.config.PIDConstants;
 
 public class Gains {
-
+    public interface Turret {
+        double kS = 0.0;
+        double kV = 0.2;
+        double kA = 0.01;
+
+        double kP = 1.0;
+        double kI = 0.0;
+        double kD = 0.05;
+    }
+
     public interface Shooter {
         public interface PID {
             double kP = 2.0;

src/main/java/com/stuypulse/robot/constants/Motors.java

@@ -5,6 +5,7 @@
 
 package com.stuypulse.robot.constants;
 
+import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
 import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
@@ -34,6 +35,18 @@
  *  - The Open Loop Ramp Rate
  */
 public interface Motors {
+    public interface Turret {
+        TalonFXConfig MOTOR_CONFIG = new TalonFXConfig()
+        .withCurrentLimitAmps(40)
+        .withRampRate(0.25)
+        .withNeutralMode(NeutralModeValue.Brake)
+        .withFFConstants(Gains.Turret.kS, Gains.Turret.kV, Gains.Turret.kA, 0)
+        .withPIDConstants(Gains.Turret.kP, Gains.Turret.kI, Gains.Turret.kD, 0)
+        .withMotionProfile(Settings.Turret.MAX_VEL.getRotations(), Settings.Turret.MAX_ACCEL.getRotations())
+        .withSensorToMechanismRatio(2.80) // seems to be off by a bit
+        .withInvertedValue(InvertedValue.Clockwise_Positive)
+        .withContinuousWrap(true);
+    }
 
     public interface Superstructure {
         TalonFXConfig intakeShooterMotorConfig = new TalonFXConfig()
@@ -59,6 +72,7 @@ public static class TalonFXConfig {
         private final OpenLoopRampsConfigs openLoopRampsConfigs = new OpenLoopRampsConfigs();
         private final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs();
         private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs();
+        private final ClosedLoopGeneralConfigs closedLoopGeneralConfigs = new ClosedLoopGeneralConfigs();
         private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs();
 
         public void configure(TalonFX motor) {
@@ -220,5 +234,13 @@ public TalonFXConfig withSensorToMechanismRatio(double sensorToMechanismRatio) {
 
             return this;
         }
+
+        public TalonFXConfig withContinuousWrap(boolean enabled) {
+            closedLoopGeneralConfigs.ContinuousWrap = enabled;
+
+            configuration.withClosedLoopGeneral(closedLoopGeneralConfigs);
+
+            return this;
+        }
     }
 }

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -12,7 +12,11 @@ public interface Gamepad {
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
-
+    //TODO: GET PORTS
+    public interface Turret {
+        int TURRET_MOTOR = 0;
+        int ENCODER_18t = 0;   
+        int ENCODER_17t = 0; 
     public interface Swerve {
         public interface Drive {
             int FRONT_LEFT = 32;

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -8,10 +8,12 @@
 import com.ctre.phoenix6.CANBus;
 import com.pathplanner.lib.path.PathConstraints;
 import com.stuypulse.stuylib.network.SmartBoolean;
-import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 
 public interface Settings {
@@ -22,6 +24,8 @@ public interface Settings {
 
     public interface EnabledSubsystems {
         SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true);
+        SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true);
+        SmartBoolean LIMELIGHT_TURRET = new SmartBoolean("Enabled Subsystems/Turret Limelight Is Enabled", true);
         SmartBoolean SUPERSTRUCTURE = new SmartBoolean("Enabled Subsystems/Superstructure Is Enabled", true);
     }
     public interface Superstructure {
@@ -39,6 +43,12 @@ public interface Indexer {
         }
     }
 
+    public interface Turret {
+        Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0));
+        Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0));
+        double TOLERANCE_DEG = 2.0;
+    }
+
     public interface Swerve {
         double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1;
         double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1;
@@ -84,6 +94,10 @@ public interface Targets {
             }
         }
     }
+
+    public interface Vision {
+        Vector<N3> MIN_STDDEVS = VecBuilder.fill(0.3, 0.3, 5);
+    }
 
     public interface Driver {
         public interface Drive {

src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java

@@ -0,0 +1,77 @@
+package com.stuypulse.robot.subsystems.turret;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.util.TurretVisualizer;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+public abstract class Turret extends SubsystemBase {
+    public static Turret instance;
+    public TurretState state;
+
+    static {
+        if (Robot.isReal()) {
+            instance = new TurretImpl();
+        } else {
+            instance = new TurretSim();
+        }
+    }
+
+    public static Turret getInstance() {
+        return instance;
+    }
+
+    public Turret() {
+        state = TurretState.ZERO;
+    }
+
+    public enum TurretState {
+        ZERO,
+        FERRYING,
+        POINT_AT_HUB;
+    }
+
+    public Rotation2d getTargetAngle() {
+        return switch (getTurretState()) {
+            case ZERO -> new Rotation2d(); 
+            case FERRYING -> getFerryAngle();
+            case POINT_AT_HUB -> getPointAtHubAngle();
+        };
+    }
+
+    public void setTurretState(TurretState targetState) {
+        state = targetState;
+    }
+
+    public TurretState getTurretState() {
+        return state;
+    }
+
+    public abstract Rotation2d getTurretAngle();
+
+    public abstract boolean atTargetAngle();
+
+    public abstract Rotation2d getPointAtHubAngle();
+
+    public abstract Rotation2d getFerryAngle();
+
+    public abstract SysIdRoutine getSysIdRoutine();
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("Turret/Angle (Deg)", getTurretAngle().getDegrees());
+
+        if (Settings.DEBUG_MODE) {
+            if (Settings.EnabledSubsystems.TURRET.get()) {
+                TurretVisualizer.getInstance().updateTurretAngle(getTurretAngle(), atTargetAngle());
+            }
+            else {
+                TurretVisualizer.getInstance().updateTurretAngle(new Rotation2d(), false);
+            }
+        }
+    }
+}