Assisted intaking

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

@@ -2,13 +2,12 @@
 
 import com.ctre.phoenix6.hardware.Pigeon2;
 import dev.doglog.DogLog;
-import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
 
 public class ImuSubsystem extends StateMachine<ImuState> {
   private final Pigeon2 imu;
-  private Rotation2d robotHeading = new Rotation2d();
+  private double robotHeading = 0;
 
   public ImuSubsystem(Pigeon2 imu) {
     super(SubsystemPriority.IMU, ImuState.DEFAULT_STATE);
@@ -17,16 +16,16 @@ public ImuSubsystem(Pigeon2 imu) {
 
   @Override
   protected void collectInputs() {
-    robotHeading = Rotation2d.fromDegrees(imu.getYaw().getValue());
+    robotHeading = imu.getYaw().getValue();
   }
 
-  public Rotation2d getRobotHeading() {
+  public double getRobotHeading() {
     return robotHeading;
   }
 
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
-    DogLog.log("Imu/RobotHeading", robotHeading.getDegrees());
+    DogLog.log("Imu/RobotHeading", robotHeading);
   }
 }

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

@@ -0,0 +1,62 @@
+package frc.robot.intake_assist;
+
+import dev.doglog.DogLog;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.imu.ImuSubsystem;
+import frc.robot.swerve.SwerveSubsystem;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+import frc.robot.vision.LimelightHelpers;
+
+public class IntakeAssistManager extends LifecycleSubsystem {
+  private static final double ASSIST_KP = 0.2;
+  private static final String LIMELIGHT_NAME = "limelight-note";
+  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(SwerveSubsystem swerve, ImuSubsystem imu) {
+    super(SubsystemPriority.INTAKE_ASSIST_MANAGER);
+    this.swerve = swerve;
+    this.imu = imu;
+  }
+
+
+
+  public ChassisSpeeds getRobotRelativeAssistSpeeds(ChassisSpeeds fieldRelativeInputSpeeds ){
+
+    double tx = LimelightHelpers.getTX(LIMELIGHT_NAME);
+
+
+    if (tx == 0) {
+      return fieldRelativeInputSpeeds;
+    }
+
+    DogLog.log("IntakeAssist/TX", tx);
+
+    double fieldRelativeNoteAngle = imu.getRobotHeading()  + tx;
+
+    double angleError = imu.getRobotHeading() - fieldRelativeNoteAngle;
+
+    double angleChange = MathUtil.clamp(MIN_ANGLE_CHANGE, MAX_ANGLE_CHANGE, angleError * ASSIST_KP);
+
+    Translation2d requestedFieldRelativeDrive = new Translation2d(fieldRelativeInputSpeeds.vxMetersPerSecond,
+    fieldRelativeInputSpeeds.vyMetersPerSecond);
+
+    Translation2d newDriveRequest = requestedFieldRelativeDrive.rotateBy(Rotation2d.fromDegrees(angleChange));
+
+    return new ChassisSpeeds(newDriveRequest.getX(), newDriveRequest.getY(), fieldRelativeInputSpeeds.omegaRadiansPerSecond);
+
+  }
+
+  public void updateSwerveSpeeds() {
+    ChassisSpeeds intakeAssistSpeeds = getRobotRelativeAssistSpeeds(swerve.getFieldRelativeSpeeds());
+    swerve.setIntakeAssistTeleopSpeeds(intakeAssistSpeeds);
+    swerve.setIntakeAssistAutoSpeeds(intakeAssistSpeeds);
+}
+
+}

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

@@ -3,6 +3,7 @@
 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.geometry.Rotation2d;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.wpilibj.Timer;
@@ -33,7 +34,7 @@ public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision, SwerveSub
     poseEstimator =
         new SwerveDrivePoseEstimator(
             SwerveSubsystem.KINEMATICS,
-            imu.getRobotHeading(),
+            Rotation2d.fromDegrees(imu.getRobotHeading()),
             swerve.getModulePositions().toArray(new SwerveModulePosition[4]),
             new Pose2d());
   }

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

@@ -76,6 +76,7 @@ protected RobotState getNextState(RobotState currentState) {
           shooter.atGoal() && arm.atGoal() ? RobotState.SUBWOOFER_SCORING : currentState;
       case UNJAM -> currentState;
       case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState;
+      case INTAKE_ASSIST -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState;
       case OUTTAKING -> queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP;
     };
   }
