Auto Align, Auto Hood Angle for 3000 RPM, Camera Poses Working

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

@@ -6,6 +6,7 @@ package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
+import java.util.Optional;
 import java.util.OptionalDouble;
 
 import org.littletonrobotics.junction.Logger;
@@ -13,8 +14,12 @@ import org.littletonrobotics.junction.Logger;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -59,6 +64,8 @@ public class RobotContainer {
 
     private Timer shiftTimer;
 
+    private boolean resetOdometryToVisualPose;
+
     public RobotContainer() {
         vision = new PhotonVision();
         drivetrain = new Drivetrain(null);
@@ -70,6 +77,7 @@ public class RobotContainer {
         //climber = new Climber();
         configureNamedCommands();
 
+        resetOdometryToVisualPose = false;
          
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
@@ -78,6 +86,12 @@ public class RobotContainer {
                 vp.visualPose()
             );
         });
+        vision.addPoseEstimateConsumer((vp) -> {
+            if(resetOdometryToVisualPose) {
+                drivetrain.resetOdometry(vp.visualPose());
+                resetOdometryToVisualPose = false;
+            }
+        });
         
 
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
@@ -93,7 +107,6 @@ public class RobotContainer {
         if(AutoConstants.kAutoConfigOk) {
             autoChooser = AutoBuilder.buildAutoChooser();
             SmartDashboard.putData("Auto Chooser", autoChooser);
-            
         }
     }
 
@@ -255,6 +268,7 @@ public class RobotContainer {
 
         intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
 
+        driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
         driver.y().whileTrue(drivetrain.zeroHeading());
 
         driver.leftTrigger().whileTrue(
@@ -285,36 +299,38 @@ public class RobotContainer {
         secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
         secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
 
-        secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
+        //hood.setDefaultCommand(hood.trackToAngle(() -> Units.degreesToRadians(MathUtil.clamp(hoodAngle, 0, 40))));
+        //secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
         //40 good for feeding
-        secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
+        //secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
         //30 degrees good for shooter far near outpost
         secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
         //10 degrees good for shooting ~33in away from hub
 
-        /* 
+        
         hood.setDefaultCommand(hood.trackToAngle(() -> {
             Pose2d drivetrainPose = drivetrain.getPose();
             Pose2d hubPose = Utilities.getHubPose();
 
             double distance = drivetrainPose.getTranslation()
-                .plus(CompetitionConstants.kRobotToShooter.getTranslation().toTranslation2d())
                 .getDistance(hubPose.getTranslation());
 
-            if(HoodConstants.kUseInterpolatorForAngle) {
+            Logger.recordOutput("Hood/DistanceToHub", distance);
-                return HoodConstants.kDistanceToAngle.get(distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
+                
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
                 } else {
-                // TODO The average actual speeds isn't <i>really</i> the exit velocity of the ball
+                    return 0;
-                // on a hooded shooter, based on documentation, it's more like 30-50% depending on
-                // hood material, surface friction, etc.
-                return Utilities.shotAngle(
-                    shooter.getAverageActualSpeeds(), 
-                    distance, 
-                    CompetitionConstants.kHubGoalHeightMeters - ShooterConstants.kShooterHeightMeters, 
-                    false
-                );
                 }
-        }));*/
+            } else {
+                return 0;
+            }
+        }));
     }
 
     private void configureNamedCommands() {

src/main/java/frc/robot/constants/AutoConstants.java

@@ -24,8 +24,6 @@ public class AutoConstants {
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 
-    public static final double kYawPIDTolerance = Units.degreesToRadians(2);
-
     public static final double kAlignPXYController = 2;
     public static final double kAlignPThetaController = 5;
 

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -38,9 +38,10 @@ public class DrivetrainConstants {
     public static final boolean kGyroReversed = true;
 
     // TODO Hold over from 2025, adjust?
-    public static final double kHeadingP = .1;
+    public static final double kHeadingP = .65;
     public static final double kXTranslationP = .5;
     public static final double kYTranslationP = .5;
+    public static final double kYawPIDTolerance = Units.degreesToRadians(1);
 
     // TODO How much do we trust gyro and encoders vs vision estimates.
     // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same

src/main/java/frc/robot/constants/HoodConstants.java

@@ -5,6 +5,7 @@ import java.io.File;
 import java.io.FileReader;
 import java.io.IOException;
 import java.nio.file.Path;
+import java.util.Map;
 
 import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
@@ -12,7 +13,9 @@ import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Filesystem;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
 
 public class HoodConstants {
     // TODO Real Values
@@ -43,9 +46,9 @@ public class HoodConstants {
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
 
-    // TODO This needs to be filled in from some source
+    public static final Map<ShooterSpeeds, InterpolatingDoubleTreeMap> kHoodInterpolators = Map.of(
-    public static final InterpolatingDoubleTreeMap kDistanceToAngle = new InterpolatingDoubleTreeMap();
+        ShooterSpeeds.kHubSpeed, new InterpolatingDoubleTreeMap()
-
+    );
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
     public static final SparkMaxConfig kConfig = new SparkMaxConfig();
@@ -66,27 +69,24 @@ public class HoodConstants {
             .feedForward
                 .sva(kS, kV, kA);
 
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
+            Double.valueOf(Units.degreesToRadians(10)));
 
-        File interpolatorFile = Path.of(
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-            Filesystem.getDeployDirectory().getAbsolutePath().toString(), 
+            Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
-            "interpolatorData.csv"
+            Double.valueOf(Units.degreesToRadians(13)));
-        ).toFile();
 
-        if(interpolatorFile.exists()) {
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-            try (BufferedReader reader = new BufferedReader(new FileReader(interpolatorFile))) {
+            Double.valueOf(Units.inchesToMeters(22.2 + 80)),
-                reader.lines().forEach((s) -> {
+            Double.valueOf(Units.degreesToRadians(17)));
-                    if(s.trim() != "") { //Empty or whitespace line protection
-                        String[] lineSplit = s.split(",");
 
-                        kDistanceToAngle.put(
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-                            Double.valueOf(lineSplit[0].replace("\"", "")), 
+            Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
-                            Double.valueOf(lineSplit[1].replace("\"", ""))
+            Double.valueOf(Units.degreesToRadians(21)));
-                        );
+
-                    }
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-                });
+            Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
-            } catch (IOException e) {
+            Double.valueOf(Units.degreesToRadians(24)));
-                // This condition is never reached because of the if exists line above
-            }
-        }
     }
 }

