树莓派小车避障之——超声波控制（2.1）

不带舵机驱动代码：

有障碍物的话------就右转或者左转避开

#!/usr/bin/env python

import RPi.GPIO as GPIO
import time
import sys
#BCM编码
PWMA   = 18
AIN1   = 22
AIN2   = 27

PWMB   = 23
BIN1   = 25
BIN2   = 24

BtnPin  = 19#这三个引脚不晓得干啥的
Gpin    = 5
Rpin    = 6
#超声波工作引脚
TRIG = 20       ##设置Trig和ECHO俩个引脚
ECHO = 21

def t_up(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        time.sleep(t_time)
        
def t_stop(t_time):
        L_Motor.ChangeDutyCycle(0)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(0)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        
def t_down(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)

def t_left(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        time.sleep(t_time)

def t_right(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        
def keysacn():##按键开关引脚
    val = GPIO.input(BtnPin)#输入引脚
    while GPIO.input(BtnPin) == False:#低电平
        val = GPIO.input(BtnPin)
    while GPIO.input(BtnPin) == True:#高电平
        time.sleep(0.01)
        val = GPIO.input(BtnPin)
        if val == True:
            GPIO.output(Rpin,1)
            while GPIO.input(BtnPin) == False:
                GPIO.output(Rpin,0)
        else:
            GPIO.output(Rpin,0)
            
def setup():
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(TRIG, GPIO.OUT)
        GPIO.setup(ECHO, GPIO.IN)
        
        GPIO.setup(Gpin, GPIO.OUT)     # Set Green Led Pin mode to output
        GPIO.setup(Rpin, GPIO.OUT)     # Set Red Led Pin mode to output
        GPIO.setup(BtnPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)    # Set BtnPin's mode is input, and pull up to high level(3.3V)

        GPIO.setup(AIN2,GPIO.OUT)
        GPIO.setup(AIN1,GPIO.OUT)
        GPIO.setup(PWMA,GPIO.OUT)
        
        GPIO.setup(BIN1,GPIO.OUT)
        GPIO.setup(BIN2,GPIO.OUT)
        GPIO.setup(PWMB,GPIO.OUT)
def distance():#同超声波测量距离原理
	GPIO.output(TRIG, 0)
	time.sleep(0.000002)

	GPIO.output(TRIG, 1)
	time.sleep(0.00001)
	GPIO.output(TRIG, 0)#准备开始阶段

	
	while GPIO.input(ECHO) == 0:
		a = 0
	time1 = time.time()
	while GPIO.input(ECHO) == 1:
		a = 1
	time2 = time.time()

	during = time2 - time1
	return during * 340 / 2 * 100

def loop():#判断要不要规避
        while True:
                dis = distance()
                if (dis < 40) == True:          #前方有障碍物,进行规避右转
                        while (dis < 40) == True:#距离《40
                                t_down(30,0.5)          #占空比改变速度
                                t_right(30,0.1)
                                dis = distance()
                else:
                        t_up(50,0)
                print(dis, 'cm')
                print('')

def destroy():
	GPIO.cleanup()

if __name__ == "__main__":
        setup()         #设置对应输入输出引脚
        L_Motor= GPIO.PWM(PWMA,100)    #左边使能A#
        L_Motor.start(0)
        R_Motor = GPIO.PWM(PWMB,100)
        R_Motor.start(0)
        t_stop(.1)
 #       keysacn()#不需要这一步也行！！！
        try:
                loop()
        except KeyboardInterrupt:
                destroy()

二、带舵机模块：

舵机运转：（如果舵机不是由树莓派供电的话，需要将该电源与树莓派共地，也就是说电源的负极必须与树莓派任意一个GND连接，否则会出现舵机控制失常等现象。）如果

DutyCycle = angle/18 + 2-------通过改变占空比控制舵机转换角度

0度对应——2%占空比

90度对应——7%占空比

180度——12%占空比

工作原理：

控制电路板接受来自信号线的控制信号，控制电机转动，电机带动一系列齿轮组，减速后传动至输出舵盘。舵机的输出轴和位置反馈电位计是相连的，舵盘转动的同时，带动位置反馈电位计，电位计将输出一个电压信号到控制电路板，进行反馈，然后控制电路板根据所在位置决定电机的转动方向和速度，从而达到目标停止。

带舵机小车代码：

#!/usr/bin/env python
from Adafruit_PWM_Servo_Driver import PWM
import RPi.GPIO as GPIO
import time
import sys

PWMA   = 18
AIN1   = 22
AIN2   = 27

PWMB   = 23
BIN1   = 25
BIN2   = 24

BtnPin  = 19#舵机接口
Gpin    = 5
Rpin    = 6

TRIG = 20
ECHO = 21
# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x40,debug = False)

servoMin = 150  # Min pulse length out of 4096
servoMax = 600  # Max pulse length out of 4096

def setServoPulse(channel, pulse):
  pulseLength = 1000000.0                   # 1,000,000 us per second
  pulseLength /= 50.0                       # 60 Hz
  print("%d us per period" % pulseLength)
  pulseLength /= 4096.0                     # 12 bits of resolution
  print("%d us per bit" % pulseLength)
  pulse *= 1000.0
  pulse /= (pulseLength*1.0)
# pwmV=int(pluse)
  print("pluse: %f  " % (pulse))
  pwm.setPWM(channel, 0, int(pulse))#调用其他函数库

#Angle to PWM
def write(servonum,x):  #改变占空比-----》改变舵机角度！！！！！！！！！！！！！！！！！！！！
  y=x/90.0+0.5
  y=max(y,0.5)
  y=min(y,2.5)
  setServoPulse(servonum,y)
  
def t_up(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        time.sleep(t_time)
        
def t_stop(t_time):
        L_Motor.ChangeDutyCycle(0)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(0)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        
def t_down(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)

def t_left(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,True)#AIN2
        GPIO.output(AIN1,False) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,False)#BIN2
        GPIO.output(BIN1,True) #BIN1
        time.sleep(t_time)

def t_right(speed,t_time):
        L_Motor.ChangeDutyCycle(speed)
        GPIO.output(AIN2,False)#AIN2
        GPIO.output(AIN1,True) #AIN1

        R_Motor.ChangeDutyCycle(speed)
        GPIO.output(BIN2,True)#BIN2
        GPIO.output(BIN1,False) #BIN1
        time.sleep(t_time)
        
def keysacn():
    val = GPIO.input(BtnPin)
    while GPIO.input(BtnPin) == False:
        val = GPIO.input(BtnPin)
    while GPIO.input(BtnPin) == True:
        time.sleep(0.01)
        val = GPIO.input(BtnPin)
        if val == True:
            GPIO.output(Rpin,1)
            while GPIO.input(BtnPin) == False:
                GPIO.output(Rpin,0)
        else:
            GPIO.output(Rpin,0)
            
def setup():
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(TRIG, GPIO.OUT)
        GPIO.setup(ECHO, GPIO.IN)
        
        GPIO.setup(Gpin, GPIO.OUT)     # Set Green Led Pin mode to output
        GPIO.setup(Rpin, GPIO.OUT)     # Set Red Led Pin mode to output
        GPIO.setup(BtnPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)    # Set BtnPin's mode is input, and pull up to high level(3.3V)

        GPIO.setup(AIN2,GPIO.OUT)
        GPIO.setup(AIN1,GPIO.OUT)
        GPIO.setup(PWMA,GPIO.OUT)
        
        GPIO.setup(BIN1,GPIO.OUT)
        GPIO.setup(BIN2,GPIO.OUT)
        GPIO.setup(PWMB,GPIO.OUT)
        pwm.setPWMFreq(50)                        # Set frequency to 60 Hz
        
def distance(): #测量距离函数
	GPIO.output(TRIG, 0)
	time.sleep(0.000002)

	GPIO.output(TRIG, 1)
	time.sleep(0.00001)
	GPIO.output(TRIG, 0)

	
	while GPIO.input(ECHO) == 0:
		a = 0
	time1 = time.time()
	while GPIO.input(ECHO) == 1:
		a = 1
	time2 = time.time()

	during = time2 - time1
	return during * 340 / 2 * 100

def front_detection():#测量前后间距
        write(0,90)
        time.sleep(0.5)
        dis_f = distance()
        return dis_f

def left_detection():#
         write(0, 175)
         time.sleep(0.5)
         dis_l = distance()
         return dis_l
        
def right_detection():
        write(0,5)
        time.sleep(0.5)
        dis_r = distance()
        return dis_r
     
def loop():
        while True:
                dis1 = front_detection()
                if (dis1 < 40) == True:
                        t_stop(0.2)
                        t_down(50,0.5)
                        t_stop(0.2)
                        dis2 = left_detection()
                        dis3 = right_detection()
                        if (dis2 < 40) == True and (dis3 < 40) == True:
                                t_left(50,1)
                        elif (dis2 > dis3) == True:
                                t_left(50,0.3)
                                t_stop(0.1)
                        else:
                                t_right(50,0.3)
                                t_stop(0.1)
                else:
                        t_up(60,0)
              #  print dis1, 'cm'
               # print ''

def destroy():
	GPIO.cleanup()

if __name__ == "__main__":
        setup()
        L_Motor= GPIO.PWM(PWMA,100)
        L_Motor.start(0)
        R_Motor = GPIO.PWM(PWMB,100)
        R_Motor.start(0)
        keysacn()
        try:
                loop()
        except KeyboardInterrupt:
                destroy()

树莓派小车避障之——超声波控制（2.1）

不带舵机驱动代码：

二、带舵机模块：

舵机运转：（如果舵机不是由树莓派供电的话，需要将该电源与树莓派共地，也就是说电源的负极必须与树莓派任意一个GND连接，否则会出现舵机控制失常等现象。）如果

工作原理：

带舵机小车代码：

小车运行视频：

猜你喜欢

树莓派小车避障之——超声波控制（2.1）

不带舵机驱动代码：

二、带舵机模块：

舵机运转：（如果舵机不是由树莓派供电的话，需要将该电源与树莓派共地，也就是说电源的负极必须与树莓派任意一个GND连接，否则会出现舵机控制失常等现象。）如果

工作原理：

带舵机小车代码 ：

小车运行视频 ：

猜你喜欢

带舵机小车代码：

小车运行视频：