intake fix atttempt

src/main/deploy/pathplanner/autos/intake.auto

@@ -0,0 +1,37 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.4096302478319247,
+      "y": 5.629231829892232
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "actuateOut"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.5
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "actuateIn"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/java/frc/robot/Constants.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 /**
@@ -57,7 +58,7 @@ public static class Speeds {
     public static final double SWERVE_DRIVE_FAST_COEFF = 1;
 
     public static final double SWERVE_DRIVE_MAX_SPEED = 4; // meters per second
-    public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 1; // TODO: Get this value
+    public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 1.5; // TODO: Get this value
 
     public static final double SHOOTER_FAST_SPIN_SPEED = .85; // TODO: Get this
     public static final double SHOOTER_SLOW_SPIN_SPEED = .85;
@@ -160,6 +161,8 @@ public static class FieldConstants {
     public static final int SPEAKER_TAG_BLUE = 7;
     public static final int SPEAKER_TAG_RED = 4;
 
+    public static final double SPEAKER_TAG_OFFSET = Units.inchesToMeters(23);
+
     // FIELD WIDTH: 651.223 INCHES
 
     // x: .2472 y: 5.5946 (USE THIS)

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.utils.UtilFuncs;
 import frc.robot.utils.helpers.AllianceHelper;
 
 /**
@@ -104,6 +105,7 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
+    // UtilFuncs.LoadField();
     m_robotContainer.teleopInit();
 
     if (m_autonomousCommand != null) {

src/main/java/frc/robot/RobotContainer.java

@@ -90,8 +90,8 @@ public RobotContainer() {
     NamedCommands.registerCommand("brake", new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
 
     NamedCommands.registerCommand("actuateOut", new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen(
-      new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)
-    ));
+      new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE))
+    );
 
     NamedCommands.registerCommand("actuateIn", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.INTAKE).alongWith(
       new SpinShooter(_shooterSubsystem, ShooterState.SHOOT, true)

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -73,20 +73,6 @@ public IntakeSubsystem() {
     _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
   }
 
-  /**
-   * Toggle reverse soft limit.
-   */
-  public void toggleReverseSoftLimit() {
-    _actuatorMotor.enableSoftLimit(SoftLimitDirection.kReverse, !_actuatorMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse));
-  }
-
-  /**
-   * Resets the reverse soft limit (and encoder) of the actuator.
-   */
-  public void resetActuatorEncoder() {
-    _actuatorEncoder.setPosition(0);
-  }
-
   /**
    * Returns true if the actuator is at the last desired state.
    */

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -312,6 +312,7 @@ public Rotation2d getHeadingRaw() {
    */
   public double speakerDistance() {
     Pose3d speakerPose = UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_POSE_RED : FieldConstants.SPEAKER_POSE_BLUE;
+    // Pose3d speakerPose = UtilFuncs.GetSpeakerPose();
 
     Translation2d speakerTranslation = new Translation2d(speakerPose.getX(), speakerPose.getY());
     Translation2d botTranslation = getPose().getTranslation();
@@ -331,6 +332,7 @@ public double[] speakerSetpoints() {
     double ySpeakerAngle;
 
     Pose3d speakerPose = UtilFuncs.GetAlliance() == Alliance.Red ? FieldConstants.SPEAKER_POSE_RED : FieldConstants.SPEAKER_POSE_BLUE;
+    // Pose3d speakerPose = UtilFuncs.GetSpeakerPose();
 
     Translation2d speakerTranslation = new Translation2d(speakerPose.getX(), speakerPose.getY());
     Translation2d botTranslation = getPose().getTranslation();

src/main/java/frc/robot/utils/UtilFuncs.java

@@ -3,6 +3,13 @@
 
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.FieldConstants;
@@ -11,6 +18,26 @@
 /** Any utility functions are here. */
 public final class UtilFuncs {
   private static DoubleSupplier _distanceSupplier;
+  private static AprilTagFieldLayout _field;
+
+  public static void LoadField() {
+    _field = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo);
+  }
+
+  public static Pose3d GetSpeakerPose() {
+    Pose3d pose;
+
+    if (GetAlliance() == Alliance.Red) {
+      pose =  _field.getTagPose(FieldConstants.SPEAKER_TAG_RED).get();
+    }
+
+    pose = _field.getTagPose(FieldConstants.SPEAKER_TAG_BLUE).get();
+
+    return new Pose3d(
+      new Translation3d(pose.getX(), pose.getY(), pose.getZ() + FieldConstants.SPEAKER_TAG_OFFSET),
+      new Rotation3d()
+    );
+  }
 
   /**
    * Supply the ShootFast function with a distance supplier.