Rotation problem in the YAGSL simulation

csos · November 22, 2024, 8:35am

Hello, I am trying to try the YAGSL project example and when trying the aimAtSpeaker and driveToPose methods, even if they change position, their rotation does not change, they return to the initial rotation.

EXAMPLE VIDEO

how can I solve this ?

nstrike · November 22, 2024, 2:41pm

To solve this let’s understand how the function is supposed to work.

github.com

BroncBotz3481/YAGSL-Example/blob/main/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java#L203-L215


      
          /**
           * Get the yaw to aim at the speaker.
           *
           * @return {@link Rotation2d} of which you need to achieve.
           */
          public Rotation2d getSpeakerYaw()
          {
            int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4;
            // Taken from PhotonUtils.getYawToPose()
            Pose3d        speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get();
            Translation2d relativeTrl         = speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation();
            return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()).plus(swerveDrive.getOdometryHeading());
          }

We get the Pose2d relative to the AprilTag pose. Thede poses are blue-origin so if you’re on the red side it might not work

csos · November 22, 2024, 7:37pm

hello thank you for the answer. as seen in the video, the robot goes to the correct rotation exactly as I want, but after reaching the target rotation, it returns to the rotation it left before.

nstrike · November 22, 2024, 8:15pm

Oh good! This is bc you have to hold that rotation or make your own drive command to rotate to it and drive at the same time.