def test_move_forward(self): a_star = mock.MagicMock(spec=AStar) a_star.read_encoders.return_value = 0, 0 odom = Odometer(Encoders(a_star), start_tracking_thread=False) m = Motors(a_star, odom) m.move_forward(1, 1) odom.update(time.time()) a_star.read_encoders.assert_called() a_star.motors.assert_called()
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_rotate_done(self): a_star = mock.MagicMock(spec=AStar) a_star.read_encoders.return_value = 0, 0 odom = Odometer(Encoders(a_star), start_tracking_thread=False) m = Motors(a_star, odom) m.rotate(0, 1) odom.update(time.time()) # Motors should not move self.assertFalse( [c for c in a_star.motors.call_args_list if c != mock.call(0, 0)])
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)
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_acceleration(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.set_speed_target(1, 1) self.update_odometry(0.5, 0.020, odom, time_provider) left, right = fake_star.cmd self.assertLess(abs(Motor.MAX_CMD - left), 10) self.assertLess(abs(Motor.MAX_CMD - right), 10)
def test_callback(self): encoder = mock.MagicMock(spec=Encoders) encoder.read_encoders.return_value = 0, 0 odom = Odometer(encoder) callback = mock.MagicMock() odom.add_measurement_callback(callback.call_back) odom.track_odometry() time.sleep(0.1) callback.call_back.assert_called()
def test_monitoring_thread_stops(self): encoder = mock.MagicMock(spec=Encoders) encoder.read_encoders.return_value = 0, 0 odom = Odometer(encoder) odom.track_odometry() odom.stop_tracking() time.sleep(0.3) time.sleep(1) encoder.reset_mock() encoder.read_encoders.assert_not_called()
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)