2022年电赛E题声源定位跟踪系统
我们组本来是奔着视觉题去的，可是到比赛的时候突然发现好像就无人机比较合适，但是我们都没玩过无人机，本想转战小车，可是材料突然发现要两辆小车，材料也不够来不及买，于是我们就选了声源这道题，但是从来没有接触到这方面，最离谱的是刚开始我意会错题目，还想上摄像头和激光雷达哈哈哈哈。
之后我们就选用了k210的麦克风阵列来玩玩，还好之前有接触过，本想自己组装奈何实力不够，只能用现成的模块。
第一天我们试了网上的开源代码，发现这个阵列模块太容易受干扰了，于是我们组想先在k210阵列收音滤波后再转stm32控制舵机。代码如下，这个代码也可以直接k210接舵机使用，但是我们发现效果不佳。
from Maix import MIC_ARRAY as mic
from Maix import GPIO
import lcd,time,image
import math
import utime
from Maix import GPIO
from board import board_info
from fpioa_manager import fm
from machine import Timer,PWM
import time
#导入 FFT 模块
#输入时域数据（例如音频数据）并进行 FFT 运算
#res = FFT.run(data, points, shift)
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！这些字一定要读！！！！！！！！！！
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！
##########################################################################
##角度只取了阵列上字母s方向的五个RGB灯珠，大概60°，覆盖赛道，所以你的声源超过这个范围系统是不会反应的#############
##############距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）##################################
#########舵机口17，舵机我用的是270°舵机#######红外是1口，用HY.value(0)控制引脚高低电平#################################################
################你如果是180°舵机，需要把109行左右的Servo(S1,-Angle*0.666)中的0.666删掉#########################################
mic.init()#默认配置
lcd.init() #320*240
#mic.init(i2s_d0=34, i2s_d1=8, i2s_d2=33, i2s_d3=9, i2s_ws=32, i2s_sclk=10,\
#sk9822_dat=7, sk9822_clk=35)#可自定义配置 IO
io_led_red = 1
fm.register(io_led_red, fm.fpioa.GPIO0)#配置红外的GPIO
HY=GPIO(GPIO.GPIO0, GPIO.OUT)
fm.register(0, fm.fpioa.GPIO1)#配置两个开关的GPIO 是0和2脚 01是第二题 10第三题 11第四题
KG0=GPIO(GPIO.GPIO1, GPIO.IN, GPIO.PULL_DOWN)
#建议拿第四题测试
fm.register(2, fm.fpioa.GPIO2)#配置两个开关的GPIO 是0和2脚 01是第二题 10第三题 11第四题
KG1=GPIO(GPIO.GPIO2, GPIO.IN, GPIO.PULL_DOWN)
#KG0.value(1)
#image.font_load(image.UTF8, 16, 16, '/sd/0xA00000_font_uincode_16_16_tblr.Dzk')
#image.font_load(image.UTF8, 16, 16, 0xA00000) #加载字库
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=17)#舵机初始化
num=0
num2=0
Angle_LB = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
t=0
t1=0
t2=0
maxnum=0
minnum=0
jiaodu=0
Angle_last=0
b=[3,1,2,5,6]
i=100
a=[]
Biaozhi2=0 #函数2标志位
cishu=0
pid=0
err=0
JD=0
#以上变量都是给函数用的工具人，不用调用或者读取，没用
##########################卡尔曼##############################
KF_lastP=0.1 #上次的协方差
KF_nowP=0 #本次的协方差
KF_x_hat=0 #卡尔曼滤波的计算值，即为后验最优值
KF_Kg=0 #卡尔曼增益系数
KF_Q=0 #过程噪声
KF_R=0.01 #测量噪声
##################################################卡尔曼##########################
def Kalman_Filter(value):
global KF_lastP #上次的协方差
global KF_nowP #本次的协方差
global KF_x_hat #卡尔曼滤波的计算值，即为后验最优值
global KF_Kg #卡尔曼增益系数
global KF_Q #过程噪声
global KF_R #测量噪声
output=0 #output为卡尔曼滤波计算值
x_t=KF_x_hat #当前先验预测值 = 上一次最优值
KF_nowP=KF_lastP+KF_Q #本次的协方差矩阵
KF_Kg=KF_nowP/(KF_nowP+KF_R)#卡尔曼增益系数计算
output=x_t+KF_Kg*(value-x_t)#当前最优值
KF_x_hat=output #更新最优值
KF_lastP=(1-KF_Kg)*KF_nowP#更新协方差矩阵
return output
##############################函数0###############################################
def get_med(jiaodu,cs):
global a
global num2
a.append(jiaodu)
if num2==(cs-1) :
num2=0
for j in range(1,len(a)):
for i in range(len(a)-1):
if a[i] > a[i+1]:
a[i], a[i+1] = a[i+1],a[i]
t2=a[int((cs-1)/2)]
a=[] #清空，准备下一次测量
return t2
else :
num2+=1
return 1000
##############################函数1###############################################
def get_LB(jd,cs): #滤波，jd是传入数据，cs是滤波等级要大于2，越高越慢，返回滤波后的数据
global Angle_LB
global num
global minnum
global jiaodu
global maxnum
Angle_LB[num]=jd
if Angle_LB[num]>Angle_LB[maxnum] : maxnum=num #取最大最小值的位号
if Angle_LB[num]=0:
AngleX+= b[i] * math.sin(i * math.pi/6)
AngleY+= b[i] * math.cos(i * math.pi/6)
AngleX=round(AngleX,6) #计算坐标转换值
AngleY=round(AngleY,6)
if AngleY<0:AngleAddPi=180
if AngleX<0 and AngleY > 0:AngleAddPi=360
if AngleX!=0 or AngleY!=0: #参数修正
if AngleY==0:
Angle=90 if AngleX>0 else 270 #填补X轴角度
else:
Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):
if Angle>315 and Angle<360: #处理315-360的角度值
Angle=Angle-360
Angle=Angle_last*0.1+ Angle*0.9 #低通滤波
t=get_LB(Angle,3) #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据
if t !=1000 :
#t1=get_med(t,3) #再取中位数
#if t1 !=1000 :
if t>30 : t=30
if t<-30 : t=-30
Angle=t
Kalman_Filter(Angle)
Angle_last=Angle
lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)
lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
#Servo(S1,-pid*0.666) #控制270°舵机，180°把0.666删除
lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右边的区域
lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*167), 15, 15, (0, 255,200))#角度位置实时
JD=Angle
AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
mic_list.append(AngleX) #X坐标
mic_list.append(AngleY) #Y坐标
mic_list.append(AngleR) #强度
mic_list.append(Angle) #角度
#Juli=2.5/math.cos(Angle)
#mic_list.append(Juli)
#print(Juli)
a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值 （从计算的声源方向设置点亮对应的LED灯）
#lcd.display(0,color=(255,0,0))
#img1=draw_line(100,0,100,240,color=(255,0,0))
#lcd.display(img1)
#lcd._line(150,0,150,240,color=(255,0,0))
#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))
return mic_list #返回列表，X坐标，Y坐标，强度，角度
##############################函数3######第三题函数###############################
def get_mic_dir3():
global Angle_LB
global Angle_last
global cishu
AngleX=0
AngleY=0
AngleR=0
Angle=0
AngleAddPi=0
Juli=0
mic_list=[]
imga = mic.get_map() # 获取声音源分布图像(返回声源黑白位图)
b = mic.get_dir(imga) # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
#print(b[0],b[1],b[2],b[10],b[11])
cishu+=1 #判断进入函数的次数
if cishu==1 :
HY.value(1) # 第一次设置红外地线高电平，关闭红外
for i in range(len(b)):
if b[i]>=2:
AngleX+= b[i] * math.sin(i * math.pi/6)
AngleY+= b[i] * math.cos(i * math.pi/6)
AngleX=round(AngleX,6) #计算坐标转换值
AngleY=round(AngleY,6)
if AngleY<0:AngleAddPi=180
if AngleX<0 and AngleY > 0:AngleAddPi=360
if AngleX!=0 or AngleY!=0: #参数修正
if AngleY==0:
Angle=90 if AngleX>0 else 270 #填补X轴角度
else:
Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):
if Angle>315 and Angle<360: #处理330-360的角度值
#if Angle<340 : Angle=Angle-5
Angle=Angle-360
#if Angle>10 and Angle<45:
#Angle=Angle+10
Angle=Angle_last*0.1+ Angle*0.9 #低通滤波
t=get_LB(Angle,10) #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据
if t !=1000 :
t1=get_med(t,3) #再取中位数
if t1 !=1000 :
if t1>30 : t1=30
if t1<-30 : t1=-30
Angle=t1
Angle_last=Angle
HY.value(0) # 设置红外地线低电平，开启红外
lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)
lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
#距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）
Servo(S1,-Angle*0.666) #控制270°舵机，180°把0.75删除
lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右边的区域
lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置实时
AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
mic_list.append(AngleX) #X坐标
mic_list.append(AngleY) #Y坐标
mic_list.append(AngleR) #强度
mic_list.append(Angle) #角度
#Juli=2.5/math.cos(Angle)
#mic_list.append(Juli)
#print(Juli)
a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值 （从计算的声源方向设置点亮对应的LED灯）
#lcd.display(0,color=(255,0,0))
#img1=draw_line(100,0,100,240,color=(255,0,0))
#lcd.display(img1)
#lcd._line(150,0,150,240,color=(255,0,0))
#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))
return mic_list #返回列表，X坐标，Y坐标，强度，角度
##############################函数5###############################################
def get_mic_dir2(): ######################第二题函数
global Angle_LB
global Angle_last
AngleX=0
AngleY=0
AngleR=0
Angle=0
AngleAddPi=0
Juli=0
mic_list=[]
imga = mic.get_map() # 获取声音源分布图像(返回声源黑白位图)
b = mic.get_dir(imga) # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
#print(b[0],b[1],b[2],b[10],b[11])
Servo(S1,0) #控制270°舵机保持中立
HY.value(1) # 设置红外地线高电平，关闭红外
for i in range(len(b)):
if b[i]>=2:
AngleX+= b[i] * math.sin(i * math.pi/6)
AngleY+= b[i] * math.cos(i * math.pi/6)
AngleX=round(AngleX,6) #计算坐标转换值
AngleY=round(AngleY,6)
if AngleY<0:AngleAddPi=180
if AngleX<0 and AngleY > 0:AngleAddPi=360
if AngleX!=0 or AngleY!=0: #参数修正
if AngleY==0:
Angle=90 if AngleX>0 else 270 #填补X轴角度
else:
Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):
if Angle>315 and Angle<360: #处理330-360的角度值
if Angle<330 : Angle=Angle-9
Angle=Angle-350
#Angle=Angle_last*0.3+ Angle*0.7 #低通滤波
t=get_LB(Angle,5) #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据
if t !=1000 :
t1=get_med(t,3) #再取中位数
if t1 !=1000 :
if t1>30 : t1=30
if t1<-30 : t1=-30
Angle=t1
Angle_last=Angle
lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)
lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
#距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）
lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右边的区域
lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置实时
#Biaozhi2=1 #等于1就不会再进入该函数了
AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
mic_list.append(AngleX) #X坐标
mic_list.append(AngleY) #Y坐标
mic_list.append(AngleR) #强度
mic_list.append(Angle) #角度
#Juli=2.5/math.cos(Angle)
#mic_list.append(Juli)
#print(Juli)
a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值 （从计算的声源方向设置点亮对应的LED灯）
#lcd.display(0,color=(255,0,0))
#img1=draw_line(100,0,100,240,color=(255,0,0))
#lcd.display(img1)
#lcd._line(150,0,150,240,color=(255,0,0))
#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))
return mic_list #返回列表，X坐标，Y坐标，强度，角度
################################下面是只运行一次的程序###############################
lcd.fill_rectangle(46,5, 230, 4, (255, 0, 0)) #上边线
lcd.fill_rectangle(46,5, 4, 230, (255, 0, 0)) #左边线
lcd.fill_rectangle(46,235, 230, 4, (255, 0, 0)) #下边线
lcd.fill_rectangle(276,5, 4, 234, (255, 0, 0)) #右边线
lcd.fill_rectangle(237,5, 4, 234, (255, 0, 0)) #右边线2
lcd.fill_rectangle(0,77, 47, 4, (255, 0, 0)) #左区域上边线
lcd.fill_rectangle(0,154, 47, 4, (255, 0, 0)) #左区域下边线
lcd.fill_rectangle(0,77, 4, 77, (255, 0, 0)) #左区域左边线
#img.draw_string(20, 60, b'你好，世界', scale=1, color=(0,0,255), x_spacing=2, mono_space=1)
##############################主循环，###############################################
#这里因为我的2引脚有问题，下拉不了，如果你的引脚初始电平是0，就不用(~KG1.value()+2)这个处理了
#可以直接用KG1.value()==1来判断，如果有问题，就把这几个if删掉，保留第四题中的函数，程序才能跑
while True:
#引脚0和2的电平是01代表第二题，且第二题只会进行一次，返回一个值在屏幕
#if (KG0.value()==0)and((~KG1.value()+2)==1) :
#get_mic_dir2()
##引脚0和2的电平是10代表第三题
#if (KG0.value()==1)and((~KG1.value()+2)==0) :
#get_mic_dir3()
###引脚0和2的电平是11代表第四题
#if (KG0.value()==1)and((~KG1.value()+2)==1) :
get_mic_dir4()
err=JD-pid #简易PID控制舵机
pid+=err*0.1
Servo(S1,-pid*0.666) #控制270°舵机，180°把0.666删除
#print(KG0.value(),(~KG1.value()+2))
#get_mic_dir()
#time.sleep_ms(1)
第二天我们组装麦克风阵列和云台开始调试。代码如下最后发现还是定点还是会有很大的误差，时准时不准。代码更改如下
from Maix import MIC_ARRAY as mic
from Maix import GPIO
import lcd,time,image
import math
import utime
from Maix import GPIO
from board import board_info
from fpioa_manager import fm
from machine import Timer,PWM
import time
#导入 FFT 模块
#输入时域数据（例如音频数据）并进行 FFT 运算
#res = FFT.run(data, points, shift)
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！这些字一定要读！！！！！！！！！！
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！
###!!!!!!!!!!!!!!!!!!!!!!!注意注意注意！！！！！！！！！！！！！！！！！！
##########################################################################
##角度只取了阵列上字母s方向的五个RGB灯珠，大概60°，覆盖赛道，所以你的声源超过这个范围系统是不会反应的#############
##############距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）##################################
#########舵机口17，舵机我用的是270°舵机#######红外是1口，用HY.value(0)控制引脚高低电平#################################################
################你如果是180°舵机，需要把109行左右的Servo(S1,-Angle*0.666)中的0.666删掉#########################################
mic.init(i2s_d0=23, i2s_d1=22, i2s_d2=21, i2s_d3=20, i2s_ws=19, i2s_sclk=18, sk9822_dat=24, sk9822_clk=25)
lcd.init() #320*240
#mic.init(i2s_d0=34, i2s_d1=8, i2s_d2=33, i2s_d3=9, i2s_ws=32, i2s_sclk=10,\
#sk9822_dat=7, sk9822_clk=35)#可自定义配置 IO
io_led_red = 1
fm.register(io_led_red, fm.fpioa.GPIO0)#配置红外的GPIO
HY=GPIO(GPIO.GPIO0, GPIO.OUT)
fm.register(0, fm.fpioa.GPIO1)#配置两个开关的GPIO 是0和2脚 01是第二题 10第三题 11第四题
KG0=GPIO(GPIO.GPIO1, GPIO.IN, GPIO.PULL_DOWN)
#建议拿第四题测试
fm.register(2, fm.fpioa.GPIO2)#配置两个开关的GPIO 是0和2脚 01是第二题 10第三题 11第四题
KG1=GPIO(GPIO.GPIO2, GPIO.IN, GPIO.PULL_DOWN)
#KG0.value(1)
#image.font_load(image.UTF8, 16, 16, '/sd/0xA00000_font_uincode_16_16_tblr.Dzk')
#image.font_load(image.UTF8, 16, 16, 0xA00000) #加载字库
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=17)#舵机初始化
num=0
num2=0
Angle_LB = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
t=0
t1=0
t2=0
maxnum=0
minnum=0
jiaodu=0
Angle_last=0
b=[3,1,2,5,6]
i=100
a=[]
Biaozhi2=0 #函数2标志位
cishu=0
pid=0
err=0
JD=0
#以上变量都是给函数用的工具人，不用调用或者读取，没用
##########################卡尔曼##############################
KF_lastP=0.1 #上次的协方差
KF_nowP=0 #本次的协方差
KF_x_hat=0 #卡尔曼滤波的计算值，即为后验最优值
KF_Kg=0 #卡尔曼增益系数
KF_Q=0 #过程噪声
KF_R=0.01 #测量噪声
##################################################卡尔曼##########################
def Kalman_Filter(value):
global KF_lastP #上次的协方差
global KF_nowP #本次的协方差
global KF_x_hat #卡尔曼滤波的计算值，即为后验最优值
global KF_Kg #卡尔曼增益系数
global KF_Q #过程噪声
global KF_R #测量噪声
output=0 #output为卡尔曼滤波计算值
x_t=KF_x_hat #当前先验预测值 = 上一次最优值
KF_nowP=KF_lastP+KF_Q #本次的协方差矩阵
KF_Kg=KF_nowP/(KF_nowP+KF_R)#卡尔曼增益系数计算
output=x_t+KF_Kg*(value-x_t)#当前最优值
KF_x_hat=output #更新最优值
KF_lastP=(1-KF_Kg)*KF_nowP#更新协方差矩阵
return output
##############################函数0###############################################
def get_med(jiaodu,cs):
global a
global num2
a.append(jiaodu)
if num2==(cs-1) :
num2=0
for j in range(1,len(a)):
for i in range(len(a)-1):
if a[i] > a[i+1]:
a[i], a[i+1] = a[i+1],a[i]
t2=a[int((cs-1)/2)]
a=[] #清空，准备下一次测量
return t2
else :
num2+=1
return 1000
##############################函数1###############################################
def get_LB(jd,cs): #滤波，jd是传入数据，cs是滤波等级要大于2，越高越慢，返回滤波后的数据
global Angle_LB
global num
global minnum
global jiaodu
global maxnum
Angle_LB[num]=jd
if Angle_LB[num]>Angle_LB[maxnum] : maxnum=num #取最大最小值的位号
if Angle_LB[num]=0:
AngleX+= b[i] * math.sin(i * math.pi/6)
AngleY+= b[i] * math.cos(i * math.pi/6)
AngleX=round(AngleX,6) #计算坐标转换值
AngleY=round(AngleY,6)
if AngleY<0:AngleAddPi=180
if AngleX<0 and AngleY > 0:AngleAddPi=360
if AngleX!=0 or AngleY!=0: #参数修正
if AngleY==0:
Angle=90 if AngleX>0 else 270 #填补X轴角度
else:
Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):
if Angle>315 and Angle<360: #处理315-360的角度值
Angle=Angle-360
Angle=Angle_last*0.1+ Angle*0.9 #低通滤波+
t=get_LB(Angle,3) #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据
if t !=1000 :
#t1=get_med(t,3) #再取中位数
#if t1 !=1000 :
if t>30 : t=30
if t<-30 : t=-30
Angle=t
Kalman_Filter(Angle)
Angle_last=Angle
lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)
lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
#Servo(S1,-pid*0.666) #控制270°舵机，180°把0.666删除
lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右边的区域
lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*167), 15, 15, (0, 255,200))#角度位置实时
JD=Angle
AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
mic_list.append(AngleX) #X坐标
mic_list.append(AngleY) #Y坐标
mic_list.append(AngleR) #强度
mic_list.append(Angle) #角度
#Juli=2.5/math.cos(Angle)
#mic_list.append(Juli)
#print(Juli)
a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值 （从计算的声源方向设置点亮对应的LED灯）
#lcd.display(0,color=(255,0,0))
#img1=draw_line(100,0,100,240,color=(255,0,0))
#lcd.display(img1)
#lcd._line(150,0,150,240,color=(255,0,0))
#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))
return mic_list #返回列表，X坐标，Y坐标，强度，角度
##############################函数3######第三题函数###############################
def get_mic_dir3():
global Angle_LB
global Angle_last
global cishu
AngleX=0
AngleY=0
AngleR=0
Angle=0
AngleAddPi=0
Juli=0
mic_list=[]
imga = mic.get_map() # 获取声音源分布图像(返回声源黑白位图)
b = mic.get_dir(imga) # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
#print(b[0],b[1],b[2],b[10],b[11])
cishu+=1 #判断进入函数的次数
if cishu==1 :
HY.value(1) # 第一次设置红外地线高电平，关闭红外
for i in range(len(b)):
if b[i]>=2:
AngleX+= b[i] * math.sin(i * math.pi/6)
AngleY+= b[i] * math.cos(i * math.pi/6)
AngleX=round(AngleX,6) #计算坐标转换值
AngleY=round(AngleY,6)
if AngleY<0:AngleAddPi=180
if AngleX<0 and AngleY > 0:AngleAddPi=360
if AngleX!=0 or AngleY!=0: #参数修正
if AngleY==0:
Angle=90 if AngleX>0 else 270 #填补X轴角度
else:
Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):
if Angle>315 and Angle<360: #处理330-360的角度值
#if Angle<340 : Angle=Angle-5
Angle=Angle-360
#if Angle>10 and Angle<45:
#Angle=Angle+10
Angle=Angle_last*0.1+ Angle*0.9 #低通滤波
t=get_LB(Angle,10) #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据
if t !=1000 :
t1=get_med(t,3) #再取中位数
if t1 !=1000 :
if t1>30 : t1=30
if t1<-30 : t1=-30
Angle=t1
Angle_last=Angle
HY.value(0) # 设置红外地线低电平，开启红外
lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)
lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
#距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）
Servo(S1,-Angle*0.666) #控制270°舵机，180°把0.75删除
lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右边的区域
lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置实时
AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
mic_list.append(AngleX) #X坐标
mic_list.append(AngleY) #Y坐标
mic_list.append(AngleR) #强度
mic_list.append(Angle) #角度
#Juli=2.5/math.cos(Angle)
#mic_list.append(Juli)
#print(Juli)
a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值 （从计算的声源方向设置点亮对应的LED灯）
#lcd.display(0,color=(255,0,0))
#img1=draw_line(100,0,100,240,color=(255,0,0))
#lcd.display(img1)
#lcd._line(150,0,150,240,color=(255,0,0))
#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))
return mic_list #返回列表，X坐标，Y坐标，强度，角度
##############################函数5###############################################
def get_mic_dir2(): ######################第二题函数
global Angle_LB
global Angle_last
AngleX=0
AngleY=0
AngleR=0
Angle=0
AngleAddPi=0
Juli=0
mic_list=[]
imga = mic.get_map() # 获取声音源分布图像(返回声源黑白位图)
b = mic.get_dir(imga) # 计算、获取声源方向(从声源位图计算声源方向 返回12个强度值 对应12个LED灯)
#print(b[0],b[1],b[2],b[10],b[11])
Servo(S1,0) #控制270°舵机保持中立
HY.value(1) # 设置红外地线高电平，关闭红外
for i in range(len(b)):
if b[i]>=2:
AngleX+= b[i] * math.sin(i * math.pi/6)
AngleY+= b[i] * math.cos(i * math.pi/6)
AngleX=round(AngleX,6) #计算坐标转换值
AngleY=round(AngleY,6)
if AngleY<0:AngleAddPi=180
if AngleX<0 and AngleY > 0:AngleAddPi=360
if AngleX!=0 or AngleY!=0: #参数修正
if AngleY==0:
Angle=90 if AngleX>0 else 270 #填补X轴角度
else:
Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
if (Angle>0 and Angle<45)or (Angle>315 and Angle<360):
if Angle>315 and Angle<360: #处理330-360的角度值
if Angle<330 : Angle=Angle-9
Angle=Angle-350
#Angle=Angle_last*0.3+ Angle*0.7 #低通滤波
t=get_LB(Angle,5) #滤波，jd是传入数据，cs是滤波等级，越高越慢，返回滤波后的数据
if t !=1000 :
t1=get_med(t,3) #再取中位数
if t1 !=1000 :
if t1>30 : t1=30
if t1<-30 : t1=-30
Angle=t1
Angle_last=Angle
lcd.draw_string(60, 200, "Angle: " + str(-Angle), lcd.BLUE, lcd.BLACK)
lcd.draw_string(60, 180, "Distance: " + str(275/math.cos(-Angle*math.pi/180)), lcd.BLUE, lcd.BLACK)
#距离只能在比赛规定场地测算，注意声源和系统垂直距离是275cm（D区中间）
lcd.fill_rectangle(251,10, 25, 225, (0, 0, 0)) #清空右边的区域
lcd.fill_rectangle(251,int(108+math.tan(Angle*math.pi/180)*165), 15, 15, (0, 255,200))#角度位置实时
#Biaozhi2=1 #等于1就不会再进入该函数了
AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
mic_list.append(AngleX) #X坐标
mic_list.append(AngleY) #Y坐标
mic_list.append(AngleR) #强度
mic_list.append(Angle) #角度
#Juli=2.5/math.cos(Angle)
#mic_list.append(Juli)
#print(Juli)
a = mic.set_led(b,(10,10,0))# 配置 RGB LED 颜色值 （从计算的声源方向设置点亮对应的LED灯）
#lcd.display(0,color=(255,0,0))
#img1=draw_line(100,0,100,240,color=(255,0,0))
#lcd.display(img1)
#lcd._line(150,0,150,240,color=(255,0,0))
#img.draw_rectangle((30,30,50,50), color = (255, 0, 0))
return mic_list #返回列表，X坐标，Y坐标，强度，角度
################################下面是只运行一次的程序###############################
lcd.fill_rectangle(46,5, 230, 4, (255, 0, 0)) #上边线
lcd.fill_rectangle(46,5, 4, 230, (255, 0, 0)) #左边线
lcd.fill_rectangle(46,235, 230, 4, (255, 0, 0)) #下边线
lcd.fill_rectangle(276,5, 4, 234, (255, 0, 0)) #右边线
lcd.fill_rectangle(237,5, 4, 234, (255, 0, 0)) #右边线2
lcd.fill_rectangle(0,77, 47, 4, (255, 0, 0)) #左区域上边线
lcd.fill_rectangle(0,154, 47, 4, (255, 0, 0)) #左区域下边线
lcd.fill_rectangle(0,77, 4, 77, (255, 0, 0)) #左区域左边线
#img.draw_string(20, 60, b'你好，世界', scale=1, color=(0,0,255), x_spacing=2, mono_space=1)
##############################主循环，###############################################
#这里因为我的2引脚有问题，下拉不了，如果你的引脚初始电平是0，就不用(~KG1.value()+2)这个处理了
#可以直接用KG1.value()==1来判断，如果有问题，就把这几个if删掉，保留第四题中的函数，程序才能跑
while True:
#引脚0和2的电平是01代表第二题，且第二题只会进行一次，返回一个值在屏幕
#if (KG0.value()==0)and((~KG1.value()+2)==1) :
#get_mic_dir2()
##引脚0和2的电平是10代表第三题
#if (KG0.value()==1)and((~KG1.value()+2)==0) :
#get_mic_dir3()
###引脚0和2的电平是11代表第四题
#if (KG0.value()==1)and((~KG1.value()+2)==1) :
get_mic_dir4()
err=JD-pid #简易PID控制舵机
pid+=err*0.1
Servo(S1,-pid*0.666) #控制270°舵机，180°把0.666删除
#print(KG0.value(),(~KG1.value()+2))
#get_mic_dir()
#time.sleep_ms(1)
第三天，选音乐，没办法像大佬他们一样用特定频率去接收音频，接着就是调试，调试
第四天写论文听天由命哈哈哈哈，之后就封箱等检测了。需要stm32端代码的可以私聊我。
最近有玩这声源题的小伙伴有点多，在这里放百度云的链接，之前打电赛的代码都放里面了，有单纯k210和模块的连接就可运行的代码，也有k210与stm32之间通讯控制的代码，继电器控制激光笔，用stm32代码先烧录k210对应的代码进k210在接stm32。代码因为比赛时间紧写得不好，我们是在里面写了两套，一套是两个麦克风阵列模块，一套是我们比赛只用一个麦克风阵列模块，都写在一个stm32代码里面了，有需要的小伙伴可参考一下，代码写得不好的话轻喷哈哈哈。
链接：https://pan.baidu.com/s/1-y6ok394kM8b801JyysLbQ
提取码：1111
--来自百度网盘超级会员V3的分享