Aligning while driwing

[kuzeyatay]

Hi. i have been trying to figure out how to auto align to the speaker while the driver is still moving the robot. basically i am trying to change the rotarion of the robot so that its perpendicular to the april tag at any time. i have had no luck with this. does anyone have an example code or can help me figure it out?

[amb]

What type of vision are you running?

If you’re using Limelight, get the ‘tx’ value from the Limelight data to set your ChassisSpeeds omega, sort of like a P-loop

[kuzeyatay]

we are using photonvision. Also we only want ids 4 and 7 to be aligned to (depending on the alliance) any idea how we might do that. im kind of a photonvision beginner

[amb]

Not very familiar with PhotonVision, but I found a documentation page:

Getting Target Data

It’s a good idea to read it to know what it’s capable of. My guess on how to do it is: iterate over the list from getTargets(), check each target’s getFiducialID(), and if it’s 4 or 7, use the target’s getYaw() to run a P-loop.

[kuzeyatay]

another question, what do you mean by a p-loop? just a kp loop?

[LSao]

Rembrandts has a very good post on using coordinates to do this: FRC 4481 Team Rembrandts | 2024 Build Thread | Open Alliance - #373 by Jochem. Essentially, if you know where your robot is and you have and an x and y coordinate that you want to aim to, you can do some simple translation math. I would recommend combining photonvision with odometry to get a pose estimate of your bot location. Have you looked into SwerveDrivePoseEstimator at all? I recommend reading this: Pose Estimators — FIRST Robotics Competition documentation (wpilib.org). Also, take a look at the photonvision docs on how to use AprilTags for localization: 3D Tracking - PhotonVision Docs.

Here is a code snippet of how our team handled aligning to the speaker. We used PID to control the angular speed and let x and y speeds still be controlled by the driver.

public Command alignWhileDrivingCommand(Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Translation2d> target) {
    PIDController pid = new PIDController(0.01, 0, 0);
    pid.setTolerance(1.5);
    pid.enableContinuousInput(-180, 180);
    return new DeferredCommand(() ->
      new RepeatCommand(
        new FunctionalCommand(
          () -> {
            // Init
          },
          () -> {
            Translation2d currentTranslation = this.getPose().getTranslation();
            Translation2d targetVector = currentTranslation.minus(target.get());
            Rotation2d targetAngle = targetVector.getAngle();
            double newSpeed;
            if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red)
              newSpeed = pid.calculate(this.getGyroYawDegrees() + 180, targetAngle.getDegrees());
            else
              newSpeed = pid.calculate(this.getGyroYawDegrees() + 180, targetAngle.getDegrees());
            this.drive(xSpeed.get(),ySpeed.get(),
            newSpeed, true, true);
            
              },
              interrupted -> {
                pid.close();
                this.drive(0.0,0.0,0.0,true,true);
              },
              () -> {
                return pid.atSetpoint();
              },
              this)), Set.of(this)
    );
  }

For reference, here is our github repository: FRC1884/season2024 at testing-1884 (github.com).
I highly recommend taking a look at our PoseEstimator.java and Vision.java classes.

[kuzeyatay]

We do have a photonvision pose estimation code that works

[LSao]

Perfect, that should make things even easier for you.