diff --git a/robot/conveyor.py b/robot/conveyor.py index 5b20564..9260841 100644 --- a/robot/conveyor.py +++ b/robot/conveyor.py @@ -1,6 +1,7 @@ import pybullet as p import time import threading +from logger import logger class ConveyorBelt: @@ -8,14 +9,18 @@ class ConveyorBelt: self.conveyor_size = size self.conveyor_position = position self.conveyor_id = self.create_conveyor(size, position) + self.conveyor_stopped = False + self.box_size = box_size self.box_spacing = box_spacing self.box_speed = box_speed self.boxes = [] self.create_boxes() + self.stop_thread = False self.thread = threading.Thread(target=self.run) self.thread.start() + def create_conveyor(self, size, position): collision_shape_id = p.createCollisionShape( @@ -83,24 +88,25 @@ class ConveyorBelt: return box_id def move_boxes(self): - # Двигаем коробки вдоль конвейера - for box_id in self.boxes[:]: - # Получаем текущую позицию коробки - pos, orn = p.getBasePositionAndOrientation(box_id) + conveyor_stopped = False + if not self.conveyor_stopped: + for box_id in self.boxes[:]: + pos, orn = p.getBasePositionAndOrientation(box_id) + + if ( + pos[0] + self.box_size[0] * 0.5 + > self.conveyor_position[0] + self.conveyor_size[0] * 0.5 + ): + # Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать) + # Удаляем коробку с конвейера + self.boxes.remove(box_id) + self.conveyor_stopped = True + self.call_robot() + else: + # Двигаем коробку вдоль конвейера (по оси X) + new_pos = [pos[0] + self.box_speed, pos[1], pos[2]] + p.resetBasePositionAndOrientation(box_id, new_pos, orn) - # Проверяем, если коробка выходит за пределы конвейера по оси X - # Начальное положение коробки + её длина (по оси X) - if ( - pos[0] + self.box_size[0] * 0.5 - > self.conveyor_position[0] + self.conveyor_size[0] * 0.5 - ): - # Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать) - # Удаляем коробку с конвейера - self.boxes.remove(box_id) - else: - # Двигаем коробку вдоль конвейера (по оси X) - new_pos = [pos[0] + self.box_speed, pos[1], pos[2]] - p.resetBasePositionAndOrientation(box_id, new_pos, orn) # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество if len(self.boxes) < int( @@ -109,6 +115,12 @@ class ConveyorBelt: new_box = self.create_box(self.box_position()) self.boxes.append(new_box) + def call_robot(self): + # Здесь можно вызвать робота для выполнения операции, например: + logger.info("Конвейер остановлен, вызываем робота для работы с коробкой.") + time.sleep(5) + self.conveyor_stopped = False + def run(self): while not self.stop_thread: self.move_boxes()