Auto Chassis align works

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

@@ -26,14 +26,15 @@ public final class Constants {
     public static class IDFilters {
       public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 };
       public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 };
+      // public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 18, 19, 20, 21 };
     }
 
-    public static final String BACK_CAMERA_NAME = "back";
+    public static final String BACK_CAMERA_NAME = "limelight-back";
     public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
     public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
     public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
 
-    public static final String FRONT_CAMERA_NAME = "front";
+    public static final String FRONT_CAMERA_NAME = "limelight-front";
     public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
   
   }
@@ -101,7 +102,7 @@ public final class Constants {
     public static final double TURRET_GEAR_RATIO  = 1;
 
     public static final int HOOD_SERVO_CHANNEL = 0;
-    public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this
+    public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
     public static final int HOOD_ENCODER_ID = 4;
     public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
     public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);

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

@@ -58,17 +58,17 @@ public class RobotContainer {
    */
 
   private void configureBindings() {
-    turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID()));
+    // turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID()));
-    hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, m_driverController.getHID()));
+    // hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, m_driverController.getHID()));
-    intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
+    // intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
 
     m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
     m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
     m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
     m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
 
-    m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
+    // m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
-    m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
+    // m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
 
     m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
 

src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java

@@ -4,9 +4,12 @@ import java.util.Arrays;
 import java.util.List;
 
 import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.littletonrobotics.junction.Logger;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.controller.PIDController;
+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.wpilibj.DriverStation;
@@ -41,15 +44,21 @@ public class AutoShootCommandChassisTurret extends Command{
 
         this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
 
-        autoCenterController = new PIDController(0.1, 0, 0);
+        autoCenterController = new PIDController(0.1, 0, 0.0);
-        autoCenterController.setTolerance(1); // Units is pixels
+        autoCenterController.setTolerance(0.2); // Units is pixels
         autoCenterController.setSetpoint(0);
- 
-        tagFilters = DriverStation.getAlliance().get() == Alliance.Blue ? Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS : Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
     }
 
     @Override
     public void initialize() {
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
+            } else {
+                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
+            }
+        });
+
         turretSubsystem.setLauncher(2500);
         hopperSubsystem.spindexerOn();
 
@@ -63,8 +72,11 @@ public class AutoShootCommandChassisTurret extends Command{
     public void execute() {
         // Need to compute shoot speed and angle
 
+        Logger.recordOutput("AutoAlign-CenteringError", autoCenterController.getError());
+
         // No april tag in camera view. Rotate towards our theoretical closest tag
-        if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
+        if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ 
+            Logger.recordOutput("AutoAlignState", "Look for tag");
             Pose3d botPose = driveSubsystem.getBotPose();
             // Find closest target to us
             List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> filedTags.getTagPose(tagId).get()).toList();
@@ -77,14 +89,17 @@ public class AutoShootCommandChassisTurret extends Command{
         }
         else // We have an april tag, center it to the camera frame
         {
+            Logger.recordOutput("AutoAlignState", "Center on tag");
             // [tx, ty, tz, pitch, yaw, roll]
             double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
-
+            if(targetsPose.length >= 1){
-            driveSubsystem.rotateChassis(autoCenterController.calculate(targetsPose[0])); // This may need to be negated
+                driveSubsystem.rotateChassis(-autoCenterController.calculate(targetsPose[0])); // This may need to be negated
+            }
         }
 
         // If robot is centered on HUB and shooter is up to speed. Run the kicker
         if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){
+            Logger.recordOutput("AutoAlignState", "FIRE!");
             hopperSubsystem.feedKicker();
         }
     }

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

@@ -15,6 +15,7 @@ import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -134,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem {
 
 
         // Set vision measurement confidence values
-        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999));
+        // driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7));
 
         this.setDefaultCommand(Commands.run(new Runnable() {
 
@@ -191,13 +192,17 @@ public class DriveSubsystem extends TestableSubsystem {
         super.periodic();
         driveSystem.periodic();
 
+        Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
+
         LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
-        if(frontCameraPose.tagCount >= 2){
+        if(frontCameraPose.tagCount >= 1){
             driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
+            Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
         }
         LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
-        if(backCameraPose.tagCount >= 2){
+        if(backCameraPose.tagCount >= 1){
             driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
+            Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
         }
     }
 

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

@@ -27,6 +27,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 import frc.robot.Constants;
+import edu.wpi.first.units.AngleUnit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -106,12 +107,8 @@ public class TurretSubsystem extends TestableSubsystem {
 
   public void setTargetElevation(Angle elev)
   {
-    if(Constants.ShooterConstants.HOOD_SERVO_INVERTED){
+    double angle = elev.in(Units.Degrees);
-      elev=Units.Degrees.of(300).minus(elev);
+    hood.setAngle(300/angle);
-    }
-    elev=elev.div(1.5);
-    elev=elev.div(Constants.ShooterConstants.DEFLECTOR_SERVO_RATIO);
-    hood.setAngle(elev.in(Units.Degrees));
   }
 
   public Command revSpeedCommand()