start

team581 · Sep 14, 2024 · c1ef608 · c1ef608
1 parent 1a59d99
commit c1ef608
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java
@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.hardware.Pigeon2;
 import dev.doglog.DogLog;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
@@ -23,6 +24,19 @@ protected void collectInputs() {
   public Rotation2d getRobotHeading() {
     return robotHeading;
   }
+  private double getDistanceToSpeaker(double distance){
+    return distance;
+  }
+  public boolean atAngleForSpeaker(){
+    double distanceToSpeaker = ;
+    double angleToSpeaker;
+
+    angleToSpeaker = Math.cos(adjacent/distanceToSpeaker);
+    if(MathUtil.isNear(, getRobotHeading(), 3)){
+      return true;
+    }
+    return false;
+  }
 
   @Override
   public void robotPeriodic() {

diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java
@@ -10,6 +10,7 @@
 import frc.robot.queuer.QueuerSubsystem;
 import frc.robot.shooter.ShooterState;
 import frc.robot.shooter.ShooterSubsystem;
+import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
 import frc.robot.vision.VisionSubsystem;
@@ -22,6 +23,7 @@ public class RobotManager extends StateMachine<RobotState> {
   public final ImuSubsystem imu;
   public final IntakeSubsystem intake;
   public final QueuerSubsystem queuer;
+  public final SwerveSubsystem swerve;
 
   private final double distanceToFeedSpot = 0.0;
   private final double distanceToSpeaker = 0.0;
@@ -33,7 +35,8 @@ public RobotManager(
       VisionSubsystem vision,
       ImuSubsystem imu,
       IntakeSubsystem intake,
-      QueuerSubsystem queuer) {
+      QueuerSubsystem queuer,
+      SwerveSubsystem swerve) {
     super(SubsystemPriority.ROBOT_MANAGER, RobotState.IDLE_NO_GP);
     this.arm = arm;
     this.shooter = shooter;
@@ -42,6 +45,7 @@ public RobotManager(
     this.imu = imu;
     this.intake = intake;
     this.queuer = queuer;
+    this.swerve = swerve;
   }
 
   @Override
@@ -63,13 +67,13 @@ protected RobotState getNextState(RobotState currentState) {
           queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP;
 
       case SPEAKER_PREPARE_TO_SCORE ->
-          shooter.atGoal() && arm.atGoal() ? RobotState.SPEAKER_SCORING : currentState;
+          shooter.atGoal() && arm.atGoal()&&swerve.isSlowEnoughToShoot() ? RobotState.SPEAKER_SCORING : currentState;
 
       case AMP_PREPARE_TO_SCORE ->
-          shooter.atGoal() && arm.atGoal() ? RobotState.AMP_SCORING : currentState;
+          shooter.atGoal() && arm.atGoal()? RobotState.AMP_SCORING : currentState;
 
       case FEEDING_PREPARE_TO_SHOOT ->
-          shooter.atGoal() && arm.atGoal() ? RobotState.FEEDING_SHOOTING : currentState;
+          shooter.atGoal() && arm.atGoal()&&swerve.isSlowEnoughToFeed() ? RobotState.FEEDING_SHOOTING : currentState;
       case PASS_PREPARE_TO_SHOOT ->
           shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : currentState;
       case SUBWOOFER_PREPARE_TO_SCORE ->