Merge Turret to Main quickly

src/main/java/frc/robot/Constants.java

@@ -1,33 +1,19 @@
 package frc.robot;
 
-import static edu.wpi.first.units.Units.Amps;
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.DegreesPerSecond;
-import static edu.wpi.first.units.Units.Inches;
-import static edu.wpi.first.units.Units.RPM;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
-import static edu.wpi.first.units.Units.Meters;
-import static edu.wpi.first.units.Units.Milliseconds;
-import static edu.wpi.first.units.Units.Percent;
-import static edu.wpi.first.units.Units.Rotations;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
-import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
-import static edu.wpi.first.units.Units.Value;
-import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.*;
 
 import java.util.List;
 import java.util.Map;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
-
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
-
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.units.AngleUnit;
 import edu.wpi.first.units.AngularAccelerationUnit;
 import edu.wpi.first.units.AngularVelocityUnit;
@@ -42,7 +28,6 @@
 import edu.wpi.first.units.measure.Per;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.units.measure.Voltage;
-import edu.wpi.first.math.system.plant.DCMotor;
 import frc.robot.generated.TunerConstants;
 
 public final class Constants
@@ -157,6 +142,33 @@ public static class ShooterConstants
             Map.entry(7.0, 5000.0)
         );
         // @formatter:on
+        // Pot calibration model:
+        // - MIN/MAX volts are measured at conceptual turret min/max angle.
+        // - ZERO_OFFSET shifts the conceptual 0-deg heading without changing hard
+        // limits.
+        public static final double TURRET_POTENTIOMETER_MIN_VOLTS       = 0.35;  // TODO: Calibrate pot voltage at min turret angle
+        public static final double TURRET_POTENTIOMETER_MAX_VOLTS       = 4.65;  // TODO: Calibrate pot voltage at max turret angle
+        public static final double TURRET_POTENTIOMETER_ZERO_OFFSET_DEG = 0.0;   // TODO: Calibrate conceptual turret zero offset (degrees)
+        // Dead-zone model (robot-relative):
+        // - Center/width define forbidden cable-wrap sector.
+        // - Width <= 0 disables dead-zone routing logic.
+        public static final double              TURRET_DEAD_ZONE_CENTER          = 180.0; // degrees (robot-relative)
+        public static final double              TURRET_DEAD_ZONE_WIDTH           = 0.0;   // degrees; set > 0 once cable-wrap dead-zone is measured
+        public static final List<List<Integer>> BLUE_HUB_CENTER_OFFSET_TAG_PAIRS = List.of(List.of(18, 27), List.of(20, 19), List.of(21, 24), List.of(26, 25));
+        public static final List<List<Integer>> RED_HUB_CENTER_OFFSET_TAG_PAIRS  = List.of(List.of(2, 11), List.of(4, 3), List.of(5, 8), List.of(10, 9));
+        public static final double              TURRET_CL_METERS                 = 0.3556; // 14.00 in
+        public static final double              TURRET_CH_METERS                 = 0.5969; // 23.50 ins
+        // @formatter:off
+        private static final InterpolatingDoubleTreeMap HOOD_ANGLE_TABLE                 = InterpolatingDoubleTreeMap.ofEntries
+        (
+            Map.entry(0.0, 20.0),
+            Map.entry(2.0, 15.0),
+            Map.entry(3.5, 22.0),
+            Map.entry(5.0, 30.0),
+            Map.entry(6.5, 38.0),
+            Map.entry(7.0, 45.0)
+        );
+        // @formatter:on
 
         // Feeder
         public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60);
@@ -167,6 +179,13 @@ public static AngularVelocity getFlywheelSpeedForDistance(Distance distance)
         {
             return RPM.of(FLYWHEEL_SPEED_TABLE.get(distance.in(Meters)));
         }
+
+        // TODO: Tune these values with testing!
+
+        public static double getHoodAngleForDistance(double meters)
+        {
+            return HOOD_ANGLE_TABLE.get(meters);
+        }
     }
 
     public static class VisionConstants

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

@@ -8,7 +8,6 @@
 import static edu.wpi.first.units.Units.Value;
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
-
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.epilogue.Logged;
@@ -24,8 +23,9 @@
 import frc.robot.subsystems.Drive;
 import frc.robot.subsystems.TestOperation;
 import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.Hood.HoodPosition;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.Turret;
 import frc.robot.util.MeasureUtil;
 
 @Logged
@@ -43,8 +43,10 @@ public class RobotContainer
     private final Intake                         _intake     = new Intake();
     private final Shooter                        _shooter    = new Shooter(_drivetrain::getState);
     private final TestOperation                  _testop     = new TestOperation();
+    private final CommandXboxController          _joystick   = new CommandXboxController(0); // TODO: Check if this can be removed
     // private final Turret _turret = new Turret(_drivetrain::getState);
-    private final Autos _autos = new Autos(_drivetrain);
+    private final Autos  _autos  = new Autos(_drivetrain);
+    private final Turret _turret = new Turret(_drivetrain::getState);
 
     public RobotContainer()
     {
@@ -94,6 +96,12 @@ private void configureBindings()
         _operator.rightTrigger().onTrue(_shooter.fireCmd());
         // Reset the field-centric heading on left bumper press.
         _driver.povDown().onTrue(_drivetrain.runOnce(_drivetrain::seedFieldCentric));
+        _joystick.leftBumper().onTrue(_drivetrain.runOnce(_drivetrain::seedFieldCentric));
+
+        // Turret simulation bindings.
+        _joystick.leftTrigger().whileTrue(_turret.getTrackCmd());
+        _joystick.y().whileTrue(_turret.getPassCmd());
+        _joystick.leftStick().onTrue(_turret.getIdleCmd());
 
         _drivetrain.registerTelemetry(_logger::telemeterize);