Beispiel #1
0
class Infrared(object):
    def __init__(self):
        super().__init__()
        self.motor = Motor()
        self.init_sensor()

    def init_sensor(self):
        GPIO.setup(AvoidSensorLeft, GPIO.IN)
        GPIO.setup(AvoidSensorRight, GPIO.IN)

    def detect(self):
        left_sensor = GPIO.input(AvoidSensorLeft)
        right_sensor = GPIO.input(AvoidSensorRight)
        return left_sensor, right_sensor

    def detect_to_turn_car(self):
        """
        遇到障碍物,红外避障模块的指示灯亮,端口电平为LOW
        未遇到障碍物,红外避障模块的指示灯灭,端口电平为HIGH

        Return:
            - Block detected: True
            - Block not detected: False
        """
        left_sensor, right_sensor = self.detect()

        logging.debug(
            f'Infra red detecting ... \nleft: {left_sensor} right: {right_sensor}'
        )
        if left_sensor is True and right_sensor is True:
            return False
        else:
            self.motor.aquire()
            if left_sensor is True and right_sensor is False:
                self.motor.spin_left(force=True)  # 右边探测到有障碍物,有信号返回,原地向左转
                time.sleep(0.01)
            elif right_sensor is True and left_sensor is False:
                self.motor.spin_right(force=True)  # 左边探测到有障碍物,有信号返回,原地向右转
                time.sleep(0.01)
            elif right_sensor is False and left_sensor is False:
                self.motor.spin_right(force=True)  # 当两侧均检测到障碍物时调用固定方向的避障(原地右转)
                time.sleep(0.01)
            self.motor.free()
            self.motor.release()
            return True
Beispiel #2
0
class KeyPilot(object):
    """
    Control smart car with Keyboard input
    """
    def __init__(self):
        self.init_motor()
        self.init_infrared()
        self.init_ultrasonic()
        self.init_headlight()

    def init_motor(self):
        self.motor = Motor()

    def init_infrared(self):
        self.infrared = Infrared()

    def init_headlight(self):
        self.headlight = HeadLight()

    def init_ultrasonic(self):
        self.ultrasonic = Ultrasonic()

    def detect_to_turn(self):
        self.motor.aquire()
        block = self.ultrasonic.detect_to_turn_car()
        self.motor.release()
        return block

    def _on_press(self, key):
        logging.debug(f'Press {key}')
        block = self.detect_to_turn()

        # if self.motor.lock:
        #     pass
        # if block:
        #     pass
        # else:
        if key == Key.up:
            logging.debug('Run forward   ^')
            self.motor.run(0.1)
        elif key == Key.down:
            logging.debug('Run backward  _')
            self.motor.back(0.1)
        elif key == Key.left:
            logging.debug('Run left      <-')
            self.motor.left(0.1)
        elif key == Key.right:
            logging.debug('Run right     ->')
            self.motor.right(0.1)
        elif key == KeyCode(char='q'):
            logging.debug('Spin left      <-')
            self.motor.spin_left(0.1)
        elif key == KeyCode(char='e'):
            logging.debug('Spin right     ->')
            self.motor.spin_right(0.1)
        elif key == Key.space:
            logging.debug('Brake          X')
            self.motor.brake(0.1)
        else:
            time.sleep(0.1)

    def _on_release(self, key):
        logging.debug(f'Press {key}')
        # if self.motor.lock:
        #     pass
        # else:
        self.motor.free()
        time.sleep(0.1)

    def listen(self, *args, **kwargs):
        '''
        Function to control the bot using threading. Define the clientSocket in the module before use
        '''
        try:
            with Listener(on_press=self._on_press,
                          on_release=self._on_release) as listener:
                listener.join()
        except KeyboardInterrupt:
            GpioMgmt().release()
            self.headlight.off()
            logging.info("[+] Exiting")
            # clientSocket.close()
            raise e
        except Exception as e:
            GpioMgmt().release()
            self.headlight.off()
            logging.error(str(e))
            # clientSocket.close()
            raise e

    def run(self):
        try:
            GpioMgmt().init_pwm()
            self.ultrasonic.init_pwm()
            self.detect_to_turn()
            self.listen()
        except Exception as e:
            GpioMgmt().init_pin()
            logging.error(str(e))
