Aligning and distance with limelight

[alecLiu]

We created two commands that work with limelight, one that aligns to an angle and the other aligns to a distance. We’d like to do both of them at the same time. This is our code for aligning to distance:

public GoToDistanceLimelight(double distanceInches, SwerveDriveSubsystem drive, LimelightPortal ll) {
super(
    new ProfiledPIDController(Constants.DISTANCE_PID_P,Constants.DISTANCE_PID_I, Constants.DISTANCE_PID_D, //need to tune this better
        new TrapezoidProfile.Constraints(Constants.MAX_DISTANCE_VELOCITY,Constants.MAX_DISTANCE_ACCELERATION)),
    
    // Close loop on heading
    ll::getDistance,
    // Set reference to target
    distanceInches,  
    // Pipe output to turn branchrobot
    (output,setpoint) -> drive.holonomicDrive(output, 0, 0),
    // Require the drive
    drive);

// Set the controller to be continuous (because it is an angle controller)
//getController().enableContinuousInput(-180, 180);
swerveDriveSubsystem = drive;
limeL = ll;
// Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
// setpoint before it is considered as having reached the reference
getController().setTolerance(Constants.DISTANCE_TOLERANCE, 10);
System.out.println("***********" + distanceInches + "**********");

}
We have something very similar for rotation, any suggestions on how to do both at the same time?

[Max2370]

Consider writing to variables instead of straight to the drive method, then running in parallel with another command to read those variables and drive. Not sure if it would work, but it’s the first thing to come to mind. If it works as it does in my head, you wouldn’t need to require the subsystem in the commands to do the math and set variables.