From c62f94818a47bc12609d563d2e634ee907f5fa2a Mon Sep 17 00:00:00 2001 From: ryanknj5 Date: Wed, 18 Sep 2024 01:31:16 +0000 Subject: [PATCH] add angletoSpeaker() to localization --- .idx/dev.nix | 58 +++++++++++++++++++ src/main/java/frc/robot/imu/ImuSubsystem.java | 22 ++----- .../localization/LocalizationSubsystem.java | 32 ++++++++++ .../frc/robot/robot_manager/RobotManager.java | 4 +- 4 files changed, 97 insertions(+), 19 deletions(-) create mode 100644 .idx/dev.nix diff --git a/.idx/dev.nix b/.idx/dev.nix new file mode 100644 index 0000000..e222ccf --- /dev/null +++ b/.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"; + }; + }; + }; +} diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java index d7abf6d..d777004 100644 --- a/src/main/java/frc/robot/imu/ImuSubsystem.java +++ b/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 { 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() { diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index 4448e29..20618b1 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/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(); diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 8991524..e27db87 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/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 ->