Beispiel #3
0
class AutoPilot(object):
    def __init__(self):
        self.init_motor()
        self.init_camera()
        # self.init_infrared()
        # self.init_ultrasonic()
        self.init_headlight()
        self.sens = DotDict({})

    def init_motor(self):
        self.motor = Motor()

    def init_camera(self):
        self.camera = Camera()

    def init_infrared(self):
        self.infrared = Infrared()

    def init_headlight(self):
        self.headlight = HeadLight()

    def init_ultrasonic(self):
        self.ultrasonic = Ultrasonic()

    def _pilot(self, deviation):
        t = 0.1
        if -5 <= deviation <= 5:
            self.motor.run(t, 20, 20)
        elif -10 <= deviation < -5:
            self.motor.left_run(t, left_speed=15, right_speed=20)
        elif -20 <= deviation < -10:
            self.motor.left_run(t, left_speed=5, right_speed=20)
        elif deviation < -20:
            self.motor.left_run(t, left_speed=1, right_speed=20)
        elif 5 < deviation <= 10:
            self.motor.right_run(t, left_speed=20, right_speed=15)
        elif 10 < deviation <= 20:
            self.motor.right_run(t, left_speed=20, right_speed=5)
        elif 20 < deviation:
            self.motor.right_run(t, left_speed=20, right_speed=1)
        self.motor.free()
        #time.sleep(0.1)

    def _run(self):
        debug = 0
        counter = 0
        dev_cache = []
        momentum = 0
        dev_m = 0
        cache_len = 0

        while self.camera.cap.isOpened():
            counter += 1
            try:
                start = time.time()
                deviation = self.camera.analyze()
                # logging.debug(f'deviation: {deviation}')
                # self._pilot(deviation)
                if len(dev_cache) == 0:
                    avg_deviation = deviation
                if abs(deviation - avg_deviation) < 20:
                    dev_cache.append(deviation)

                if len(dev_cache) >= 3:
                    counter = 0
                    end = time.time()
                    delta = end - start
                    logging.debug(f'analyze 3 times: {delta:.3f}s')

                    avg_deviation = sum(dev_cache) / len(dev_cache)
                    dev_m = (0.3 * dev_m + avg_deviation) / 1.3
                    logging.info(f'deviation momentum: {dev_m}')
                    self._pilot(dev_m)
            except KeyboardInterrupt as e:
                counter = 0
                dev_cache = []
                time.sleep(20)
                break

    def run(self):
        try:
            logging.debug('Start smartcar in mode auto_pilot with camera ...')
            GpioMgmt().init_pwm()
            # self.ultrasonic.init_pwm()
            self._run()
        except KeyboardInterrupt as e:
            GpioMgmt().release()
            logging.info("[+] Exiting")
            raise e
        except Exception as e:
            GpioMgmt().init_pin()
            logging.error(str(e))
