【Beagle Bone开源设计】超声波避障小车

只看该作者 · 2015-2-27 01:09

本帖最后由 fjjjnk1234 于 2015-2-27 11:31 编辑

操作系统用的是Debian，程序用Python编写，主要实现电机驱动，舵机控制和超声波测距模块的控制。

大致思路：通过超声波测距模块不断测量小车距离障碍物的距离，当距离小于20cm时，小车停止前进，超声波测距模块固定在舵机上，此时舵机由中间位置向左转动90度，测量一次距离，再由中间位置向右转动90度，测量一次距离，比较左右两边距离的大小，实现左转、右转或后退。电池用的是玩具车上拆下来的电池

【超声波测距部分】

TRIGGER引脚接P8_12

ECHO引脚接P8_11

程序：

复制

#!/usr/bin/python

import Adafruit_BBIO.GPIO as GPIO

import time

def getDistanceCM():

        GPIO_TRIGGER = 'P8_12';

        GPIO_ECHO = 'P8_11';

        GPIO.setup(GPIO_TRIGGER,GPIO.OUT);

        GPIO.setup(GPIO_ECHO,GPIO.IN);



        GPIO_HIGH = GPIO.HIGH;

        GPIO_LOW = GPIO.LOW;



        GPIO.output(GPIO_TRIGGER, GPIO_LOW);

        time.sleep(0.00001);



        GPIO.output(GPIO_TRIGGER, GPIO_HIGH);

        time.sleep(0.00001);

        GPIO.output(GPIO_TRIGGER, GPIO_LOW);



        while GPIO.input(GPIO_ECHO)==GPIO_LOW:

                pass;

        start = time.time();



        while GPIO.input(GPIO_ECHO)==GPIO_HIGH:

                pass;

        stop = time.time();



        elapsed = stop-start;

        distance = elapsed * 34000;

        distance = distance / 2;

        GPIO.cleanup();

        return distance;



if __name__ == '__main__':

        while True:

                distance = getDistanceCM();

                time.sleep(0.5);

                print 'Distance: %f cm' % distance;

实验结果：

【舵机测试部分】

舵机控制用Python库的PWM实现，控制起来也挺简单的。

舵机的基准信号都是周期为20ms，宽度为1.5ms。这个基准信号定义的位置为中间位置。舵机有最大转动角度，中间位置的定义就是从这个位置到最大角度与最小角度的量完全一样。

Python库中的PWM函数，一条语句就能控制舵机

PWM.start(channel,duty, freq=2000, polarity=0)

程序：

舵机信号引脚接P8_13

复制

#!/usr/bin/python

import Adafruit_BBIO.PWM as PWM

import time



def servo_middle():

        PWM.start("P8_13", 7.5, 50, 0)



def servo_left():

        PWM.start("P8_13", 10, 50, 0)



def servo_right():

        PWM.start("P8_13", 5, 50, 0)



while True:

        servo_left()

        time.sleep(1)

        servo_right()

        time.sleep(1)

        servo_middle()

        time.sleep(1)

实现舵机左转、右转和处于中间位置

【电机驱动部分】

主要通过1个L298N模块驱动4个减速电机，左边两个电机接一起，接L298N模块的OUT3、OUT4，右边两个电机接一起，接L298N模块的OUT1、OUT2。

IN1接P8_7

IN2接P8_8

IN3接P9_11

IN4接P9_12

程序：

复制

#!/usr/bin/python

import Adafruit_BBIO.GPIO as GPIO

import time



RIGHT_IN1 = 'P8_7'

RIGHT_IN2 = 'P8_8'

LEFT_IN3 = 'P9_11'

LEFT_IN4 = 'P9_12'



GPIO.setup(RIGHT_IN1, GPIO.OUT)

GPIO.setup(RIGHT_IN2, GPIO.OUT)

GPIO.setup(LEFT_IN3, GPIO.OUT)

GPIO.setup(LEFT_IN4, GPIO.OUT)



def front():

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, True)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, True)



def back():

        GPIO.output(RIGHT_IN1, True)

        GPIO.output(RIGHT_IN2, False)

        GPIO.output(LEFT_IN3, True)

        GPIO.output(LEFT_IN4, False)



def stop():

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, False)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, False)



def left():

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, True)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, False)



def right():

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, False)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, True)



if __name__ == '__main__':

    while True:

        front()

        time.sleep(5)

        back()

        time.sleep(5)

        left()

        time.sleep(5)

        right()

        time.sleep(5)

        stop()

        time.sleep(5)

实现小车前进、后退、左转弯、右转弯和停止

【综合测试】

程序仍然存在BUG，刚开始正常运行，一段时间后感觉程序就乱了，有待进一步解决，还请各位朋友指点一下。:L:L:L

程序：

复制

#!/usr/bin/python

import Adafruit_BBIO.GPIO as GPIO

import Adafruit_BBIO.PWM as PWM

import time



def front():

        RIGHT_IN1 = 'P8_7'

        RIGHT_IN2 = 'P8_8'

        LEFT_IN3 = 'P9_11'

        LEFT_IN4 = 'P9_12'

        GPIO.setup(RIGHT_IN1, GPIO.OUT)

        GPIO.setup(RIGHT_IN2, GPIO.OUT)

        GPIO.setup(LEFT_IN3, GPIO.OUT)

        GPIO.setup(LEFT_IN4, GPIO.OUT)

        print 'front'

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, True)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, True)



