More auto shoot

lib/RoboLib

@@ -1 +1 @@
-Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee
+Subproject commit ad53b44e2fb4c06b72ff2546b30e37446ac2fb8d

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

@@ -22,9 +22,16 @@ public final class Constants {
   }
 
   public static class CameraConstanst {
+
+    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 String BACK_CAMERA_NAME = "back";
     public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
-    public static final int BACK_HUB_TRACKING_PIPELINE_NUMBER = 1;
+    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 int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;

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

@@ -60,6 +60,7 @@ public class RobotContainer {
   private void configureBindings() {
     turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID()));
     hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, 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));
@@ -69,9 +70,8 @@ public class RobotContainer {
     m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
     m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
 
-    m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, intakeSubsystem, turretSubsystem));
+    m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
 
-    intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
   }
 
   /**

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

@@ -1,16 +1,20 @@
 package frc.robot.commands;
 
-import java.lang.constant.Constable;
+import java.util.Arrays;
+import java.util.List;
 
 import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
-import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers.LimelightTarget_Fiducial;
+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.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 
 public class AutoShootCommandChassisTurret extends Command{
@@ -19,41 +23,67 @@ public class AutoShootCommandChassisTurret extends Command{
     private DriveSubsystem driveSubsystem;
     
     private HopperSubsystem hopperSubsystem;
-    private IntakeSubsystem intakeSubsystem;
     private TurretSubsystem turretSubsystem;
 
     private PIDController autoCenterController;
 
-    public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, IntakeSubsystem intakeSubsystem, TurretSubsystem turretSubsystem)
+    private int[] tagFilters;
+
+    private AprilTagFieldLayout filedTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
+
+    private final double chassisRotateSpeed = Math.PI * 0.1;
+
+    public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem)
     {
         this.driveSubsystem = driveSubsystem;
         this.hopperSubsystem = hopperSubsystem;
-        this.intakeSubsystem = intakeSubsystem;
         this.turretSubsystem = turretSubsystem;
 
+        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
+
         autoCenterController = new PIDController(0.1, 0, 0);
-        autoCenterController.setTolerance(1);
+        autoCenterController.setTolerance(1); // 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() {
         turretSubsystem.setLauncher(2500);
         hopperSubsystem.spindexerOn();
+
+        // Change to HUB tracking pipeline
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER);
+        LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
+
     }
 
     @Override
     public void execute() {
         // Need to compute shoot speed and angle
 
-        // Get apriltags
+        // No april tag in camera view. Rotate towards our theoretical closest tag
-        // Change to HUB tracking pipeline
+        if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
-        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_PIPELINE_NUMBER);
+            Pose3d botPose = driveSubsystem.getBotPose();
+            // Find closest target to us
+            List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> filedTags.getTagPose(tagId).get()).toList();
+            for(int i=0;i<tagFilters.length; i++)
+            {
+                Pose3d closet = botPose.nearest(tagPoses);
+                Transform3d delta = botPose.minus(closet);
+                driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? chassisRotateSpeed : -chassisRotateSpeed); // May need to flip this
+            }
+        }
+        else // We have an april tag, center it to the camera frame
+        {
+            // [tx, ty, tz, pitch, yaw, roll]
+            double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
 
-        LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
+            driveSubsystem.rotateChassis(autoCenterController.calculate(targetsPose[0])); // This may need to be negated
-
+        }
-        // driveSubsystem. autoCenterController.calculate(0);
 
+        // If robot is centered on HUB and shooter is up to speed. Run the kicker
         if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){
             hopperSubsystem.feedKicker();
         }
@@ -71,6 +101,8 @@ public class AutoShootCommandChassisTurret extends Command{
         hopperSubsystem.stopKicker();
         hopperSubsystem.spindexerOff();
 
+        driveSubsystem.stopDrive();
+
         LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
     }
 

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

@@ -4,15 +4,7 @@
 
 package frc.robot.commands;
 
-import frc.robot.subsystems.ExampleSubsystem;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-
 public final class Autos {
-  /** Example static factory for an autonomous command. */
-  public static Command exampleAuto(ExampleSubsystem subsystem) {
-    return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem));
-  }
 
   private Autos() {
     throw new UnsupportedOperationException("This is a utility class!");

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

@@ -1,43 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.subsystems.ExampleSubsystem;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. */
-public class ExampleCommand extends Command {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ExampleSubsystem m_subsystem;
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ExampleCommand(ExampleSubsystem subsystem) {
-    m_subsystem = subsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

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

@@ -4,7 +4,6 @@
 
 package frc.robot.commands;
 
-import frc.robot.Constants;
 import frc.robot.subsystems.HopperSubsystem;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;

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

@@ -6,8 +6,6 @@ package frc.robot.commands;
 
 import frc.robot.subsystems.IntakeSubsystem;
 
-import javax.lang.model.util.ElementScanner14;
-
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 

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

@@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.Command;
 public class ManualShootCommand extends Command {
   private final TurretSubsystem m_subsystem;
   private final XboxController m_controller;
-  private double tempAng;
 
   public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) {
     m_subsystem = subsystem;

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

@@ -6,15 +6,18 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
 import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
 import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
 import org.littletonrobotics.junction.Logger;
 
-import com.fasterxml.jackson.annotation.JsonCreator.Mode;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -129,6 +132,10 @@ public class DriveSubsystem extends TestableSubsystem {
                                 .build())
                 .build();
 
+
+        // Set vision measurement confidence values
+        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999));
+
         this.setDefaultCommand(Commands.run(new Runnable() {
 
             @Override
@@ -169,9 +176,9 @@ public class DriveSubsystem extends TestableSubsystem {
                     },
                     this);
 
-            // autoChooser = AutoBuilder.buildAutoChooser();
+            autoChooser = AutoBuilder.buildAutoChooser();
-            // autoChooser.addOption("Do Nothing", Commands.none());
+            autoChooser.setDefaultOption("Do Nothing", Commands.none());
-            // SmartDashboard.putData("Auto Chooser", autoChooser);
+            SmartDashboard.putData("Auto Chooser", autoChooser);
 
         } catch (Exception e) {
             e.printStackTrace();
@@ -183,6 +190,15 @@ public class DriveSubsystem extends TestableSubsystem {
     public void periodic() {
         super.periodic();
         driveSystem.periodic();
+
+        LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
+        if(frontCameraPose.tagCount >= 2){
+            driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
+        }
+        LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
+        if(backCameraPose.tagCount >= 2){
+            driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
+        }
     }
 
     @Override
@@ -192,8 +208,19 @@ public class DriveSubsystem extends TestableSubsystem {
     }
 
     public Command getAutoCommand(){
-        return Commands.none();
+        return autoChooser.getSelected();
-        // return autoChooser.getSelected();
+    }
+
+    public void stopDrive(){
+        driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
+    }
+
+    public void rotateChassis(double rotSpeed){
+        driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
+    }
+
+    public Pose3d getBotPose() {
+        return driveSystem.getRobotPose3d();
     }
 
     @PostTest()

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

@@ -10,11 +10,7 @@ import org.fairportrobotics.frc.posty.test.PostTest;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
-import edu.wpi.first.wpilibj2.command.Command;
-
 import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
-import static org.fairportrobotics.frc.robolib.motors.Utils.*;
 import frc.robot.Constants;
 
 public class HopperSubsystem extends TestableSubsystem {

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

@@ -17,8 +17,6 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import static org.fairportrobotics.frc.robolib.motors.Utils.*;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 
 public class IntakeSubsystem extends TestableSubsystem {
   /** Creates a new ExampleSubsystem. */