Beispiel #4
0
class AutoPilot(object):
    def __init__(self):
        self.init_motor()
        self.init_infrared()
        self.init_ultrasonic()
        self.init_headlight()
        self.sens = DotDict({})

    def init_motor(self):
        self.motor = Motor()

    def init_infrared(self):
        self.infrared = Infrared()

    def init_headlight(self):
        self.headlight = HeadLight()

    def init_ultrasonic(self):
        self.ultrasonic = Ultrasonic()

    def infrared_detect(self):
        left_sensor, right_sensor = self.infrared.detect()
        self.sens.infrared = DotDict({
            'left': left_sensor,
            'right': right_sensor
        })

    def ultrasonic_detect(self):
        front_distance, left_distance, right_distance = self.ultrasonic.detect(
        )
        self.sens.distance = DotDict({
            'front': front_distance,
            'left': left_distance,
            'right': right_distance
        })

    def detect(self):
        self.ultrasonic_detect()
        self.infrared_detect()

    def _pilot(self):
        block = True
        self.detect()
        if self.sens.distance.front > 30:
            infrared_block = self.infrared_detect_to_turn_car()
            if not infrared_block:
                block = False
                self._run_when_distance_long()
        elif 20 <= self.sens.distance.front <= 50:
            infrared_block = self.infrared_detect_to_turn_car()
            if not infrared_block:
                self.motor.right(0.2)
        elif self.sens.distance.front < 20:
            self.motor.turn_back()

        self.motor.free()
        return block

    def _run_when_distance_long(self, norm=200):
        run_time = (self.sens.distance.front - 20) / norm
        self.motor.run(run_time)
        logging.debug('Run time   ^  %f' % run_time)

    def infrared_detect_to_turn_car(self):
        infrared_block = True
        if self.sens.infrared.left is True and self.sens.infrared.right is False:
            self.motor.turn_right()
        elif self.sens.infrared.left is False and self.sens.infrared.right is True:
            self.motor.turn_left()
        elif self.sens.infrared.left is False and self.sens.infrared.right is False:
            self.motor.turn_back()
        else:
            infrared_block = False

        return infrared_block

    def _on_press(self, key):
        logging.debug(f'Press {key}')
        if key == Key.space:
            self.motor.brake()
        elif key == Key.esc:
            self._pilot()
        if key == Key.up:
            logging.debug('Run forward   ^')
            self.motor.run(0.1)
        elif key == Key.down:
            logging.debug('Run backward  _')
            self.motor.back(0.1)
        elif key == Key.left:
            logging.debug('Run left      <-')
            self.motor.left(0.1)
        elif key == Key.right:
            logging.debug('Run right     ->')
            self.motor.right(0.1)
        elif key == KeyCode(char='q'):
            logging.debug('Spin left      <-')
            self.motor.spin_left(0.1)
        elif key == KeyCode(char='e'):
            logging.debug('Spin right     ->')
            self.motor.spin_right(0.1)
        else:
            time.sleep(0.1)

    def _on_release(self, key):
        if key != Key.ctrl:
            self.motor.free()
        time.sleep(0.1)

    def listen(self, *args, **kwargs):
        '''
        Function to control the bot using threading. Define the clientSocket in the module before use
        '''
        try:
            with Listener(on_press=self._on_press,
                          on_release=self._on_release) as listener:
                listener.join()
        except KeyboardInterrupt:
            GpioMgmt().release()
            self.headlight.off()
            logging.info("[+] Exiting")
            # clientSocket.close()
            raise e
        except Exception as e:
            GpioMgmt().release()
            self.headlight.off()
            logging.error(str(e))
            # clientSocket.close()
            raise e

    def _run(self):
        self.listen()

    def run(self):
        try:
            logging.debug('Start smartcar in mode auto_pilot with sensors ...')
            GpioMgmt().init_pwm()
            self.ultrasonic.init_pwm()
            self._run()
        except Exception as e:
            GpioMgmt().init_pin()
            logging.error(str(e))
