예제 #1
0
class Robot:
    # US sensors constants.
    US_SENSORS = [
        {
            'name': 'front_bottom',
            'trigger_limit': 8,
            'sensors': 0
        },
        {
            'name': 'left',
            'trigger_limit': 10,
            'sensors': 2
        },
        {
            'name': 'back',
            'trigger_limit': 8,
            'sensors': 4
        },
        {
            'name': 'right',
            'trigger_limit': 10,
            'sensors': 3
        },
        {
            'name': 'front_top',
            'trigger_limit': 8,
            'sensors': 1
        },
    ]

    # Take in count the obstacle dimension (assuming another robot)
    # + the robot size.
    OBSTACLES_DIMENSION = 50

    def __init__(self):
        """
        position: init position dict with keys:
            - "angle": a X-axis relative angle.
            - "point": the first position.
        """
        #  self._us_sensors = RangeSensor(4)
        self._motors = Motors(5)
        self._kinematic = Kinematics(6)
        self._us = RangeSensor(4)

        logging.info('Building the graph map.')
        #  self._graph = build_graph(robot_diagonal)
        logging.info('Finished to build the graph map.')
        self._blocking_servo = -1

    def reset_kinematics(self):
        self._kinematic.up_clamp()
        time.sleep(0.2)
        self._kinematic.open_clamp()
        time.sleep(0.2)
        self._kinematic.reset_funny()
        time.sleep(0.2)
        self._kinematic.push_back()
        time.sleep(1)

    def wait_motors(self, enable=True, timeout=0):
        first_time = time.time()
        while not self._motors.is_done():
            if enable:
                if self._blocking_servo != -1:
                    i = self._blocking_servo
                    test = self._us.get_range(i)
                    print(test)
                    if test > self.US_SENSORS[i]['trigger_limit']:
                        self._motors.restart()
                        self._blocking_servo = -1
                    else:
                        continue
                us_sensors = self._us.get_ranges()
                print(us_sensors)
                for i, us in enumerate(us_sensors):
                    if 'front' in self.US_SENSORS[i][
                            'name'] or 'back' in self.US_SENSORS[i]['name']:
                        if us < self.US_SENSORS[i]['trigger_limit']:
                            self._motors.stop()
                            self._blocking_servo = i
                            break
            if (time.time() - first_time) > timeout and timeout != 0:
                break
            time.sleep(0.1)

    def take_modules(self, number=1):
        for i in range(number):
            self._kinematic.open_clamp()
            self._kinematic.down_clamp()
            time.sleep(DELAY_UP_DOWN_CLAMP)
            self._motors.forward(9)
            self.wait_motors(enable=False, timeout=2)

            self._kinematic.close_clamp()
            time.sleep(1)
            #  self._motors.set_speed(40)
            self._motors.backward(7)
            if number == 1:
                self._motors.forward(6)
                self.wait_motors(enable=False, timeout=2)
                self._motors.backward(6)
                self.wait_motors(enable=False, timeout=2)
            self.wait_motors(enable=False)
            self._motors.backward(8)
            self.wait_motors(enable=False)

            #SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
            self._kinematic.open_clamp()
            time.sleep(0.5)
            self._motors.forward(3)
            self.wait_motors(enable=False, timeout=2)
            self._kinematic.close_clamp()
            time.sleep(1)
            #---

            self._motors.forward(3)
            self._kinematic.up_clamp()
            self.wait_motors(enable=False, timeout=3)
            time.sleep(0.5)
            self._kinematic.open_clamp()
            time.sleep(1)
        self._kinematic.down_clamp()
        time.sleep(0.5)
        self._kinematic.up_clamp()

    def eject_modules(self, number=1):
        for i in range(number):
            self._kinematic.push_out()
            time.sleep(1.5)
            self._kinematic.push_back()
            time.sleep(1.5)
        self._motors.forward(2)
        self.wait_motors(timeout=2)
        self._motors.backward(8)
        self.wait_motors(timeout=2)

    def get_kin(self):
        return self._kinematic

    def get_motors(self):
        return self._motors

    def finalize(self):
        self._motors.stop()

        self._kinematic.launch_funny()
        time.sleep(0.8)
        self._kinematic.reset_funny()
