Radians normalize issue

[edibucura]

There is a known bug going around the FTC scene at the moment related to the normalization of radians in the SDK. This was noticed because the Gobilda Pinpoint driver uses the built-in Pose2D to save the pose of the robot. When ESD happens the bytes that are being read might suffer from interference, sometimes making the heading close to infinite. At the moment the angle normalize function uses a while loop which substracts 2PI continuously. When a very big number is fed into that function the opmode is stuck in that loop for some seconds. org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.normalizeRadians is the function with the issue

[alan412]

Thanks for mentioning this here. I have logged it as an issue for it to be fixed in the future. (Probably not until the off-season release). In the meantime, you can fix it by putting this in your code and calling instead of normalizeRadians

     double myNormalizeRadians(double radians ){
        if(Math.abs(radians) > 4 * Math.PI){
            double numRevolutions = Math.floor(Math.abs(radians) / (2*Math.PI));
            radians = Math.signum(radians) * (Math.abs(radians) - (numRevolutions * (2*Math.PI)));
        }
        return AngleUnit.normalizeRadians(radians);
  }

[edibucura]

Thanks for mentioning this here. I have logged it as an issue for it to be fixed in the future. (Probably not until the off-season release). In the meantime, you can fix it by putting this in your code and calling instead of normalizeRadians

 double myNormalizeRadians(double radians ){
    if(Math.abs(radians) > 4 * Math.PI){
        double numRevolutions = Math.floor(Math.abs(radians) / (2*Math.PI));
        radians = Math.signum(radians) * (Math.abs(radians) - (numRevolutions * (2*Math.PI)));
    }
    return AngleUnit.normalizeRadians(radians);

}

This can't really be used with how gobilda's driver works already because the call is not direct, but indirect thru pose2d

thread id=222 tid=6026 name="OpModeThread"
    at org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.normalizeRadians(AngleUnit.java:219)
    at org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.normalize(AngleUnit.java:190)
    at org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.fromRadians(AngleUnit.java:102)
    at org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.fromUnit(AngleUnit.java:122)
    at org.firstinspires.ftc.robotcore.external.navigation.Pose2D.getHeading(Pose2D.java:57)
    at com.pedropathing.localization.localizers.PinpointLocalizer.getPoseEstimate(PinpointLocalizer.java:257)

Anyways the main issue would be on the Gobilda's side because it can end up with an almost infinite heading, and that should probably be clamped before or just return an error state / use previous heading data

[alan412]

In your version of pedropathing, you could put the call to your normalizeRadians here: https://github.com/Pedro-Pathing/PedroPathing/blob/c1c4aa426f96901ad7c63a13e0455bc1b532866f/src/main/java/com/pedropathing/localization/localizers/PinpointLocalizer.java#L179

You could also modify your copy of the GoBilda pinpoint driver. (Obviously do one or the other, but not both.)

Hope this helps!!

--Alan

[edibucura]

In your version of pedropathing, you could put the call to your normalizeRadians here: https://github.com/Pedro-Pathing/PedroPathing/blob/c1c4aa426f96901ad7c63a13e0455bc1b532866f/src/main/java/com/pedropathing/localization/localizers/PinpointLocalizer.java#L179

You could also modify your copy of the GoBilda pinpoint driver. (Obviously do one or the other, but not both.)

Hope this helps!!

--Alan

In pedro beta testing we already have a driver with our own Pose2D so it won't happen anymore, I made this issue mostly because I didn't see it reported on the github already while a high amount of people were complaining about this Thanks for the info this could help other teams who do not use Pedro fix this issue for themselves

[qwertychouskie]

Is this fixed in 10.3? I didn't see it in the changelog.

[alan412]

Yes, it was fixed in 10.3. I apologize for it not being in the changelog.

[alan412]

For those that would like more detail, there were 2 things going on here.

1. GoBilda Pinpoint was returning garbage during ESD which caused a huge number to be sent to normalizeRadians. The driver that is in the SDK checks for that and if the data isn't in a reasonable range then it throws it away. (This doesn't guarantee that the data is accurate, but makes it more likely.)

2. The library methods for normalizeRadians and normalizeDegrees used a simple while loop (because it never expected huge positive or negative numbers) which took longer the farther it was out of range. This has been changed by calculating if it is more than 2 rotations and if it is to calculate the number of rotations and remove them instead of just using the while loop. This does nothing with the accuracy of the data, but does reduce the amount of time it takes to normalize it. This change was made for both normalizeRadians and normalizeDegrees.