Beispiel #5
0
class Ultrasonic(object):
    def __init__(self):
        super().__init__()
        self.pwm_servo = None
        self.motor = Motor()
        self.init_utltra()
        self.init_servo()
        self.init_infrared()
        self.init_headlight()
        # self.init_pwm()

    def init_utltra(self):
        """
        电机引脚初始化为输出模式
        按键引脚初始化为输入模式
        超声波引脚初始化
        """
        GPIO.setup(EchoPin, GPIO.IN)
        GPIO.setup(TrigPin, GPIO.OUT)

    def init_servo(self):
        """舵机引脚初始化
        """
        GPIO.setup(ServoPin, GPIO.OUT)

    def init_pwm(self):
        """
        设置舵机的频率和起始占空比
        """
        self.pwm_servo = GPIO.PWM(ServoPin, 50)
        self.pwm_servo.start(0)

    def init_infrared(self):
        self.infrared = Infrared()

    def init_headlight(self):
        self.headlight = HeadLight()

    def _detect_distance(self):
        """
        超声波函数
        TrigPin 输入至少10us的高电平信号
        触发超声波模块的测距功能 
        ___|-|____
        模块自动发出8个40kHz的超声波脉冲,并自动检测是否有信号返回
        _____|-|-|-|-|-|-|-|_____
        一旦检测到有回波信号则Echo引脚会输出高电平

        高电平持续时间就是超声波从发射到返回的时间
        """
        # 触发超声波模块测距功能
        GPIO.output(TrigPin, GPIO.HIGH)
        time.sleep(0.000015)
        GPIO.output(TrigPin, GPIO.LOW)
        # 检测回波信号, 一旦检测到有回波信号则Echo引脚会输出高电平
        while not GPIO.input(EchoPin):
            pass
        # start time
        t1 = time.time()
        # 检测高电平持续时间
        while GPIO.input(EchoPin):
            pass
        # end time
        t2 = time.time()
        # S = 高电平时间*声速(340m/s) / 2       (m)
        #                               * 100 (cm)
        distance = ((t2 - t1) * 340 / 2) * 100
        time.sleep(0.01)
        return int(distance)

    def detect_distance(self):
        dist_list = []
        for _ in range(5):
            dist = self._detect_distance()
            if dist >= 500 or dist == 0:
                dist = self._detect_distance()
            dist_list.append(dist)
        distance_list = sorted(dist_list)
        distance = sum(distance_list[0:3]) / 3
        return distance

    def servo_steer(self, pos):
        """
        舵机旋转到指定角度
        """
        # for i in range(18):
        self.pwm_servo.ChangeDutyCycle(2.5 + 10 * pos / 180)

    def detect_right_distance(self):
        #舵机旋转到0度,即右侧,测距
        self.servo_steer(0)
        time.sleep(0.8)
        right_distance = self.detect_distance()
        logging.info("Ultrasonic detecting ... \nRight distance: %d " %
                     right_distance)
        return right_distance

    def detect_left_distance(self):
        #舵机旋转到180度,即左侧,测距
        self.servo_steer(180)
        time.sleep(0.8)
        left_distance = self.detect_distance()
        logging.info("Ultrasonic detecting ... \nLeft distance: %d " %
                     left_distance)
        return left_distance

    def detect_front_distance(self):
        #舵机旋转到90度,即前方,测距
        self.servo_steer(90)
        time.sleep(0.8)
        front_distance = self.detect_distance()
        logging.info("Ultrasonic detecting ... \nFront distance: %d " %
                     front_distance)
        return front_distance

    def servo_detect_to_turn_car(self):
        """
        舵机旋转超声波测距避障,led根据车的状态显示相应的颜色
        """
        self.headlight.green()
        right_distance = self.detect_right_distance()
        left_distance = self.detect_left_distance()
        front_distance = self.detect_front_distance()

        self.motor.aquire()
        if left_distance < 30 and right_distance < 30 and front_distance < 30:
            # 亮品红色,掉头
            self.headlight.magenta()
            self.motor.spin_right(0.1, left_speed=60, right_speed=60)
            time.sleep(0.01)
        elif left_distance >= right_distance:
            # 亮蓝色, 向左转
            self.headlight.yellow()
            self.motor.spin_left(0.1, left_speed=60, right_speed=60)
            time.sleep(0.01)
        elif left_distance <= right_distance:
            # 亮品红色,向右转
            self.headlight.yellow()
            self.motor.spin_right(0.1, left_speed=60, right_speed=60)
            time.sleep(0.01)
        self.motor.release()

    def detect_to_turn_car(self):
        block = True
        front_distance = self.detect_front_distance()

        if front_distance >= 30:
            block = self.infrared.detect_to_turn_car()
        elif front_distance < 30:
            self.servo_detect_to_turn_car()

        self.motor.free()
        time.sleep(0.1)
        return block

    def detect(self):
        left_distance = None
        right_distance = None
        front_distance = self.detect_front_distance()
        if front_distance < 50:
            right_distance = self.detect_right_distance()
            left_distance = self.detect_left_distance()
            # front_distance = self.detect_front_distance()
            self.servo_steer(90)
        time.sleep(0.1)
        return front_distance, left_distance, right_distance