예제 #2
0
class Robot:
    _DELAY_OPEN_ClOSE_CLAMP = 0.7
    _DELAY_UP_DOWN_CLAMP = 2.5
    _DELAY_IN_OUT_BLOCK = 3

    # US sensors constants.
    US_SENSORS = [
        {
            'name': 'front_bottom',
            'trigger_limit': 10,
            'sensors': [0, 1]
        },
        {
            'name': 'left',
            'trigger_limit': 10,
            'sensors': [2]
        },
        {
            'name': 'back',
            'trigger_limit': 5,
            'sensors': [4]
        },
        {
            'name': 'right',
            'trigger_limit': 10,
            'sensors': [3]
        },
        {
            'name': 'front_top',
            'trigger_limit': 10,
            'sensors': [0, 1]
        },
    ]

    DIMENSION = {'length': 32.5, 'width': 19.2}

    # Take in count the obstacle dimension (assuming another robot)
    # + the robot size.
    OBSTACLES_DIMENSION = 50

    def __init__(self, position):
        """
        position: init position dict with keys:
            - "angle": a X-axis relative angle.
            - "point": the first position.
        """
        self._position = position

        #  self._us_sensors = RangeSensor(4)
        self._motors = Motors(5)
        self._kinematic = Kinematics(6)

        logging.info('Building the graph map.')
        robot_diagonal = math.sqrt(self.DIMENSION['length'] +
                                   self.DIMENSION['width']**2)
        self._graph = build_graph(robot_diagonal)
        logging.info('Finished to build the graph map.')

    def take_modules(self, number=1, distance=0):
        for i in range(number):
            self._kinematic.down_clamp()
            time.sleep(self._DELAY_UP_DOWN_CLAMP)
            self._motors.forward(distance)
            while not self._motors.is_done(self.__done_callback):
                time.sleep(0.5)
            self._kinematic.close_clamp()
            time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            self._motors.backward(distance)
            while not self._motors.is_done(self.__done_callback):
                time.sleep(0.5)

            #SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
            self._kinematic.open_clamp()
            time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            self._motors.forward(2.5)
            while not self._motors.is_done(self.__done_callback):
                time.sleep(0.5)
            self._kinematic.close_clamp()
            time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            #---

            self._kinematic.up_clamp()
            time.sleep(self._DELAY_UP_DOWN_CLAMP)
            self._kinematic.open_clamp()

    def eject_modules(self, number=1):
        for i in range(number):
            self._kinematic.push_out()
            time.sleep(self._DELAY_IN_OUT_BLOCK)
            if number == 4 and i == 2:
                self._kinematic.open_clamp()
                time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
            self._kinematic.push_back()
            time.sleep(self._DELAY_IN_OUT_BLOCK)

    def move_to(self, target):
        """
        target: a dict with keys:
            - "angle": a X-axis relative constrain target angle.
            - "point": position of the target.
        """
        while True:
            instructions = self._graph.get_path(self._position, target)
            status = self._motors.move_with_instructions(
                instructions, self.__move_callback, self.__done_callback)
            if status == 'ok':
                break

    def finalize(self):
        self._motors.stop()

        self._kinematic.launch_funny()
        time.sleep(1)
        self._kinematic.reset_funny()

    def __move_callback(self):
        """
        Return a string giving the state if we need to stop or not the regulation because of
        an obstacle.
        """
        #  ranges = self._us_sensors.get_ranges()
        ranges = [20, 20, 20, 20, 20]

        for us_data in self.US_SENSORS:
            for i, sensor in enumerate(us_data['sensors']):
                if ranges[sensor] == 0:
                    # Zero value means that we are too far from obstacles.
                    continue
                if ranges[sensor] < us_data['trigger_limit']:
                    logging.warn(
                        'Motors stopped becauce of the %s%i US sensors at %i cm',
                        us_data['name'], i, ranges[sensor])
                    self._motors.stop()
                    # TODO: avoid recalculation if we have the near same position.
                    self._graph.reset_obstacles()
                    self._graph.add_obstacle(self._position, self.DIMENSION,
                                             self.OBSTACLES_DIMENSION,
                                             us_data['name'], ranges[sensor])
                    return 'obstacle'
        return 'continue'

    def __done_callback(self, distance_travelled, status='ok'):
        """
        Update the position and angle informations of the robots.
        """
        self._move_target = None

        logging.info('Regulation is done!')
        self.__update_robot_position(distance_travelled)
        print(self._position)

        return status

    def __update_robot_position(self, distance_travelled):
        left = distance_travelled['left']
        right = distance_travelled['right']

        if self.__is_opposite_sign(left, right):
            # We finished a rotation regulation.
            angle = ((right - left) / 2) / Motors.ANGLE_CORRECTION
            self._position['angle'] += angle
            logging.info('Robot turned with an angle of %i', angle)
        else:
            # We finished a lead regulation.
            angle = self._position['angle']
            distance = (left + right) / 2

            self._position['point'][0] += distance * math.cos(
                math.radians(angle))
            self._position['point'][1] += distance * math.sin(
                math.radians(angle))

    def __is_opposite_sign(self, n, m):
        if n > 0 and m > 0:
            return False
        elif n < 0 and m < 0:
            return False
        return True