Turret logic

src/main/java/org/team2342/frc/Constants.java

@@ -6,6 +6,7 @@
 
 package org.team2342.frc;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -243,6 +244,40 @@ public static final class ShooterConstants {
             HOOD_SIM_MOTOR);
   }
 
+  public static final class TurretConstants {
+    public static final Transform3d TURRET_TRANSFORM =
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(-4.960),
+                Units.inchesToMeters(5.997),
+                Units.inchesToMeters(14.823)),
+            new Rotation3d(Rotation2d.k180deg));
+
+    public static final double AT_POSITION_THRESHOLD = 0.01;
+
+    public static final double STARTING_ANGLE = Units.degreesToRadians(0);
+    public static final double MIN_TURRET_ANGLE = Units.degreesToRadians(-90);
+    public static final double MAX_TURRET_ANGLE = Units.degreesToRadians(90);
+
+    public static final double GEAR_RATIO = (46.0 / 12.0) * (100.0 / 10.0);
+
+    public static final SmartMotorConfig TURRET_CONFIG =
+        new SmartMotorConfig()
+            .withControlType(ControlType.PROFILED_POSITION)
+            .withGearRatio(GEAR_RATIO)
+            .withIdleMode(IdleMode.BRAKE)
+            .withProfileConstraintsRad(new TrapezoidProfile.Constraints(Math.PI, Math.PI))
+            .withSupplyCurrentLimit(40);
+
+    public static final PIDFFConfigs PID_CONFIG = new PIDFFConfigs().withKP(100).withKI(10);
+
+    public static final DCMotor TURRET_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotorSim TURRET_SIM =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(TURRET_SIM_MOTOR, 0.003, 100.0 / 16.0),
+            TURRET_SIM_MOTOR);
+  }
+
   public static final class CANConstants {
     public static final int PIGEON_ID = 13;
     public static final int[] FL_IDS = {1, 5, 9};
@@ -262,6 +297,8 @@ public static final class CANConstants {
     public static final int INDEXER_BELT_ID = 21;
     public static final int INDEXER_FEEDER_ID = 22;
 
+    public static final int TURRET_ID = 23;
+
     public static final int PDH_ID = 62;
   }
 }

src/main/java/org/team2342/frc/RobotContainer.java

@@ -29,6 +29,7 @@
 import org.team2342.frc.Constants.IndexerConstants;
 import org.team2342.frc.Constants.IntakeConstants;
 import org.team2342.frc.Constants.ShooterConstants;
+import org.team2342.frc.Constants.TurretConstants;
 import org.team2342.frc.Constants.VisionConstants;
 import org.team2342.frc.commands.DriveCommands;
 import org.team2342.frc.commands.RotationLockedDrive;
@@ -44,6 +45,7 @@
 import org.team2342.frc.subsystems.intake.Wheels;
 import org.team2342.frc.subsystems.shooter.Flywheel;
 import org.team2342.frc.subsystems.shooter.Hood;
+import org.team2342.frc.subsystems.shooter.Turret;
 import org.team2342.frc.subsystems.vision.Vision;
 import org.team2342.frc.subsystems.vision.VisionIO;
 import org.team2342.frc.subsystems.vision.VisionIOPhoton;
