Assisted intaking

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

@@ -15,6 +15,7 @@
 import frc.robot.generated.BuildConstants;
 import frc.robot.imu.ImuSubsystem;
 import frc.robot.intake.IntakeSubsystem;
+import frc.robot.intake_assist.IntakeAssistManager;
 import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.queuer.QueuerSubsystem;
 import frc.robot.robot_manager.RobotCommands;
@@ -39,7 +40,9 @@ public class Robot extends TimedRobot {
   private final ArmSubsystem arm = new ArmSubsystem(hardware.armLeft, hardware.armRight);
   private final IntakeSubsystem intake =
       new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor);
-  private final SwerveSubsystem swerve = new SwerveSubsystem();
+
+  private final IntakeAssistManager intakeAssistManager = new IntakeAssistManager();
+  private final SwerveSubsystem swerve = new SwerveSubsystem(intakeAssistManager);
   private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon);
   private final Limelight leftLimelight =
       new Limelight("left", InterpolatedVisionDataset.HOME.leftSet);
@@ -176,8 +179,11 @@ private void configureBindings() {
                 () -> {
                   robotManager.setConfirmShotActive(false);
                 }));
-    hardware.driverController.leftTrigger().onTrue(robotCommands.intakeCommand());
-
+    hardware
+        .driverController
+        .leftTrigger()
+        .onTrue(robotCommands.intakeAssistCommand())
+        .onFalse(robotCommands.intakeCommand());
     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 static double robotHeading = 0;
   private double pitch;
   private double angularVelocity;
   private double pitchRate;
@@ -22,15 +21,15 @@ public ImuSubsystem(Pigeon2 imu) {
 
   @Override
   protected void collectInputs() {
-    robotHeading = Rotation2d.fromDegrees(imu.getYaw().getValue());
+    robotHeading = imu.getYaw().getValue();
     angularVelocity = imu.getRate();
     pitch = imu.getPitch().getValueAsDouble();
     pitchRate = imu.getAngularVelocityYWorld().getValueAsDouble();
     roll = imu.getRoll().getValueAsDouble();
     rollRate = imu.getAngularVelocityXWorld().getValueAsDouble();
   }
 
-  public Rotation2d getRobotHeading() {
+  public static double getRobotHeading() {
     return robotHeading;
   }
 
@@ -61,7 +60,7 @@ public void setAngle(double zeroAngle) {
   @Override
   public void robotPeriodic() {
     super.robotPeriodic();
-    DogLog.log("Imu/RobotHeading", robotHeading.getDegrees());
+    DogLog.log("Imu/RobotHeading", robotHeading);
     DogLog.log("Imu/AngularVelocity", angularVelocity);
     DogLog.log("Imu/Pitch", pitch);
   }

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

@@ -0,0 +1,51 @@
+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.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 = 3.5;
+  private static final String LIMELIGHT_NAME = "limelight-note";
+  private static final double MAX_ANGLE_CHANGE = -35.0;
+  private static final double MIN_ANGLE_CHANGE = 35.0;
+
+  public IntakeAssistManager() {
+    super(SubsystemPriority.INTAKE_ASSIST_MANAGER);
+  }
+
+  public ChassisSpeeds getRobotRelativeAssistSpeeds(ChassisSpeeds fieldRelativeInputSpeeds) {
+
+    double tx = LimelightHelpers.getTX(LIMELIGHT_NAME);
+
+    if (tx == 0) {
+      return fieldRelativeInputSpeeds;
+    }
+
+    DogLog.log("IntakeAssist/TX", tx);
+
+    double fieldRelativeNoteAngle = ImuSubsystem.getRobotHeading() + tx;
+
+    double angleError = ImuSubsystem.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);
+  }
+}

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

@@ -39,7 +39,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));
@@ -46,7 +51,7 @@ public Command idleWithGpCommand() {
   }
 
   public Command stopIntakingCommand() {
-    return Commands.runOnce(robot::stopIntakingRequest, requirements)
+    return Commands.runOnce(robot::stowRequest, requirements)
         .andThen(robot.waitForState(RobotState.IDLE_NO_GP));
   }
 

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