def back():

        RIGHT_IN1 = 'P8_7'

        RIGHT_IN2 = 'P8_8'

        LEFT_IN3 = 'P9_11'

        LEFT_IN4 = 'P9_12'

        GPIO.setup(RIGHT_IN1, GPIO.OUT)

        GPIO.setup(RIGHT_IN2, GPIO.OUT)

        GPIO.setup(LEFT_IN3, GPIO.OUT)

        GPIO.setup(LEFT_IN4, GPIO.OUT)

        print 'back'

        GPIO.output(RIGHT_IN1, True)

        GPIO.output(RIGHT_IN2, False)

        GPIO.output(LEFT_IN3, True)

        GPIO.output(LEFT_IN4, False)



def stop():

        RIGHT_IN1 = 'P8_7'

        RIGHT_IN2 = 'P8_8'

        LEFT_IN3 = 'P9_11'

        LEFT_IN4 = 'P9_12'

        GPIO.setup(RIGHT_IN1, GPIO.OUT)

        GPIO.setup(RIGHT_IN2, GPIO.OUT)

        GPIO.setup(LEFT_IN3, GPIO.OUT)

        GPIO.setup(LEFT_IN4, GPIO.OUT)

        print 'stop'

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, False)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, False)



def left():

        RIGHT_IN1 = 'P8_7'

        RIGHT_IN2 = 'P8_8'

        LEFT_IN3 = 'P9_11'

        LEFT_IN4 = 'P9_12'

        GPIO.setup(RIGHT_IN1, GPIO.OUT)

        GPIO.setup(RIGHT_IN2, GPIO.OUT)

        GPIO.setup(LEFT_IN3, GPIO.OUT)

        GPIO.setup(LEFT_IN4, GPIO.OUT)

        print 'left'

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, True)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, True)



def right():

        RIGHT_IN1 = 'P8_7'

        RIGHT_IN2 = 'P8_8'

        LEFT_IN3 = 'P9_11'

        LEFT_IN4 = 'P9_12'

        GPIO.setup(RIGHT_IN1, GPIO.OUT)

        GPIO.setup(RIGHT_IN2, GPIO.OUT)

        GPIO.setup(LEFT_IN3, GPIO.OUT)

        GPIO.setup(LEFT_IN4, GPIO.OUT)

        print 'right'

        GPIO.output(RIGHT_IN1, False)

        GPIO.output(RIGHT_IN2, False)

        GPIO.output(LEFT_IN3, False)

        GPIO.output(LEFT_IN4, True)



def getDistanceCM():

        GPIO_TRIGGER = 'P8_12';

        GPIO_ECHO = 'P8_11';

        GPIO.setup(GPIO_TRIGGER,GPIO.OUT);

        GPIO.setup(GPIO_ECHO,GPIO.IN);



        GPIO_HIGH = GPIO.HIGH;

        GPIO_LOW = GPIO.LOW;



        GPIO.output(GPIO_TRIGGER, GPIO_LOW);

        time.sleep(0.00001);



        GPIO.output(GPIO_TRIGGER, GPIO_HIGH);

        time.sleep(0.00001);

        GPIO.output(GPIO_TRIGGER, GPIO_LOW);



        while GPIO.input(GPIO_ECHO)==GPIO_LOW:

                pass;

        start = time.time();



        while GPIO.input(GPIO_ECHO)==GPIO_HIGH:

                pass;

        stop = time.time();



        elapsed = stop-start;

        distance = elapsed * 34000;

        distance = distance / 2;

        GPIO.cleanup();

        return distance;



def servo_middle():

        PWM.start("P8_13", 7.5, 50, 0)



def servo_left():

        PWM.start("P8_13", 10, 50, 0)



def servo_right():

        PWM.start("P8_13", 5, 50, 0)



if __name__ == '__main__':

    servo_middle()

    while True:

        distance = getDistanceCM()

        time.sleep(0.5)

        print 'Distance: %f cm' % distance

        if distance > 50:

            front()

            time.sleep(0.5)

            stop()

        elif distance > 20:

            front()

            time.sleep(0.2)

            stop()

        else:

            stop()

            servo_left()

            distance_l = getDistanceCM()

            time.sleep(0.5)

            print 'Distance_l: %f cm' % distance_l



            servo_right()

            distance_r = getDistanceCM()

            time.sleep(0.5)

            print 'Distance_r: %f cm' % distance_r



            servo_middle()



            if distance_l < 15.0 and distance_r < 15.0:

                back()

                time.sleep(0.3)

                stop()

            elif distance_l > distance_r:

                left()

                time.sleep(0.2)

                stop()

            else:

                right()

                time.sleep(0.2)

                stop()

RIGHT_IN1 = 'P8_7'
RIGHT_IN2 = 'P8_8'
LEFT_IN3 = 'P9_11'
LEFT_IN4 = 'P9_12'
GPIO.setup(RIGHT_IN1, GPIO.OUT)
GPIO.setup(RIGHT_IN2, GPIO.OUT)
GPIO.setup(LEFT_IN3, GPIO.OUT)
GPIO.setup(LEFT_IN4, GPIO.OUT)

这几句定义引脚输出的语句，本来是放在程序的开头执行，程序结合起来感觉语句好像“失效”了，电机不转，然后就在front()、back()、left()、right()等函数中都加上了，电机终于能转了，可能是对Python编程不够了解。