@@ -67,6 +69,7 @@ public class RobotContainer {
   @Getter private final Wheels wheels;
   @Getter private final Flywheel flywheel;
   @Getter private final Hood hood;
+  @Getter private final Turret turret;
 
   @Getter private final Conductor conductor;
 
@@ -105,35 +108,28 @@ public RobotContainer() {
                 new VisionIOPhoton(
                     VisionConstants.SWERVE_CAMERA_PARAMETERS,
                     PoseStrategy.CONSTRAINED_SOLVEPNP,
+                    PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
+        turret =
+            new Turret(
+                new SmartMotorIOTalonFX(
+                    CANConstants.TURRET_ID,
+                    TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG)));
+                    PoseStrategy.CONSTRAINED_SOLVEPNP,
                     PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR),
                 new VisionIOPhoton(
                     VisionConstants.SHOOTER_CAMERA_PARAMETERS,
                     PoseStrategy.CONSTRAINED_SOLVEPNP,
                     PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
-        indexer =
-            new Indexer(
-                new DumbMotorIOTalonFXFOC(
-                    CANConstants.INDEXER_BELT_ID, IndexerConstants.INDEXER_BELT_CONFIG),
-                new DumbMotorIOTalonFXFOC(
-                    CANConstants.INDEXER_FEEDER_ID, IndexerConstants.INDEXER_FEEDER_CONFIG));
-
-        wheels =
-            new Wheels(
-                new DumbMotorIOTalonFXFOC(
-                    CANConstants.INTAKE_WHEEL_MOTOR_ID,
-                    IntakeConstants.INTAKE_WHEELS_MOTOR_CONFIG));
         flywheel =
             new Flywheel(
                 new SmartMotorIOTalonFX(
                     CANConstants.FLYWHEEL_MOTOR_ID,
                     ShooterConstants.FLYWHEEL_CONFIG.withPIDFFConfigs(
                         ShooterConstants.FLYWHEEL_PID_CONFIGS)));
-        hood =
-            new Hood(
-                new SmartMotorIOTalonFX(
-                    CANConstants.HOOD_MOTOR_ID,
-                    ShooterConstants.HOOD_MOTOR_CONFIG.withPIDFFConfigs(
-                        ShooterConstants.HOOD_MOTOR_PID_CONFIGS)));
+
+        indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {});
+        wheels = new Wheels(new DumbMotorIO() {});
+        hood = new Hood(new SmartMotorIO() {});
 
         LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev);
         break;
@@ -187,6 +183,7 @@ public RobotContainer() {
                     ShooterConstants.HOOD_SIM_MOTOR,
                     ShooterConstants.HOOD_SIM,
                     1));
+        turret = new Turret(new SmartMotorIO() {});
 
         break;
 
@@ -208,6 +205,7 @@ public RobotContainer() {
         wheels = new Wheels(new DumbMotorIO() {});
         flywheel = new Flywheel(new SmartMotorIO() {});
         hood = new Hood(new SmartMotorIO() {});
+        turret = new Turret(new SmartMotorIO() {});
 
         break;
     }

src/main/java/org/team2342/frc/subsystems/shooter/Turret.java

@@ -0,0 +1,97 @@
+// Copyright (c) 2026 Team 2342
+// https://github.com/FRCTeamPhoenix
+//
+// This source code is licensed under the MIT License.
+// See the LICENSE file in the root directory of this project.
+
+package org.team2342.frc.subsystems.shooter;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+import org.team2342.frc.Constants.TurretConstants;
+import org.team2342.lib.logging.ExecutionLogger;
+import org.team2342.lib.motors.smart.SmartMotorIO;
+import org.team2342.lib.motors.smart.SmartMotorIOInputsAutoLogged;
+
+public class Turret extends SubsystemBase {
+
+  private final SmartMotorIO turretMotor;
+  private final SmartMotorIOInputsAutoLogged inputs = new SmartMotorIOInputsAutoLogged();
+  private final Alert motorAlert = new Alert("Turret motor is disconnected!", AlertType.kError);
+  private double goal = TurretConstants.STARTING_ANGLE;
+
+  public Turret(SmartMotorIO turretMotor) {
+    this.turretMotor = turretMotor;
+    setName("Shooter/Turret");
+    setDefaultCommand(run(() -> turretMotor.runVoltage(0.0)));
+  }
+
+  @Override
+  public void periodic() {
+    turretMotor.updateInputs(inputs);
+    Logger.processInputs("Shooter/Turret", inputs);
+    motorAlert.set(!inputs.motorsConnected[0]);
+
+    ExecutionLogger.log("Shooter/Turret");
+  }
+
+  public void runPosition(Rotation2d target) {
+    this.goal = calculateTurretAngle(target);
+    turretMotor.runPosition(goal);
+  }
+
+  public Command runPositionCommand(Rotation2d target) {
+    return run(() -> runPosition(target)).withName("Turret RunPosition");
+  }
+
+  public void goToPosition(Rotation2d target) {
+    this.goal = calculateTurretAngle(target);
+    turretMotor.runPosition(goal);
+  }
+
+  public Command goToPositionCommand(Rotation2d target) {
+    return run(() -> goToPosition(target))
+        .until(() -> Math.abs(inputs.positionRad - goal) <= TurretConstants.AT_POSITION_THRESHOLD)
+        .withName("Turret GoToPosition");
+  }
+
+  public Command runVoltage(double voltage) {
+    return run(() -> turretMotor.runVoltage(voltage)).withName("Turret Voltage");
+  }
+
+  public Command stop() {
+    return runOnce(() -> turretMotor.runVoltage(0.0));
+  }
+
+  @AutoLogOutput(key = "Shooter/Turret/Position")
+  public Rotation2d getTurretPosition() {
+    return Rotation2d.fromRadians(inputs.positionRad);
+  }
+
+  public double getTurretVelocity() {
+    return inputs.velocityRadPerSec;
+  }
+
+  @AutoLogOutput(key = "Shooter/Turret/Setpoint")
+  public Rotation2d getTurretSetpoint() {
+    return new Rotation2d(goal);
+  }
+
+  public void zeroTurret() {
+    turretMotor.setPosition(0.0);
+  }
+
+  private double calculateTurretAngle(Rotation2d angle) {
+    double calculatedAngle = MathUtil.inputModulus(angle.getRadians(), -Math.PI, Math.PI);
+    // if (calculatedAngle < 0) calculatedAngle += 2 * Math.PI;
+    // if (calculatedAngle > Math.PI * 2) calculatedAngle -= 2 * Math.PI;
+    return MathUtil.clamp(
+        calculatedAngle, TurretConstants.MIN_TURRET_ANGLE, TurretConstants.MAX_TURRET_ANGLE);
+  }
+}