@@ -155,6 +156,12 @@ protected void afterTransition(RobotState newState) {
         intake.setState(IntakeState.INTAKING);
         queuer.setState(QueuerState.INTAKING);
       }
+      case INTAKE_ASSIST -> {
+        arm.setState(ArmState.IDLE);
+        shooter.setState(ShooterState.IDLE_STOPPED);
+        intake.setState(IntakeState.INTAKING);
+        queuer.setState(QueuerState.INTAKING);
+      }
       case OUTTAKING -> {
         arm.setState(ArmState.IDLE);
         shooter.setState(ShooterState.IDLE_STOPPED);
@@ -252,6 +259,13 @@ public void intakeRequest() {
     }
   }
 
+  public void intakeAssistRequest() {
+    switch(getState()) {
+      case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}
+      default -> setStateFromRequest(RobotState.INTAKE_ASSIST);
+    }
+  }
+
   // TODO: This seems like we ended up not really needing it, can remove it in favor of
   // stowRequest()
   public void stopIntakingRequest() {
@@ -261,6 +275,13 @@ public void stopIntakingRequest() {
     }
   }
 
+  public void stopIntakeAssistRequest() {
+    switch (getState()) {
+      case INTAKE_ASSIST -> setStateFromRequest(RobotState.IDLE_NO_GP);
+      default -> {}
+    }
+  }
+
   public void outtakeRequest() {
     switch (getState()) {
       case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}

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

@@ -18,6 +18,7 @@ public enum RobotState {
 
   INTAKING,
   OUTTAKING,
+  INTAKE_ASSIST,
 
   UNJAM,
 

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/swerve/SwerveSubsystem.java

@@ -117,6 +117,16 @@ public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) {
     sendSwerveRequest();
   }
 
+  public void setIntakeAssistTeleopSpeeds(ChassisSpeeds speeds) {
+    teleopSpeeds = speeds;
+    sendSwerveRequest();
+  }
+
+  public void setIntakeAssistAutoSpeeds(ChassisSpeeds speeds) {
+    autoSpeeds = speeds;
+    sendSwerveRequest();
+  }
+
   public void driveTeleop(double x, double y, double theta) {
     double leftY =
         -1.0
@@ -192,6 +202,7 @@ private boolean calculateMovingSlowEnoughForFloorShot(ChassisSpeeds speeds) {
     return linearSpeed < MAX_FLOOR_SPEED_SHOOTING;
   }
 
+
   private void sendSwerveRequest() {
     switch (getState()) {
       case TELEOP ->
@@ -209,15 +220,23 @@ private void sendSwerveRequest() {
                   .withVelocityY(teleopSpeeds.vyMetersPerSecond)
                   .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
                   .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
-        } else {
+
+         } else {
+                    drivetrain.setControl(
+                        drive
+                            .withVelocityX(teleopSpeeds.vxMetersPerSecond)
+                            .withVelocityY(teleopSpeeds.vyMetersPerSecond)
+                            .withRotationalRate(teleopSpeeds.omegaRadiansPerSecond)
+                            .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+                  }
+                }
+        case INTAKE_ASSIST_TELEOP ->
           drivetrain.setControl(
               drive
                   .withVelocityX(teleopSpeeds.vxMetersPerSecond)
                   .withVelocityY(teleopSpeeds.vyMetersPerSecond)
                   .withRotationalRate(teleopSpeeds.omegaRadiansPerSecond)
                   .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
-        }
-      }
       case AUTO ->
           drivetrain.setControl(
               drive
@@ -232,6 +251,14 @@ private void sendSwerveRequest() {
                   .withVelocityY(autoSpeeds.vyMetersPerSecond)
                   .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
                   .withDriveRequestType(DriveRequestType.Velocity));
+
+      case INTAKE_ASSIST_AUTO ->
+            drivetrain.setControl(
+              drive
+                  .withVelocityX(teleopSpeeds.vxMetersPerSecond)
+                  .withVelocityY(teleopSpeeds.vyMetersPerSecond)
+                  .withRotationalRate(teleopSpeeds.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+        }
     }
   }
-}

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;