turret: add pose-supplier, targeting helpers, and default command

src/main/java/frc/robot/RobotContainer.java

@@ -22,6 +22,8 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
+import frc.robot.commands.turret.DefaultCommand;
+import frc.robot.subsystems.Turret;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import java.io.File;
 import swervelib.SwerveInputStream;
@@ -40,6 +42,9 @@ public class RobotContainer {
   private final SwerveSubsystem drivebase =
       new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/7660-chassis0"));
 
+  // Turret subsystem, constructed with a supplier that returns the current odometry pose
+  private final Turret turret = new Turret(drivebase::getPose);
+
   // Establish a Sendable Chooser that will be able to be sent to the SmartDashboard, allowing
   // selection of desired auto
   private final SendableChooser<Command> autoChooser;
@@ -125,6 +130,9 @@ public RobotContainer() {
 
     // Put the autoChooser on the SmartDashboard
     SmartDashboard.putData("Auto Chooser", autoChooser);
+
+    // Set the turret default command to compute targets from odometry
+    turret.setDefaultCommand(new DefaultCommand(turret));
   }
 
   /**

src/main/java/frc/robot/commands/turret/DefaultCommand.java

@@ -0,0 +1,36 @@
+package frc.robot.commands.turret;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Turret;
+
+/** Default command for the turret: periodically compute a target and set a turret setpoint. */
+public class DefaultCommand extends Command {
+  private final Turret turret;
+
+  public DefaultCommand(Turret turret) {
+    this.turret = turret;
+    addRequirements(turret);
+  }
+
+  @Override
+  public void execute() {
+    Rotation2d robotRelative = turret.getRobotRelativeAngle();
+
+    // Send the setpoint to the turret subsystem (stubbed implementation).
+    turret.setTurretSetpoint(robotRelative);
+
+    // Publish the setpoint for debugging.
+    SmartDashboard.putNumber("Turret/SetpointRad", robotRelative.getRadians());
+    // Also publish the chosen target from the turret for visibility
+    var target = turret.getLastTarget();
+    SmartDashboard.putNumber("Turret/TargetX", target.getX());
+    SmartDashboard.putNumber("Turret/TargetY", target.getY());
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false; // Default command never finishes
+  }
+}

src/main/java/frc/robot/lib/TurretHelpers.java

@@ -0,0 +1,91 @@
+package frc.robot.lib;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public final class TurretHelpers {
+  private TurretHelpers() {}
+
+  // Field dimensions / thresholds (converted from inches to meters)
+  public static final double MAX_X = Units.inchesToMeters(651.22);
+  public static final double BLUE_THRESHOLD_X = Units.inchesToMeters(182.11);
+  public static final double RED_THRESHOLD_X = Units.inchesToMeters(469.11);
+  public static final double THRESHOLD_Y = Units.inchesToMeters(158.84);
+  // Frequently used goal Y positions (in inches) and their meter conversions
+  public static final double GOAL_Y_IN_LOW = 100.0;
+  public static final double GOAL_Y_IN_HIGH = 220.0;
+  public static final double GOAL_Y_LOW = Units.inchesToMeters(GOAL_Y_IN_LOW);
+  public static final double GOAL_Y_HIGH = Units.inchesToMeters(GOAL_Y_IN_HIGH);
+
+  // Representative centers for each goal area (meters). These are placeholders
+  // suitable for computing angles; replace with precise coordinates if available.
+  // Provide distinct hub centers for Red and Blue teams so callers can differentiate.
+  public static final Translation2d HUB_CENTER_RED =
+      new Translation2d(RED_THRESHOLD_X + (MAX_X - RED_THRESHOLD_X) / 2.0, THRESHOLD_Y);
+  public static final Translation2d HUB_CENTER_BLUE =
+      new Translation2d(BLUE_THRESHOLD_X / 2.0, THRESHOLD_Y);
+  public static final Translation2d GOAL_R1_CENTER = new Translation2d(MAX_X, GOAL_Y_LOW);
+  public static final Translation2d GOAL_R2_CENTER = new Translation2d(MAX_X, GOAL_Y_HIGH);
+  public static final Translation2d GOAL_B1_CENTER = new Translation2d(0.0, GOAL_Y_LOW);
+  public static final Translation2d GOAL_B2_CENTER = new Translation2d(0.0, GOAL_Y_HIGH);
+
+  /**
+   * Decide which goal area to aim at based purely on the robot position and alliance. Mirrors the
+   * decision tree provided by the user.
+   *
+   * @param x robot X position (meters)
+   * @param y robot Y position (meters)
+   * @param alliance robot alliance (Red or Blue)
+   * @return the chosen goal area as a Translation2d (representative center)
+   */
+  public static Translation2d getTarget(double x, double y, Alliance alliance) {
+    if (alliance == Alliance.Red) {
+      if (x > RED_THRESHOLD_X) {
+        return HUB_CENTER_RED;
+      } else {
+        if (y < THRESHOLD_Y) {
+          return GOAL_R1_CENTER;
+        } else {
+          return GOAL_R2_CENTER;
+        }
+      }
+    } else { // Blue
+      if (x < BLUE_THRESHOLD_X) {
+        return HUB_CENTER_BLUE;
+      } else {
+        if (y < THRESHOLD_Y) {
+          return GOAL_B1_CENTER;
+        } else {
+          return GOAL_B2_CENTER;
+        }
+      }
+    }
+  }
+
+  /** Backwards-compatible overload that accepts a Translation2d robot position. */
+  public static Translation2d getTarget(Translation2d turretPosition, Alliance alliance) {
+    return getTarget(turretPosition.getX(), turretPosition.getY(), alliance);
+  }
+
+  /** Compute the field-relative angle (radians) from turret position to target position. */
+  public static Rotation2d getFieldRelativeAngle(
+      Translation2d turretPosition, Translation2d targetPosition) {
+    double dx = targetPosition.getX() - turretPosition.getX();
+    double dy = targetPosition.getY() - turretPosition.getY();
+    return new Rotation2d(Math.atan2(dy, dx));
+  }
+
+  /**
+   * Convert a field-relative desired turret angle into a robot-relative turret angle. This returns
+   * the angle the turret must take relative to the robot's forward direction. This is a pure
+   * computation; it does not command the Neo.
+   */
+  public static Rotation2d getRobotRelative(Rotation2d fieldRelativeAngle, Rotation2d robotAngle) {
+    double turretRadians = fieldRelativeAngle.getRadians() - robotAngle.getRadians();
+    // normalize to [-pi, pi]
+    turretRadians = Math.atan2(Math.sin(turretRadians), Math.cos(turretRadians));
+    return new Rotation2d(turretRadians);
+  }
+}

src/main/java/frc/robot/subsystems/Turret.java

@@ -6,13 +6,38 @@
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.lib.TurretHelpers;
+import java.util.function.Supplier;
 
-public class Turret {
+/** Simple turret subsystem that holds a Neo and a supplier for the robot pose. */
+public class Turret extends SubsystemBase {
   private final SparkMax turretMotor =
       new SparkMax(Constants.Turret.MOTOR_ID, SparkLowLevel.MotorType.kBrushless);
+  private final Supplier<Pose2d> getPose;
 
+  // Most recent setpoint (robot-relative radians). Useful for debugging and tests.
+  private Rotation2d lastSetpoint = new Rotation2d();
+  // Most recently chosen target (field coordinates, meters)
+  private Translation2d lastTarget = new Translation2d();
+
+  /** Default constructor. Uses a trivial Pose2d supplier (origin) when no supplier is provided. */
   public Turret() {
+    this(() -> new Pose2d());
+  }
+
+  /**
+   * Construct a Turret with a pose supplier function.
+   *
+   * @param getPose a Supplier that returns the current {@link Pose2d} of the robot
+   */
+  public Turret(Supplier<Pose2d> getPose) {
+    this.getPose = getPose;
     SparkMaxConfig turretConfig = configureTurretMotor();
     turretMotor.configure(
         turretConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
@@ -24,4 +49,48 @@ public SparkMaxConfig configureTurretMotor() {
     turretConfig.idleMode(IdleMode.kBrake).inverted(false);
     return turretConfig;
   }
+
+  /**
+   * Set the turret desired angle relative to the robot forward direction. This is a stub that
+   * records the setpoint; convert to motor commands in a follow-up change.
+   *
+   * @param robotRelativeAngle turret angle relative to robot forward
+   */
+  public void setTurretSetpoint(Rotation2d robotRelativeAngle) {
+    this.lastSetpoint = robotRelativeAngle;
+    // TODO: convert Rotation2d to motor position/velocity and command the Neo here.
+  }
+
+  /** Get the most recent turret setpoint (robot-relative angle). Primarily useful for testing. */
+  public Rotation2d getLastSetpoint() {
+    return lastSetpoint;
+  }
+
+  /**
+   * Compute the desired turret angle relative to the robot using the stored pose supplier and
+   * TurretHelpers decision logic.
+   *
+   * @return desired turret angle relative to the robot forward direction
+   */
+  public Rotation2d getRobotRelativeAngle() {
+    Pose2d pose = getPose.get();
+    Translation2d robotPos = pose.getTranslation();
+    var alliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue);
+
+    Translation2d target = TurretHelpers.getTarget(robotPos, alliance);
+    // record the chosen target for external inspection / dashboard
+    this.lastTarget = target;
+    Rotation2d fieldAngle = TurretHelpers.getFieldRelativeAngle(robotPos, target);
+    Rotation2d robotRelative = TurretHelpers.getRobotRelative(fieldAngle, pose.getRotation());
+    return robotRelative;
+  }
+
+  /**
+   * Returns the last computed target translation (field coordinates).
+   *
+   * @return last chosen {@link Translation2d} target
+   */
+  public Translation2d getLastTarget() {
+    return lastTarget;
+  }
 }

src/test/java/frc/robot/lib/TurretTest.java

@@ -0,0 +1,125 @@
+package frc.robot.lib;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class TurretTest {
+
+  @ParameterizedTest
+  @MethodSource("targetProvider")
+  void getTarget_tableDriven(
+      double turretX, double turretY, Alliance alliance, double expX, double expY) {
+    Translation2d turretPos = new Translation2d(turretX, turretY);
+    Translation2d target = TurretHelpers.getTarget(turretPos, alliance);
+    assertNotNull(target);
+    assertEquals(expX, target.getX(), 1e-9);
+    assertEquals(expY, target.getY(), 1e-9);
+  }
+
+  static Stream<Arguments> targetProvider() {
+    return Stream.of(
+        // A: (78.305, 79.423)
+        // Red -> GOAL_R1
+        Arguments.of(
+            Units.inchesToMeters(78.305),
+            Units.inchesToMeters(79.423),
+            Alliance.Red,
+            TurretHelpers.MAX_X,
+            TurretHelpers.GOAL_Y_LOW),
+        // A: (78.305, 79.423) Blue -> HUB
+        Arguments.of(
+            Units.inchesToMeters(78.305),
+            Units.inchesToMeters(79.423),
+            Alliance.Blue,
+            TurretHelpers.HUB_CENTER_BLUE.getX(),
+            TurretHelpers.HUB_CENTER_BLUE.getY()),
+
+        // C: (78.305, 238.263)
+        // Red -> GOAL_R2
+        Arguments.of(
+            Units.inchesToMeters(78.305),
+            Units.inchesToMeters(238.263),
+            Alliance.Red,
+            TurretHelpers.MAX_X,
+            TurretHelpers.GOAL_Y_HIGH),
+        // C: (78.305, 238.263) Blue -> HUB
+        Arguments.of(
+            Units.inchesToMeters(78.305),
+            Units.inchesToMeters(238.263),
+            Alliance.Blue,
+            TurretHelpers.HUB_CENTER_BLUE.getX(),
+            TurretHelpers.HUB_CENTER_BLUE.getY()),
+
+        // D: (325.61, 79.423)
+        // Red -> GOAL_R1
+        Arguments.of(
+            Units.inchesToMeters(325.61),
+            Units.inchesToMeters(79.423),
+            Alliance.Red,
+            TurretHelpers.MAX_X,
+            TurretHelpers.GOAL_Y_LOW),
+        // D: (325.61, 79.423) Blue -> GOAL_B1
+        Arguments.of(
+            Units.inchesToMeters(325.61),
+            Units.inchesToMeters(79.423),
+            Alliance.Blue,
+            0.0,
+            TurretHelpers.GOAL_Y_LOW),
+
+        // E: (325.61, 238.263)
+        // Red -> GOAL_R2
+        Arguments.of(
+            Units.inchesToMeters(325.61),
+            Units.inchesToMeters(238.263),
+            Alliance.Red,
+            TurretHelpers.MAX_X,
+            TurretHelpers.GOAL_Y_HIGH),
+        // E: (325.61, 238.263) Blue -> GOAL_B2
+        Arguments.of(
+            Units.inchesToMeters(325.61),
+            Units.inchesToMeters(238.263),
+            Alliance.Blue,
+            0.0,
+            TurretHelpers.GOAL_Y_HIGH),
+
+        // F: (560.165, 79.423)
+        // Red -> HUB
+        Arguments.of(
+            Units.inchesToMeters(560.165),
+            Units.inchesToMeters(79.423),
+            Alliance.Red,
+            TurretHelpers.HUB_CENTER_RED.getX(),
+            TurretHelpers.HUB_CENTER_RED.getY()),
+        // F: (560.165, 79.423) Blue -> GOAL_B1
+        Arguments.of(
+            Units.inchesToMeters(560.165),
+            Units.inchesToMeters(79.423),
+            Alliance.Blue,
+            0.0,
+            TurretHelpers.GOAL_Y_LOW),
+
+        // H: (560.165, 238.263)
+        // Red -> HUB
+        Arguments.of(
+            Units.inchesToMeters(560.165),
+            Units.inchesToMeters(238.263),
+            Alliance.Red,
+            TurretHelpers.HUB_CENTER_RED.getX(),
+            TurretHelpers.HUB_CENTER_RED.getY()),
+        // H: (560.165, 238.263) Blue -> GOAL_B2
+        Arguments.of(
+            Units.inchesToMeters(560.165),
+            Units.inchesToMeters(238.263),
+            Alliance.Blue,
+            0.0,
+            TurretHelpers.GOAL_Y_HIGH));
+  }
+}