Add held cone X offset for scoring

src/main/java/frc/robot/managers/vision/AutoScoreManager.java

@@ -25,11 +25,12 @@ public class AutoScoreManager extends LifecycleSubsystem {
   private final SwerveSubsystem swerve;
   private final SuperstructureManager superstructure;
 
-  private double xP = -0.15;
-  private double yP = 0.3;
+  private static final double xP = -0.15;
+  private static final double yP = 0.3;
+  private double xOffset = 0;
   // tune setpoint value
-  private double ySetpoint = 5.5;
-  private double angleRange = .5;
+  private static final double ySetpoint = 5.5;
+  private static final double angleRange = .5;
 
   public AutoScoreManager(
       LimelightSubsystem limelight, SwerveSubsystem swerve, SuperstructureManager superstructure) {
@@ -46,19 +47,18 @@ public Command getAutoScoreMidCone() {
     // TODO(Simon): Finish command.
     // Do we drive to position, raise up, then score? Do we raise up, drive to position, then raise
     // up?
-    return limelight
-        .setPipelineCommand(limelight.retroPipeline)
+    return storeConeOffsetCommand()
+        .andThen(limelight.setPipelineCommand(limelight.retroPipeline))
         .andThen(alignWithVisionTargetCommand().until(() -> atLocation()))
         .andThen(
             Commands.runOnce(
                 () -> {
-                  swerve.setChassisSpeeds(new ChassisSpeeds(0, 0, 0), false);
+                  swerve.setChassisSpeeds(new ChassisSpeeds(), false);
                 }))
         .andThen(superstructure.getScoreCommand(NodeHeight.MID, 0.15))
         .andThen(Commands.runOnce(() -> superstructure.set(States.STOWED)))
         .finallyDo(
             (boolean interrupted) -> {
-
               // Set drive speeds to 0.
               swerve.setChassisSpeeds(new ChassisSpeeds(), false);
               swerve.disableSnapToAngle();
@@ -68,6 +68,15 @@ public Command getAutoScoreMidCone() {
             });
   }
 
+  private Command storeConeOffsetCommand() {
+    return limelight
+        .setPipelineCommand(LimelightSubsystem.HELD_CONE_PIPELINE)
+        // Short delay to ensure Limelight has the cone offset ready
+        .andThen(Commands.waitSeconds(0.1))
+        // Store cone offset for later
+        .andThen(Commands.runOnce(() -> xOffset = limelight.getHeldConeXOffset(), limelight));
+  }
+
   private CommandBase alignWithVisionTargetCommand() {
     return Commands.run(
         () -> {
@@ -93,15 +102,17 @@ private ChassisSpeeds calculateSwerveSpeeds() {
     // Get closest middle cone target.
     VisionTarget closestNode = limelight.getClosestMiddleConeTarget();
     // Calculate X and Y speeds
-    double ySpeed = closestNode.x * xP;
-    double xSpeed = (closestNode.y - ySetpoint) * yP;
+    double sidewaysSpeed = (closestNode.x + xOffset) * xP;
+    double forwardSpeed = (closestNode.y - ySetpoint) * yP;
     Logger.getInstance().recordOutput("Vision/LimelightX", closestNode.x);
+    Logger.getInstance().recordOutput("Vision/LimelightXOffset", xOffset);
+    Logger.getInstance().recordOutput("Vision/LimelightXWithOffset", closestNode.x + xOffset);
     Logger.getInstance().recordOutput("Vision/LimelightY", closestNode.y);
 
     if (closestNode.valid) {
-      return new ChassisSpeeds(xSpeed, ySpeed, 0);
+      return new ChassisSpeeds(forwardSpeed, sidewaysSpeed, 0);
     } else {
-      return new ChassisSpeeds(0, 0, 0);
+      return new ChassisSpeeds();
     }
   }
 

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

@@ -5,6 +5,7 @@
 package frc.robot.vision;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
@@ -16,7 +17,11 @@
 public class LimelightSubsystem extends LifecycleSubsystem {
   public final String limelightName;
   public final int retroPipeline = 1;
-  private SwerveSubsystem swerve;
+  // TODO: Change this to the correct pipeline index
+  public static final int HELD_CONE_PIPELINE = 999;
+  // TODO: Tune this value to convert held cone angle (relative to Limelight) to robot angle
+  public static final double HELD_CONE_X_OFFSET_ANGLE_SCALAR = 1;
+  private final SwerveSubsystem swerve;
 
   public LimelightSubsystem(String limelightName, SwerveSubsystem swerve) {
     super(SubsystemPriority.VISION, "LimelightSubsystem_" + limelightName);
@@ -41,6 +46,23 @@ public void setPipeline(int index) {
     LimelightHelpers.setPipelineIndex(limelightName, index);
   }
 
+  /** Get the X offset for scoring a held cone. */
+  public double getHeldConeXOffset() {
+    setPipeline(HELD_CONE_PIPELINE);
+
+    LimelightResults results = LimelightHelpers.getLatestResults(limelightName);
+
+    if (results.targetingResults.targets_Retro.length == 0) {
+      return 0;
+    }
+
+    LimelightTarget_Retro rawTarget = results.targetingResults.targets_Retro[0];
+
+    VisionTarget visionTarget = new VisionTarget(rawTarget, true);
+
+    return visionTarget.x * HELD_CONE_X_OFFSET_ANGLE_SCALAR;
+  }
+
   public VisionTarget getClosestMiddleConeTarget() {
     /* Return the closest middle cone target, as detected using retroreflective tape.
      *
@@ -105,16 +127,21 @@ public VisionTarget getClosestGroundCone() {
 
   public CommandBase setPipelineCommand(int pipeline) {
     return runOnce(
-        () -> {
-          // TODO(Simon): Set proper limelight pipeline and turn on LEDs.
-          setPipeline(pipeline);
-
-          if (pipeline == retroPipeline) {
-            turnOnLights();
-          } else {
-            turnOffLights();
-          }
-        });
+            () -> {
+              // TODO(Simon): Set proper limelight pipeline and turn on LEDs.
+              setPipeline(pipeline);
+
+              if (pipeline == retroPipeline) {
+                turnOnLights();
+              } else {
+                turnOffLights();
+              }
+            })
+        // Wait until Limelight has acknowleged the pipeline change
+        .andThen(
+            Commands.waitUntil(
+                () -> LimelightHelpers.getCurrentPipelineIndex(limelightName) == pipeline))
+        .withTimeout(0.25);
   }
 
   @Override

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

@@ -4,6 +4,8 @@
 
 package frc.robot.vision;
 
+import frc.robot.vision.LimelightHelpers.LimelightTarget_Retro;
+
 public class VisionTarget {
   public double x;
   public double y;
@@ -18,4 +20,8 @@ public VisionTarget(double x, double y, double width, double height, boolean val
     this.height = height;
     this.valid = valid;
   }
+
+  public VisionTarget(LimelightTarget_Retro retroTarget, boolean valid) {
+    this(retroTarget.tx, retroTarget.ty, 0, 0, valid);
+  }
 }