[wpimath] Report error when x and y components of Rotation2d are both zero (#6767)

Fixes #6766.
This commit is contained in:
Tyler Veness
2024-07-08 11:23:36 -07:00
committed by GitHub
parent 3d22eeca9d
commit 7f6ba54b68
2 changed files with 13 additions and 0 deletions

View File

@@ -10,6 +10,7 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.proto.Rotation2dProto;
import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
@@ -118,6 +119,8 @@ public class Rotation2d
} else {
m_sin = 0.0;
m_cos = 1.0;
MathSharedStore.reportError(
"x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace());
}
m_value = Math.atan2(m_sin, m_cos);
}