src/main/java/org/team2342/frc/util/FiringSolver.java

@@ -9,6 +9,11 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import org.littletonrobotics.junction.Logger;
+import org.team2342.frc.Constants.TurretConstants;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -28,14 +33,6 @@ public class FiringSolver {
 
   private FiringSolution lastSolution = null;
 
-  public static Transform3d TURRET_TRANSFORM =
-      new Transform3d(
-          new Translation3d(
-              Units.inchesToMeters(-4.960),
-              Units.inchesToMeters(5.997),
-              Units.inchesToMeters(14.823)),
-          new Rotation3d(Rotation2d.k180deg));
-
   private static final InterpolatingDoubleTreeMap angleMap = new InterpolatingDoubleTreeMap();
   private static final InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap();
   private static final InterpolatingDoubleTreeMap tofMap = new InterpolatingDoubleTreeMap();
@@ -77,19 +74,20 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) {
 
     Translation2d hub =
         AllianceUtils.flipToAlliance(FieldConstants.Hub.topCenterPoint).toTranslation2d();
-    Pose2d turretPose = new Pose3d(position).transformBy(TURRET_TRANSFORM).toPose2d();
+    Pose2d turretPose =
+        new Pose3d(position).transformBy(TurretConstants.TURRET_TRANSFORM).toPose2d();
     double robotAngle = position.getRotation().getRadians();
 
     double velX =
         velocity.vxMetersPerSecond
             + velocity.omegaRadiansPerSecond
-                * (TURRET_TRANSFORM.getY() * Math.cos(robotAngle)
-                    - TURRET_TRANSFORM.getX() * Math.sin(robotAngle));
+                * (TurretConstants.TURRET_TRANSFORM.getY() * Math.cos(robotAngle)
+                    - TurretConstants.TURRET_TRANSFORM.getX() * Math.sin(robotAngle));
     double velY =
         velocity.vyMetersPerSecond
             + velocity.omegaRadiansPerSecond
-                * (TURRET_TRANSFORM.getX() * Math.cos(robotAngle)
-                    - TURRET_TRANSFORM.getY() * Math.sin(robotAngle));
+                * (TurretConstants.TURRET_TRANSFORM.getX() * Math.cos(robotAngle)
+                    - TurretConstants.TURRET_TRANSFORM.getY() * Math.sin(robotAngle));
 
     double tof;
     Pose2d predictedPose = turretPose;
@@ -106,8 +104,9 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) {
     Logger.recordOutput("FiringSolver/PredictedPose", predictedPose);
     Logger.recordOutput("FiringSolver/PredictedDistance", predictedDistance);
 
-    Rotation2d turretAngle =
-        hub.minus(predictedPose.getTranslation()).getAngle().plus(Rotation2d.k180deg);
+    Rotation2d turretAngle = hub.minus(predictedPose.getTranslation()).getAngle();
+    turretAngle = turretAngle.minus(position.getRotation()).minus(Rotation2d.k180deg);
+
     double hoodAngle = angleMap.get(predictedDistance);
     double wheelSpeed = speedMap.get(predictedDistance);