src/main/java/frc/robot/constants/PhotonConstants.java

@@ -18,7 +18,7 @@ public class PhotonConstants {
         Units.inchesToMeters(28.5),
         new Rotation3d(
             Units.degreesToRadians(0), 
-            Units.degreesToRadians(24.0), 
+            Units.degreesToRadians(-24.0), 
             Units.degreesToRadians(30.0)
         )
     );
@@ -28,7 +28,7 @@ public class PhotonConstants {
         Units.inchesToMeters(28.5),
         new Rotation3d(
             Units.degreesToRadians(0.0), 
-            Units.degreesToRadians(24.0), 
+            Units.degreesToRadians(-24.0), 
             Units.degreesToRadians(-10.0)
         )
     );

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

@@ -82,12 +82,12 @@ public class Drivetrain extends SubsystemBase {
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
         yawRotationController = new PIDController(
-            AutoConstants.kPThetaController, 
+            DrivetrainConstants.kHeadingP, 
             0, 
             0
         );
         yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
-        yawRotationController.setTolerance(AutoConstants.kYawPIDTolerance);
+        yawRotationController.setTolerance(DrivetrainConstants.kYawPIDTolerance);
 
         // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
         estimator = new SwerveDrivePoseEstimator(
@@ -262,10 +262,17 @@ public class Drivetrain extends SubsystemBase {
                         targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
                     }
 
-                    return yawRotationController.calculate(
+                    Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians());
+                    Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians());
+
+                    double outputPower = -yawRotationController.calculate(
                         getHeading().getRadians(),
                         targetRotation.getRadians()
                     );
+
+                    Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower);
+
+                    return outputPower;
                 }, 
                 () -> true
             )

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

@@ -13,6 +13,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;