add angletoSpeaker() to localization

.idx/dev.nix

@@ -0,0 +1,58 @@
+# To learn more about how to use Nix to configure your environment
+# see: https://developers.google.com/idx/guides/customize-idx-env
+{ pkgs, ... }: {
+  # Which nixpkgs channel to use.
+  channel = "stable-23.11"; # or "unstable"
+
+  # Use https://search.nixos.org/packages to find packages
+  packages = [
+    # pkgs.go
+    # pkgs.python311
+    # pkgs.python311Packages.pip
+    # pkgs.nodejs_20
+    # pkgs.nodePackages.nodemon
+    pkgs.jdk17
+  ];
+
+  # Sets environment variables in the workspace
+  env = { };
+  idx = {
+    # Search for the extensions you want on https://open-vsx.org/ and use "publisher.id"
+    extensions = [
+      "editorconfig.editorconfig"
+      "vscjava.vscode-java-pack"
+      "richardwillis.vscode-spotless-gradle"
+    ];
+
+    # Enable previews
+    previews = {
+      enable = false;
+      previews = {
+        # web = {
+        #   # Example: run "npm run dev" with PORT set to IDX's defined port for previews,
+        #   # and show it in IDX's web preview panel
+        #   command = ["npm" "run" "dev"];
+        #   manager = "web";
+        #   env = {
+        #     # Environment variables to set for your server
+        #     PORT = "$PORT";
+        #   };
+        # };
+      };
+    };
+
+    # Workspace lifecycle hooks
+    workspace = {
+      # Runs when a workspace is first created
+      onCreate = {
+        # Example: install JS dependencies from NPM
+        # npm-install = "npm install";
+      };
+      # Runs when the workspace is (re)started
+      onStart = {
+        # Example: start a background task to watch and re-build backend code
+        # watch-backend = "npm run watch-backend";
+      };
+    };
+  };
+}

src/main/java/frc/robot/imu/ImuSubsystem.java

@@ -4,12 +4,17 @@
 import dev.doglog.DogLog;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
+import frc.robot.vision.VisionSubsystem;
 
 public class ImuSubsystem extends StateMachine<ImuState> {
   private final Pigeon2 imu;
   private Rotation2d robotHeading = new Rotation2d();
+  VisionSubsystem vision = new VisionSubsystem(null);
+  private final LocalizationSubsystem localization = new LocalizationSubsystem(this, vision);
+
 
   public ImuSubsystem(Pigeon2 imu) {
     super(SubsystemPriority.IMU, ImuState.DEFAULT_STATE);
@@ -24,23 +29,6 @@ protected void collectInputs() {
   public Rotation2d getRobotHeading() {
     return robotHeading;
   }
-  private double getDistanceToSpeaker(double distance){
-    return distance;
-  }
-  public double angleToSpeaker(){
-    double angle =
-        Units.radiansToDegrees(
-            Math.atan2(target.getY() - current.getY(), target.getX() - current.getX()));
-
-  }
-  public boolean atAngleForSpeaker(double angleToSpeaker, double robotHeading){
-    angleToSpeaker = angleToSpeaker();
-    robotHeading=getRobotHeading();
-    if(MathUtil.isNear(angleToSpeaker(), getRobotHeading(), 3)){
-      return true;
-    }
-    return false;
-  }
 
   @Override
   public void robotPeriodic() {

src/main/java/frc/robot/localization/LocalizationSubsystem.java

@@ -1,9 +1,11 @@
 package frc.robot.localization;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.units.Distance;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.config.RobotConfig;
 import frc.robot.imu.ImuSubsystem;
@@ -38,6 +40,36 @@ public Pose2d getPose() {
     return poseEstimator.getEstimatedPosition();
   }
 
+  public double angleToSpeaker(){
+
+
+
+    Pose2d speakerPose = vision.getSpeakerPose();
+    double angleToSpeaker = distanceAngleToTarget(speakerPose, getPose());
+
+    return angleToSpeaker;
+
+  }
+  public double angleToFeeding(){
+    double angleToFeeding = 0.0;
+    return angleToFeeding;
+
+  }
+  public boolean atAngleForSpeaker(){
+
+    if(MathUtil.isNear(angleToSpeaker(), imu.getRobotHeading().getDegrees(), 3)){
+      return true;
+    }
+    return false;
+  }
+  public boolean atAngleForFeeding(){
+
+    if(MathUtil.isNear(angleToFeeding(), getRobotHeading().getDegrees(), 3)){
+      return true;
+    }
+    return false;
+  }
+
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();

src/main/java/frc/robot/robot_manager/RobotManager.java

@@ -67,13 +67,13 @@ protected RobotState getNextState(RobotState currentState) {
           queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP;
 
       case SPEAKER_PREPARE_TO_SCORE ->
-          shooter.atGoal() && arm.atGoal()&&swerve.isSlowEnoughToShoot() ? RobotState.SPEAKER_SCORING : currentState;
+          shooter.atGoal() && arm.atGoal()&&swerve.isSlowEnoughToShoot()&&imu.atAngleForSpeaker()? RobotState.SPEAKER_SCORING : currentState;
 
       case AMP_PREPARE_TO_SCORE ->
           shooter.atGoal() && arm.atGoal()? RobotState.AMP_SCORING : currentState;
 
       case FEEDING_PREPARE_TO_SHOOT ->
-          shooter.atGoal() && arm.atGoal()&&swerve.isSlowEnoughToFeed() ? RobotState.FEEDING_SHOOTING : currentState;
+          shooter.atGoal() && arm.atGoal()&&swerve.isSlowEnoughToFeed()&&imu.atAngleForFeeding() ? RobotState.FEEDING_SHOOTING : currentState;
       case PASS_PREPARE_TO_SHOOT ->
           shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : currentState;
       case SUBWOOFER_PREPARE_TO_SCORE ->