02-红外循迹+超声波避障

## 红外循迹+超声波避障
### 一、认识声波
>声波的性质:
声波是声音的传播形式，声波是一种机械波，由声源振动产生，声波可以被接收器记录，通过分析接收到的声波相位、振幅等动力学特征，可以得到介质的物理性质和力学特征。
声波探测的原理:
声波发射器向某一方向发射声波，在发射时刻的同时开始计时，声波在空气中传播，途中碰到障碍物就立即返回来，声波接收器收到反射波就立即停止计时。声波在空气中的传播速度为340m/s，根据计时器记录的时间t，就可以计算出发射点距障碍物的距离(s)，即：s=340t/2
### 二、认识超声波传感器
>超声波传感器是将超声波信号转换成其它能量信号的传感器。超声波是振动频率高于20kHz的机械波。它具有频率高、波长短、绕射现象小，特别是方向性好、能够成为射线而定向传播等特点。超声波对液体、固体的穿透本领很大。
![](/media/202506/2025-06-04_174051_6592650.6200763148768474.png#align=center)
小车配套的超声波传感器由1个发射器和1个接收器组成，并通过其连接到主控的GPIO引脚进行通信。需要使用GPIO库来读取传感器的状态，小车主控连接超声波发射器和接收器的GPIO比引脚号分别是449、474。
<div style="text-align: center">
<table style="display: inline-table; margin: 0 auto">
  <tr>
    <td></td><td></td><td></td>
  </tr>
  <tr>
    <td>传感器编号</td>
    <td>发射器</td>
    <td>接收器</td>
  <tr>
    <td>GPIO引脚号</td>
    <td>449</td>
    <td>474</td>
  </tr>
</table>
</div>

>使用发射器和接收器的端口号初始化一个超声波传感器对象（Ultrasonic），就可以控制和检测超声波信号了。可以根据超声波的发射和接收的间隔时间（Ultrasonic对象的output_time()方法）来判断小车前方的障碍物，并控制小车及时躲避。
### 三、程序流程
![](/media/202506/2025-06-05_091542_4944580.037763857865797945.png#align=center)
### 四、程序实现
	from time import sleep
	from bot.utils import Voice
	from tool import *
	import time
	import os
	import sys
	import json
	import math
	import numpy as np
	from bot.ugv import Motor
	from sensor import Ultrasonic，Infrared
	import cv2 as cv
	from cap import VideoCapture

def Ultrasonic_get_distance(sonic):
		# Echo引脚高电平持续时间就是超声波由发射到返回的时间，即用时间x声波速度/2等于单程即超声波距物体距离值
		distance = sonic.output_time() * 340 / 2 * 100
		# t2-t1时间单位s,声波速度340m/s,x100将距离值单位m转换成cm
		# print("distance is %d" % distance)  # 打印距离值
		if distance < 500:  # 正常检测距离值
		# print("distance is %d"%distance)
			DISTANCE = round(distance, 2)
		else:
			print("distance is 0")  # 如果距离值大于5m,超出检测范围
			DISTANCE = 0
		return DISTANCE

if __name__ == "__main__":
		#超声波传感器引脚脚号
		TRIG=449
		ECHO=474
		#初始速度
		speed=2500
		# #红外循迹传感器引脚脚号
		io1=444
		io2=496
		io3=443
		io4=510
		# 初始化语音模块
		voice_ = voice()
		try:
			sonic = Ultrasonic(TRIG,ECHO)
		except Exception as e:
			voice_.send(250)
			print(f"初始化超声波模块异常：{e}")
			sys.exit(1)  # 退出程序，返回非零值表示异常退出
		try:
			infrared1=Infrared(io1)
			infrared2=Infrared(io2)
			infrared3=Infrared(io3)
			infrared4=Infrared(io4)
		except Exception as e:
			voice_.send(250)
			print(f"初始化红外传感器异常：{e}")
			sys.exit(1)  # 退出程序，返回非零值表示异常退出

Control_Motor.GS_run(speed,speed)
		flag_turn=0
		status_run=0

try:
			cap = VideoCapture()
		except Exception as e:
			# 语音：未检测到摄像头
			voice_.send(248)
			print(f"初始化摄像头异常，error: {e}")
			time.sleep(5)
			exit(0)

voice_.send(116)

while True:
			try: 
				#获取距离
				dis = Ultrasonic_get_distance(sonic)
				print("dis:",dis)
				#读取io口当前接收状态
				io1_status=infrared1.Read_Status()
				io2_status=infrared2.Read_Status()
				io3_status=infrared3.Read_Status()
				io4_status=infrared4.Read_Status()
				#打印io口状态，方便调试
				print("io:",io1_status,io2_status,io3_status,io4_status)    
				if int(dis) < 25:#距离小于25cm
					duobi=1
				else:
					duobi=0
				if duobi==0:
					if io1_status==0 and io4_status==1:
						speed_L=speed-1200
						speed_R=speed+1200
						Control_Motor.GS_run(speed_L,speed_R)
						flag_turn=0
						status_run=0
					elif io4_status==0 and io1_status==1:
						speed_L=speed+1200
						speed_R=speed-1200
						Control_Motor.Advance()
						Control_Motor.GS_run(speed_L,speed_R)
						flag_turn=0
						status_run=1
					elif (io2_status==0 and io3_status==0) and (io1_status==1 or io4_status==1):
						speed_L=speed
						speed_R=speed
						Control_Motor.Advance()
						Control_Motor.GS_run(speed_L,speed_R)
						flag_turn=0
					elif (io2_status==0 and io3_status==1) and (io1_status==1 or io4_status==1):
						speed_L=speed-500
						speed_R=speed+500
						Control_Motor.GS_run(speed_L,speed_R)
						Control_Motor.Advance()
						flag_turn=0
						status_run=0
					elif (io2_status==1 and io3_status==0) and (io1_status==1 or io4_status==1):
						speed_L=speed+500
						speed_R=speed-500
						Control_Motor.GS_run(speed_L,speed_R)
						Control_Motor.Advance()
						flag_turn=0
						status_run=1
					elif io1_status==1 and io2_status==1 and io3_status==1 and io4_status==1:
						if flag_turn>25:
							speed_L=1500
							speed_R=1500
							Control_Motor.GS_run(speed_L,speed_R)
							if status_run==0:
								Control_Motor.Rotate_Left()
							else:
								Control_Motor.Rotate_Right()
						flag_turn=flag_turn+1
					else:
						Control_Motor.Stop()
					time.sleep(0.01)
				elif duobi==1:
					Control_Motor.Stop()
						print("前面有障碍")
					time.sleep(2)

if has_digit_input() == 'q':
					break

except KeyboardInterrupt:
				break
		#释放资源
		Control_Motor.close()
		infrared1.close()
		infrared2.close()
		infrared3.close()
		infrared4.close()
		sonic.close()
		cap.release()
		voice_.close()
		print("END----------------------")