mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-03 03:01:44 +00:00
19 lines
538 B
Python
19 lines
538 B
Python
|
import pytest
|
||
|
|
import math
|
||
|
|
|
||
|
|
from wpimath import DifferentialDriveOdometry, Rotation2d
|
||
|
|
|
||
|
|
|
||
|
|
def test_encoder_distances():
|
||
|
|
odometry = DifferentialDriveOdometry(
|
||
|
|
gyroAngle=Rotation2d.fromDegrees(45), leftDistance=0, rightDistance=0
|
||
|
|
)
|
||
|
|
|
||
|
|
pose = odometry.update(
|
||
|
|
gyroAngle=Rotation2d.fromDegrees(135), rightDistance=0, leftDistance=5 * math.pi
|
||
|
|
)
|
||
|
|
|
||
|
|
assert pose.x == pytest.approx(5.0, abs=1e-9)
|
||
|
|
assert pose.y == pytest.approx(5.0, abs=1e-9)
|
||
|
|
assert pose.rotation().degrees() == pytest.approx(90.0, abs=1e-9)
|