def test_yaw(self):
encoder = mock.MagicMock(spec=Encoders)
encoder.read_encoders.return_value = 0, 0
odom = Odometer(encoder)
odom.track_odometry()
time.sleep(0.1)
half_circle = WHEEL_DISTANCE_MM * math.pi / 2
encoder.read_encoders.return_value = (half_circle /
DISTANCE_PER_TICK_MM,
-half_circle /
DISTANCE_PER_TICK_MM)
time.sleep(0.1)
yaw = odom.get_situation().yaw
self.assertEqual(math.pi, yaw)
quarter_circle = half_circle / 2
encoder.read_encoders.return_value = (quarter_circle /
DISTANCE_PER_TICK_MM,
-quarter_circle /
DISTANCE_PER_TICK_MM)
time.sleep(0.1)
yaw = odom.get_situation().yaw
self.assertEqual(math.pi / 2, yaw)
def test_rotate(self):
time_provider = TimeProvider(increment=0)
fake_star = FakeAStar(time_provider)
odom = Odometer(Encoders(fake_star), start_tracking_thread=False)
m = Motors(fake_star, odom)
m.rotate(math.pi, 1)
self.update_odometry(2, 0.020, odom, time_provider)
self.assertLess(abs(math.pi - odom.get_situation().yaw), 0.1)
self.assertLess(abs(odom.get_situation().dist), 0.1)
def test_square(self):
time_provider = TimeProvider(increment=0)
fake_star = FakeAStar(time_provider)
odom = Odometer(Encoders(fake_star), start_tracking_thread=False)
m = Motors(fake_star, odom)
m.move_forward(1, 1)
self.update_odometry(2, 0.020, odom, time_provider)
self.assertLess(abs(odom.get_situation().dist - 1), 0.1)
m.rotate(math.pi / 2, 1)
self.update_odometry(4, 0.020, odom, time_provider)
self.assertLess(
abs(self.get_delta(odom.get_situation().yaw, math.pi / 2)), 0.1)
self.assertLess(
abs(self.get_delta(odom.get_situation().yaw, math.pi / 2)), 0.5)
m.move_forward(1, 1)
self.update_odometry(3, 0.020, odom, time_provider)
self.assertLess(abs(odom.get_situation().dist - 2), 0.15)
m.rotate(math.pi / 2, 1)
self.update_odometry(3, 0.020, odom, time_provider)
self.assertLess(abs(self.get_delta(odom.get_situation().yaw, math.pi)),
0.1)
m.move_forward(1, 1)
self.update_odometry(2, 0.020, odom, time_provider)
self.assertLess(abs(odom.get_situation().dist - 3), 0.15)
m.rotate(math.pi / 2, 1)
self.update_odometry(2, 0.020, odom, time_provider)
self.assertLess(
abs(self.get_delta(odom.get_situation().yaw, 3 * math.pi / 2)),
0.3)
m.move_forward(1, 1)
self.update_odometry(2, 0.020, odom, time_provider)
self.assertLess(abs(odom.get_situation().dist - 4), 0.2)
m.rotate(math.pi / 2, 1)
self.update_odometry(3, 0.020, odom, time_provider)
distance_from_zero = math.sqrt(odom.get_situation().x**2 +
odom.get_situation().y**2)
self.assertLess(distance_from_zero, 0.2)
self.assertLess(abs(self.get_delta(odom.get_situation().yaw, 0)), 0.1)
def test_distance(self):
encoder = mock.MagicMock(spec=Encoders)
encoder.read_encoders.return_value = 0, 0
odom = Odometer(encoder)
odom.track_odometry()
time.sleep(0.1)
encoder.read_encoders.return_value = 1000, 1000
time.sleep(0.1)
distance = odom.get_situation().dist
self.assertEqual(distance, DISTANCE_PER_TICK_MM)
def test_forward(self):
time_provider = TimeProvider(increment=0)
fake_star = FakeAStar(time_provider)
odom = Odometer(Encoders(fake_star), start_tracking_thread=False)
m = Motors(fake_star, odom)
m.move_forward(1, 1)
self.update_odometry(2, 0.020, odom, time_provider)
situation = odom.get_situation()
self.assertLess(abs(1 - situation.dist), 0.1)
self.assertLess(abs(self.get_delta(situation.yaw, 0)), 0.1)