很简略的五路红外+超声波循迹+红绿灯的小程序(2020-09-0
2020-09-02 本文已影响0人
我开心0536
小车01.jpg
非常简略的五路红外+超声波循迹+红绿灯的小车程序，
未连接电机，只是简单测试
程序已调试成功，但精度不高，后续需要继续改进
【功能如下】
1、检测到前方30厘米有障碍，会左右灯全亮、喇叭响
2、发生碰撞时，会左右灯全亮、喇叭响
3、偏左或偏右时，会有转向灯提示，
4、如果是严重偏左或偏右时，会有对应的转向灯+喇叭提示
#===================================
# 五路循迹+超声波+红绿灯2个
# 2020.09.02 我开心
#===================================
try:
import RPi.GPIO as GPIO
import time
import tm1637 #请保证tm1637.py已复制到当前目录下
except RuntimeError:
print(" import 引入错误 ... ")
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False) #关闭警告
#===================================
# 红绿灯
#===================================
#红绿灯-左
#led_left_R=2
#led_left_Y=3
#led_left_G=14
led_list_left=[2,3,14] #红黄绿
#红绿灯-右
#led_right_R=21
#led_right_Y=26
#led_right_G=20
led_list_right=[21,26,20] #红黄绿
#GPIO.setup(led_left_R, GPIO.OUT)
#GPIO.setup(led_left_Y, GPIO.OUT)
#GPIO.setup(led_left_G, GPIO.OUT)
GPIO.setup(led_list_left, GPIO.OUT,initial=GPIO.LOW)
#GPIO.setup(led_right_R, GPIO.OUT)
#GPIO.setup(led_right_Y, GPIO.OUT)
#GPIO.setup(led_right_G, GPIO.OUT)
GPIO.setup(led_list_right, GPIO.OUT,initial=GPIO.LOW)
def led_liang(): #定义函数 LED所有灯常亮
for led in led_list_left:
GPIO.output(led, GPIO.HIGH)
for led in led_list_right:
GPIO.output(led, GPIO.HIGH)
def led_mie(): #定义函数 LED所有灯常灭
for led in led_list_left:
GPIO.output(led, GPIO.LOW)
for led in led_list_right:
GPIO.output(led, GPIO.LOW)
def led_left(): #定义函数 LED 左转流水灯
led_mie()
for led in led_list_left:
GPIO.output(led, GPIO.LOW)
time.sleep(0.05)
GPIO.output(led, GPIO.HIGH)
time.sleep(0.05)
def led_right(): #定义函数 LED 右转流水灯
led_mie()
for led in led_list_right:
GPIO.output(led, GPIO.LOW)
time.sleep(0.05)
GPIO.output(led, GPIO.HIGH)
time.sleep(0.05)
#===================================
# 超声波
#===================================
trig=24 #send-pin
echo=25 #receive-pin
GPIO.setup(trig,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(echo,GPIO.IN)
def chaoshengbo(): #定义函数 LED 右转流水灯
#send
GPIO.output(trig,True)
time.sleep(0.00011) #1us
GPIO.output(trig,False)
#start recording
while GPIO.input(echo)==0:
pass
start=time.time()
#end recording
while GPIO.input(echo)==1:
pass
end=time.time()
#compute distance
distance=round((end-start)*343/2*100,2)
print('距离前方:{0}cm'.format(distance))
return distance
#===================================
# 喇叭9号口
#===================================
GPIO.setup(9,GPIO.OUT,initial=GPIO.LOW)
def speak():
GPIO.output(9,GPIO.HIGH) #喇叭响
time.sleep(0.05)
GPIO.output(9,GPIO.LOW)
#===================================
# 五路循迹+碰撞检测
#===================================
S1=15 #左1
S2= 4 #左2
S3=18 #中
S4=17 #右2
S5=27 #右1
CLP=23 #碰撞
NEAR=22 #测距
GPIO.setup(S1, GPIO.IN)
GPIO.setup(S2, GPIO.IN)
GPIO.setup(S3, GPIO.IN)
GPIO.setup(S4, GPIO.IN)
GPIO.setup(S5, GPIO.IN)
GPIO.setup(CLP, GPIO.IN)
GPIO.setup(NEAR, GPIO.IN)
def wulu(): #定义五路及碰撞检测
SS=SS1=SS2=SS3=SS4=SS5=CLPCLP=0
if GPIO.input(S1)==1:
SS1=1
print(' S1左1 检测到道路！')
if GPIO.input(S2)==1:
SS2=10
print(' S2左2 检测到道路！')
if GPIO.input(S3)==1:
SS3=100
print(' S3中间 检测到道路！')
if GPIO.input(S4)==1:
SS4=1000
print(' S4右2 检测到道路！')
if GPIO.input(S5)==1:
SS5=10000
print(' S5右1 检测到道路！')
if GPIO.input(CLP)==1:
CLPCLP=100000
print(' CLP碰撞 发生碰撞！！！！！！')
SS=SS1+SS2+SS3+SS4+SS5+CLPCLP #返回值，目前本算法不太精确，正在改进中！！！
#print(str(SS))
return SS
#===================================
# 电机
#===================================
INT1=19
INT2=16
INT3=13
INT4=6
GPIO.setup(INT1,GPIO.OUT)
GPIO.setup(INT2,GPIO.OUT)
GPIO.setup(INT3,GPIO.OUT)
GPIO.setup(INT4,GPIO.OUT)
def car_forward(): #定义前进函数
GPIO.output(INT1,GPIO.HIGH) #将INT1接口设置为高电压
GPIO.output(INT2,GPIO.LOW) #将INT2接口设置为低电压
GPIO.output(INT3,GPIO.HIGH) #将INT3接口设置为高电压
GPIO.output(INT4,GPIO.LOW) #将INT4接口设置为低电压
def car_back(): #定义后退函数
GPIO.output(INT1,GPIO.LOW)
GPIO.output(INT2,GPIO.HIGH)
GPIO.output(INT3,GPIO.LOW)
GPIO.output(INT4,GPIO.HIGH)
def car_left(): #定义左转函数
GPIO.output(INT1,GPIO.HIGH)
GPIO.output(INT2,GPIO.LOW)
GPIO.output(INT3,GPIO.LOW)
GPIO.output(INT4,GPIO.HIGH)
def car_right(): #定义右转函数
GPIO.output(INT1,GPIO.LOW)
GPIO.output(INT2,GPIO.HIGH)
GPIO.output(INT3,GPIO.HIGH)
GPIO.output(INT4,GPIO.LOW)
def car_stop(): #定义停止函数
GPIO.output(INT1,GPIO.LOW)
GPIO.output(INT2,GPIO.LOW)
GPIO.output(INT3,GPIO.LOW)
GPIO.output(INT4,GPIO.LOW)
#===================================
# LED屏幕
#===================================
CLK=16
DIO=19
led = tm1637.TM1637(clk=CLK, dio=DIO) #实例化TM1637
def led8888():
#测试，全点亮
led.show('8888', True)
tm.sleep(1)
#全灭4个空格
led.show(' ')
tm.sleep(1)
#显示数字
led.number(1234)
tm.sleep(1)
#===================================
#===================================
# 主程序开始！！！！！
#===================================
#===================================
i=1 #循环次数
juli=0 #超声波距离
daolu=0 #返回的道路值
led.scroll('Li Jia Hao 2020') # 4 fps
try:
while True:
print(' 第'+str(i)+'次开始')
led.number(i) #在LED屏幕显内容
juli=chaoshengbo()
if juli < 30: #如果距离前方30厘米有障碍
car_stop()
print('前方30厘米有障碍！有障碍！有障碍！')
led_liang() #LED灯全亮提醒！
speak() #喇叭响
else: #=前方没有障碍的情况下，检测道路
led_mie()
daolu=wulu() #取得返回的道路数据
if daolu>=100000:
print('碰撞 发生碰撞！！！！！！')
#car_back()
led_liang() #LED灯全亮提醒！
speak() #喇叭响
elif daolu>=10000 :
print('车辆严重偏左')
#car_right()
#car_right()
led_right() #右转向灯亮
speak() #喇叭响
elif daolu>=1000 :
print('车辆偏左')
#car_right()
led_right() #右转向灯亮
elif daolu>=100 :
print('车辆正常行驶，一切OK！')
#car_forward()
elif daolu>=10 :
print('车辆偏右')
#car_left()
led_left() #左转向灯亮
elif daolu>=1 :
print('车辆严重偏右')
#car_left()
#car_left()
led_left() #左转向灯亮
speak() #喇叭响
else:
#car_stop()
print('路呢？路在何方？？？ 请把小车放在黑色道路上')
#speak() #喇叭响
#循环的次数+1
i=i+1
time.sleep(0.2)
except KeyboardInterrupt:
print('KeyboardInterrupt 程序被人为中止....')
finally:
GPIO.cleanup()
print('Exit 程序共运行了'+str(i)+'次，程序结束。')
