Problem with NavX inversion

[kvanrhein]

I have two robots, both with navX, one is upside down, and works great with yagsl out of the box. The other is right side up and I could not get it to work with invert true or false, so I looked in the code and found that the inversion seemed to be having no effect on getZ() from gyro, so I downloaded the library and changed the code in navXSwerve.java to do the inversion in getRotation3d instead of getRawRotation3d and that seems to work.

The right-side-up navX I set invert to true and now it works with this code change:

public Rotation3d getRotation3d()
{
if (inverted) {
return getRawRotation3d().rotateBy(offset.unaryMinus()).unaryMinus();
}
else {
return getRawRotation3d().rotateBy(offset.unaryMinus());
}
}

Don't know why, maybe I am imagining things...

[thenetworkgrinch]

That makes no sense. Could you send a screencap of it happening and shorten your fix then submit a PR?

[kvanrhein]

I agree, it doesn't make sense, which is why I am asking about it.

I don't know how to "submit a pr" but here is a link to a video I made (sorry, the audio is horrible):
screencapture
and a link to the modified NavXSwerve.java:
NavXSwerve.java

[kvanrhein]

Looking deeper found this:
original raw rotation3d reported by navX: w 0.0102 x -0.0569 y 0.9983 z -0.0057
result of unaryMinus of that angle: w 0.0102 x 0.0569 y -0.9983 z 0.0057

Now, using getZ() function:
final var cycz = 1.0 - 2.0 * (y * y + z * z);
final var cysz = 2.0 * (w * z + x * y);
final var cy_sq = cycz * cycz + cysz * cysz;
if (cy_sq > 1e-20) {
return Math.atan2(cysz, cycz);
} else {
return Math.atan2(2.0 * w * z, w * w - z * z);
}

for both the original and inverted values, this formula yields -3.0276 which is why the yaw is not changing after inversion

[thenetworkgrinch]

That solution would be fine if the invertedYaw function was more of a negate function which negated every parameter

[kvanrhein]

We have 3 robots we will be testing with navX imu's I will run this code on all three and see how it goes...

btw, thanks for all the work on yagsl, we have three teams working together in the Chicago Robotics Alliance trying to get similar code bases based on yagsl, this month is my team's (1781) first use of yagsl and command based programming; yagsl is making it easy for us to move into new terrain.

[kvanrhein]

Oh, just saw your last post. I'll try that then, all three negated.

[kvanrhein]

that works.

[kvanrhein]

private Rotation3d negate(Rotation3d rot) {
    return new Rotation3d(-rot.getX(), -rot.getY(), -rot.getZ());
  }

  /**
   * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
   *
   * @return {@link Rotation3d} from the IMU.
   */
  @Override
  public Rotation3d getRawRotation3d()
  {
    return inverted ? negate(imu.getRotation3d()) : imu.getRotation3d();
  }