Assisted intaking

src/main/java/frc/robot/intake_assist/IntakeAssistManager.java

@@ -0,0 +1,55 @@
+package frc.robot.intake_assist;
+
+import dev.doglog.DogLog;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.imu.ImuSubsystem;
+import frc.robot.localization.LocalizationSubsystem;
+import frc.robot.swerve.SwerveSubsystem;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+
+public class IntakeAssistManager extends LifecycleSubsystem {
+  private static final String LIMELIGHT_NAME = "limelight-note";
+  private final LocalizationSubsystem localization;
+  private final SwerveSubsystem swerve;
+  private final ImuSubsystem imu;
+  private static final double MAX_ANGLE_CHANGE = -35.0;
+  private static final double MIN_ANGLE_CHANGE = 35.0;
+
+  public IntakeAssistManager(
+      LocalizationSubsystem localization, SwerveSubsystem swerve, ImuSubsystem imu) {
+    super(SubsystemPriority.INTAKE_ASSIST_MANAGER);
+
+    this.localization = localization;
+    this.swerve = swerve;
+    this.imu = imu;
+  }
+
+
+
+  public double getRobotRelativeAssistSpeeds(ChassisSpeeds fieldRelativeInputSpeeds ){
+
+    double tx = NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tx").getDouble(0);
+
+    Rotation2d robotToNoteAngle = Rotation2d.fromDegrees(tx);
+
+    Rotation2d fieldRelativeNoteAngle = imu.getRobotHeading().plus(robotToNoteAngle);
+
+    Rotation2d driverRequest = fieldRelativeNoteAngle;
+
+    double angleError = imu.getRobotHeading().minus(driverRequest).getDegrees();
+
+    double angleChange = Math.max(MAX_ANGLE_CHANGE, Math.min(MIN_ANGLE_CHANGE, angleError * 0.2));
+
+    return angleChange;
+
+
+
+  }
+
+
+}

src/main/java/frc/robot/swerve/SwerveState.java

@@ -4,5 +4,7 @@ public enum SwerveState {
   TELEOP,
   TELEOP_SNAPS,
   AUTO,
-  AUTO_SNAPS;
+  AUTO_SNAPS,
+  INTAKE_ASSIST_TELEOP,
+  INTAKE_ASSIST_AUTO;
 }

src/main/java/frc/robot/util/scheduling/SubsystemPriority.java

@@ -23,7 +23,8 @@ public enum SubsystemPriority {
 
   // Robot manager runs last so that all sensor data is fresh before processing state transitions
   ROBOT_MANAGER(0),
-  AUTOS(0);
+  AUTOS(0),
+  INTAKE_ASSIST_MANAGER(0);
 
   final int value;