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