Barge tuning

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

@@ -806,7 +806,7 @@ public Robot() {
                     AutoAim.approachAlgae(
                         swerve,
                         () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()),
-                        0.75)),
+                        1)),
                 Commands.waitUntil(
                         new Trigger(
                                 () ->

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

@@ -51,6 +51,7 @@ public class ExtensionKinematics {
   public static final Pose2d L3_POSE = solveFK(L3_EXTENSION);
   public static final Pose2d L4_POSE =
       new Pose2d(new Translation2d(0.23, 2.05), Rotation2d.fromDegrees(110.0));
+
   public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE);
 
   public static final ExtensionState LOW_ALGAE_EXTENSION =

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

@@ -776,10 +776,11 @@ private void configureStateTransitionCommands() {
         .whileTrue(
             Commands.parallel(
                 elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION),
-                shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS),
+                // Make it initially extend to the full 90 degrees
+                shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS),
                 wrist.setSlowTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS)))
         .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SHOOT_NET_POS))
-        .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS))
+        .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS))
         .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION))
         .and(scoreReq)
         .onTrue(forceState(SuperState.SCORE_ALGAE_NET));

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -56,7 +56,9 @@ public class ElevatorSubsystem extends SubsystemBase {
   public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0);
   public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0);
 
-  public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5);
+  // 1 inch high (maybe not needed?)
+  public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(62.5);
+
   public static final double ALGAE_PROCESSOR_EXTENSION = 0.01;
 
   public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0);

src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java

@@ -39,6 +39,7 @@ public class WristSubsystem extends SubsystemBase {
   public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0);
   public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS =
       Rotation2d.fromDegrees(-20.0);
+
   public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(105);
   public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0);
 

src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.util.Units;
 import frc.robot.Robot;
 import frc.robot.Robot.AlgaeIntakeTarget;
 import java.util.Arrays;
@@ -55,7 +56,8 @@ public static Pose2d getRobotTargetLocation(Pose2d original) {
   }
 
   public static Pose2d getOffsetLocation(Pose2d original) {
-    return original.transformBy(new Transform2d((-0.3), 0, Rotation2d.kZero));
+    return original.transformBy(
+        new Transform2d((-0.3 - Units.inchesToMeters(6)), 0, Rotation2d.kZero));
   }
 
   /** Gets the closest offset target to the given pose. */

src/main/java/frc/robot/utils/autoaim/AutoAim.java

@@ -32,7 +32,7 @@ public class AutoAim {
       new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501));
   public static final Translation2d RED_REEF_CENTER = ChoreoAllianceFlipUtil.flip(BLUE_REEF_CENTER);
 
-  public static double BLUE_NET_X = 8.08;
+  public static double BLUE_NET_X = 8.08 + Units.inchesToMeters(4);
   public static double RED_NET_X = ChoreoAllianceFlipUtil.flipX(BLUE_NET_X);
 
   public static Pose2d BLUE_PROCESSOR_POS = new Pose2d(5.973, 0, Rotation2d.fromDegrees(270));