BroncBotz3481/YAGSL

Heading correction appears to be broken when testing with NavX2 gyro

Closed this issue · 4 comments

Blargleflakes commented

Hello,

I am a FRC software mentor trying to make use of the YAGSL template to operate a swerve drive and I am having issues with getting heading correction to work correctly with the current version of the template. I am currently using a NavX2 gyro and have observed the following results:

  • Using the heading correction with the gyro non-inverted seemed to cause it the drive to drift off its heading worse than without it enabled.
  • Inverting the gyro with heading correction seems to fix this to an extent, but breaks field oriented drive.

Out of curiosity, I have swapped from using YAGSL as vendor dependency to just using it as a folder in my robot project template so I can test and debug changes. Based on testing so far this block of code stood out to me.

YAGSL/swervelib/SwerveDrive.java

Lines 449 to 466 in bd949c2

if (headingCorrection)
{
if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND
&& (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
{
if (!correctionEnabled)
{
lastHeadingRadians = getOdometryHeading().getRadians();
correctionEnabled = true;
}
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians());
} else
{
correctionEnabled = false;
}
}

I noticed that the lastHeadingRadians value is being used as the current measurement for the rather than heading of the current pose for the heading controller. Is there a reason for this? Swapping the arguments applied to this controller seems to fix heading correction for my swerve configuration.

Also I noticed a use of a correctionEnabled flag, which appears to be used as a toggle to continuously update the lastRadianHeadings variable. Is this variable actually needed? I was able to replace the above code block with the following and it seemed to allow heading correction to work correctly:

    if (headingCorrection)
    {
      if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND
          && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
              || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
      {
        velocity.omegaRadiansPerSecond =
            swerveController.headingCalculate(getPose().getRotation().getRadians(), lastHeadingRadians);
      }
      else
      {
        lastHeadingRadians = getPose().getRotation().getRadians();
      }
    }

I am not sure what is policy regarding making updates and/or contributions to this repository or if this something already being worked on, so I am merely just bringing this issue up with a possible solution with hopes of an official fix or clarification on how to use this template in the future.

thenetworkgrinch commented

There is an outstanding PR for this right now that is being investigated and I hope to be fixed within the next day or 2.

thenetworkgrinch commented

I just read the rest of your issue, if you make a PR to the dev branch it will get merged. The lastHeadingRadians is supposed to hold the last heading that heading correction will aim to rotate to.

thenetworkgrinch commented

Fixed with merge, requires PID tuning of heading PID