@@ -13,6 +13,7 @@
 import frc.robot.shooter.ShooterState;
 import frc.robot.shooter.ShooterSubsystem;
 import frc.robot.swerve.SnapUtil;
+import frc.robot.swerve.SwerveState;
 import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
@@ -102,7 +103,7 @@ protected RobotState getNextState(RobotState currentState) {
           shooter.atGoal() && arm.atGoal() ? RobotState.PODIUM_SCORING : currentState;
 
       case UNJAM -> currentState;
-      case INTAKING -> queuer.hasNote() ? RobotState.INTAKING_BACK : currentState;
+      case INTAKING, INTAKE_ASSIST -> queuer.hasNote() ? RobotState.INTAKING_BACK : currentState;
       case INTAKING_BACK -> !queuer.hasNote() ? RobotState.INTAKING_FORWARD_PUSH : currentState;
     };
   }
@@ -221,6 +222,7 @@ protected void afterTransition(RobotState newState) {
         intake.setState(IntakeState.INTAKING);
         queuer.setState(QueuerState.INTAKING);
         swerve.setSnapsEnabled(false);
+        swerve.setState(SwerveState.TELEOP);
       }
       case INTAKING_BACK -> {
         arm.setState(ArmState.IDLE);
@@ -236,6 +238,17 @@ protected void afterTransition(RobotState newState) {
         queuer.setState(QueuerState.INTAKING_FORWARD_PUSH);
         swerve.setSnapsEnabled(false);
       }
+      case INTAKE_ASSIST -> {
+        arm.setState(ArmState.IDLE);
+        shooter.setState(ShooterState.IDLE_STOPPED);
+        intake.setState(IntakeState.INTAKING);
+        queuer.setState(QueuerState.INTAKING);
+        if (DriverStation.isTeleop()) {
+          swerve.setState(SwerveState.INTAKE_ASSIST_TELEOP);
+        } else {
+          swerve.setState(SwerveState.INTAKE_ASSIST_AUTO);
+        }
+      }
       case OUTTAKING -> {
         arm.setState(ArmState.IDLE);
         shooter.setState(ShooterState.IDLE_STOPPED);
@@ -354,15 +367,15 @@ public void intakeRequest() {
     }
   }
 
-  // TODO: This seems like we ended up not really needing it, can remove it in favor of
-  // stowRequest()
-  public void stopIntakingRequest() {
+  public void intakeAssistRequest() {
     switch (getState()) {
-      case INTAKING -> setStateFromRequest(RobotState.IDLE_NO_GP);
-      default -> {}
+      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 outtakeRequest() {
     switch (getState()) {
       case CLIMBING_1_LINEUP, CLIMBING_2_HANGING -> {}

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

@@ -20,6 +20,7 @@ public enum RobotState {
   INTAKING_BACK,
   INTAKING_FORWARD_PUSH,
   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

@@ -12,6 +12,7 @@
 import edu.wpi.first.math.util.Units;
 import frc.robot.config.RobotConfig;
 import frc.robot.fms.FmsSubsystem;
+import frc.robot.intake_assist.IntakeAssistManager;
 import frc.robot.util.ControllerHelpers;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
@@ -55,6 +56,8 @@ public class SwerveSubsystem extends StateMachine<SwerveState> {
 
   public final Pigeon2 drivetrainPigeon = drivetrain.getPigeon2();
 
+  private final IntakeAssistManager intakeAssistManager;
+
   private final SwerveRequest.FieldCentric drive =
       new SwerveRequest.FieldCentric()
           // I want field-centric driving in open loop
@@ -83,6 +86,10 @@ public class SwerveSubsystem extends StateMachine<SwerveState> {
 
   private ChassisSpeeds autoSpeeds = new ChassisSpeeds();
 
+  private ChassisSpeeds intakeAssistTeleopSpeeds = new ChassisSpeeds();
+
+  private ChassisSpeeds intakeAssistAutoSpeeds = new ChassisSpeeds();
+
   public ChassisSpeeds getRobotRelativeSpeeds() {
     return robotRelativeSpeeds;
   }
@@ -107,16 +114,27 @@ public void setSnapToAngle(double angle) {
     goalSnapAngle = angle;
   }
 
-  public SwerveSubsystem() {
+  public SwerveSubsystem(IntakeAssistManager intakeAssistManager) {
     super(SubsystemPriority.SWERVE, SwerveState.TELEOP);
     modulePositions = calculateModulePositions();
+    this.intakeAssistManager = intakeAssistManager;
   }
 
   public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) {
     autoSpeeds = speeds;
     sendSwerveRequest();
   }
 
+  public void setIntakeAssistTeleopSpeeds(ChassisSpeeds speeds) {
+    intakeAssistTeleopSpeeds = speeds;
+    sendSwerveRequest();
+  }
+
+  public void setIntakeAssistAutoSpeeds(ChassisSpeeds speeds) {
+    intakeAssistAutoSpeeds = speeds;
+    sendSwerveRequest();
+  }
+
   public void driveTeleop(double x, double y, double theta) {
     double leftY =
         -1.0
@@ -159,6 +177,7 @@ protected void collectInputs() {
     fieldRelativeSpeeds = calculateFieldRelativeSpeeds();
     slowEnoughToShoot = calculateMovingSlowEnoughForSpeakerShot(robotRelativeSpeeds);
     slowEnoughToFeed = calculateMovingSlowEnoughForFloorShot(robotRelativeSpeeds);
+
   }
 
   private List<SwerveModulePosition> calculateModulePositions() {
@@ -209,6 +228,7 @@ private void sendSwerveRequest() {
                   .withVelocityY(teleopSpeeds.vyMetersPerSecond)
                   .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
                   .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+
         } else {
           drivetrain.setControl(
               drive
@@ -218,6 +238,16 @@ private void sendSwerveRequest() {
                   .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
         }
       }
+      case INTAKE_ASSIST_TELEOP -> {
+         intakeAssistTeleopSpeeds = intakeAssistManager.getRobotRelativeAssistSpeeds(teleopSpeeds);
+
+          drivetrain.setControl(
+              drive
+                  .withVelocityX(intakeAssistTeleopSpeeds.vxMetersPerSecond)
+                  .withVelocityY(intakeAssistTeleopSpeeds.vyMetersPerSecond)
+                  .withRotationalRate(intakeAssistTeleopSpeeds.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.OpenLoopVoltage));
+      }
       case AUTO ->
           drivetrain.setControl(
               drive
@@ -232,8 +262,18 @@ private void sendSwerveRequest() {
                   .withVelocityY(autoSpeeds.vyMetersPerSecond)
                   .withTargetDirection(Rotation2d.fromDegrees(goalSnapAngle))
                   .withDriveRequestType(DriveRequestType.Velocity));
+
+      case INTAKE_ASSIST_AUTO -> {
+        intakeAssistAutoSpeeds = intakeAssistManager.getRobotRelativeAssistSpeeds(autoSpeeds);
+          drivetrain.setControl(
+              drive
+                  .withVelocityX(intakeAssistAutoSpeeds.vxMetersPerSecond)
+                  .withVelocityY(intakeAssistAutoSpeeds.vyMetersPerSecond)
+                  .withRotationalRate(intakeAssistAutoSpeeds.omegaRadiansPerSecond)
+                  .withDriveRequestType(DriveRequestType.Velocity));
     }
   }
+  }
 
   public void setState(SwerveState newState) {
     setStateFromRequest(newState);

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;
 

src/main/java/frc/robot/vision/VisionSubsystem.java

@@ -37,8 +37,8 @@ public VisionSubsystem(ImuSubsystem imu, Limelight leftLimelight, Limelight righ
 
   @Override
   protected void collectInputs() {
-    robotHeading = imu.getRobotHeading().getDegrees();
-    DogLog.log("Vision/RobotHeading", imu.getRobotHeading().getDegrees());
+    robotHeading = imu.getRobotHeading();
+    DogLog.log("Vision/RobotHeading", imu.getRobotHeading());
     angularVelocity = imu.getRobotAngularVelocity();
     pitch = imu.getPitch();
     pitchRate = imu.getPitchRate();