Assisted intaking

src/main/java/frc/robot/Robot.java

@@ -43,7 +43,7 @@ public class Robot extends TimedRobot {
   private final LocalizationSubsystem localization = new LocalizationSubsystem(imu, vision, swerve);
   private final Autos autos = new Autos();
   private final RobotManager robotManager =
-      new RobotManager(arm, shooter, localization, vision, imu, intake, queuer);
+      new RobotManager(arm, shooter, localization, vision, imu, intake, queuer, swerve);
 
   private final RobotCommands robotCommands = new RobotCommands(robotManager);
 
@@ -162,7 +162,7 @@ private void configureBindings() {
         .driverController
         .leftTrigger()
         .onTrue(robotCommands.intakeCommand())
-        .onFalse(robotCommands.stowCommand());
+        .onFalse(robotCommands.intakeAssistCommand());
     hardware
         .driverController
         .rightBumper()

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/RobotCommands.java

@@ -20,6 +20,11 @@ public Command intakeCommand() {
         .andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
   }
 
+  public Command intakeAssistCommand() {
+    return Commands.runOnce(robot::intakeAssistRequest, requirements)
+        .andThen(robot.waitForState(RobotState.IDLE_WITH_GP));
+  }
+
   public Command outtakeCommand() {
     return Commands.runOnce(robot::outtakeRequest, requirements)
         .andThen(robot.waitForState(RobotState.IDLE_NO_GP));

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

@@ -1,5 +1,6 @@
 package frc.robot.robot_manager;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.arm.ArmState;
 import frc.robot.arm.ArmSubsystem;
 import frc.robot.imu.ImuSubsystem;
@@ -10,6 +11,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 +24,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 +36,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 +46,7 @@ public RobotManager(
     this.imu = imu;
     this.intake = intake;
     this.queuer = queuer;
+    this.swerve = swerve;
   }
 
   @Override
@@ -75,7 +80,7 @@ protected RobotState getNextState(RobotState currentState) {
       case SUBWOOFER_PREPARE_TO_SCORE ->
           shooter.atGoal() && arm.atGoal() ? RobotState.SUBWOOFER_SCORING : currentState;
       case UNJAM -> currentState;
-      case INTAKING -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState;
+      case INTAKING, INTAKE_ASSIST -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : currentState;
       case OUTTAKING -> queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP;
     };
   }
@@ -155,6 +160,17 @@ 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);
+        if (DriverStation.isTeleop()) {
+          swerve.setIntakeAssistTeleopSpeeds(swerve.getRobotRelativeSpeeds());
+        } else {
+          swerve.setIntakeAssistAutoSpeeds(swerve.getRobotRelativeSpeeds());
+        }
+      }
       case OUTTAKING -> {
         arm.setState(ArmState.IDLE);
         shooter.setState(ShooterState.IDLE_STOPPED);
@@ -252,6 +268,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() {

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(autoSpeeds.vxMetersPerSecond)
+                  .withVelocityY(autoSpeeds.vyMetersPerSecond)
+                  .withRotationalRate(autoSpeeds.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.Velocity));
+        }
     }
   }
-}

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;