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)