我們組本來是奔著視覺題去的，可是到比賽的時候突然發現好像就無人機比較合適，但是我們都沒玩過無人機，本想轉戰小車，可是材料突然發現要兩輛小車，材料也不夠來不及買，于是我們就選了聲源這道題，但是從來沒有接觸到這方面，最離譜的是剛開始我意會錯題目，還想上攝像頭和激光雷達哈哈哈哈。

?

?

之后我們就選用了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 aglobal num2a.append(jiaodu)if num2==(cs-1) :num2=0for 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 t2else :num2+=1return 1000##############################函數1############################################### def get_LB(jd,cs): #濾波，jd是傳入數據，cs是濾波等級要大于2，越高越慢，返回濾波后的數據global Angle_LBglobal numglobal minnumglobal jiaoduglobal maxnumAngle_LB[num]=jdif Angle_LB[num]>Angle_LB[maxnum] : maxnum=num #取最大最小值的位號if Angle_LB[num]<Angle_LB[minnum] : minnum=numif num==cs:Angle_LB[maxnum]=0 #去掉最大值最小值Angle_LB[minnum]=0for i in range(cs):jiaodu+=Angle_LB[i]jiaodu/=(cs-1)num=0maxnum=0minnum=0return int(jiaodu)else:num+=1return 1000 ##############################函數2##########舵機函數############################### def Servo(servo,angle):S1.duty((angle+90)/180*10+2.5) ##############################函數3######第四題函數############################### def get_mic_dir4():global Angle_LBglobal Angle_lastglobal pidglobal JDAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計算、獲取聲源方向(從聲源位圖計算聲源方向 返回12個強度值 對應12個LED燈)#print(b[0],b[1],b[2],b[10],b[11])for i in range(len(b)):if b[i]>=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=180if AngleX<0 and AngleY > 0:AngleAddPi=360if 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-360Angle=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=30if t<-30 : t=-30Angle=tKalman_Filter(Angle)Angle_last=Anglelcd.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=AngleAngleR=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_LBglobal Angle_lastglobal cishuAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_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=180if AngleX<0 and AngleY > 0:AngleAddPi=360if 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-5Angle=Angle-360#if Angle>10 and Angle<45:#Angle=Angle+10Angle=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=30if t1<-30 : t1=-30Angle=t1Angle_last=AngleHY.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_LBglobal Angle_lastAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_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=180if AngleX<0 and AngleY > 0:AngleAddPi=360if 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-9Angle=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=30if t1<-30 : t1=-30Angle=t1Angle_last=Anglelcd.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)) #右邊線2lcd.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.1Servo(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 aglobal num2a.append(jiaodu)if num2==(cs-1) :num2=0for 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 t2else :num2+=1return 1000##############################函數1############################################### def get_LB(jd,cs): #濾波，jd是傳入數據，cs是濾波等級要大于2，越高越慢，返回濾波后的數據global Angle_LBglobal numglobal minnumglobal jiaoduglobal maxnumAngle_LB[num]=jdif Angle_LB[num]>Angle_LB[maxnum] : maxnum=num #取最大最小值的位號if Angle_LB[num]<Angle_LB[minnum] : minnum=numif num==cs:Angle_LB[maxnum]=0 #去掉最大值最小值Angle_LB[minnum]=0for i in range(cs):jiaodu+=Angle_LB[i]jiaodu/=(cs-1)num=0maxnum=0minnum=0return int(jiaodu)else:num+=1return 1000 ##############################函數2##########舵機函數############################### def Servo(servo,angle):S1.duty((angle+90)/180*10+2.5) ##############################函數3######第四題函數############################### def get_mic_dir4():global Angle_LBglobal Angle_lastglobal pidglobal JDAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_list=[]imga = mic.get_map() # 獲取聲音源分布圖像(返回聲源黑白位圖)b = mic.get_dir(imga) # 計算、獲取聲源方向(從聲源位圖計算聲源方向 返回12個強度值 對應12個LED燈)#print(b[0],b[1],b[2],b[10],b[11])for i in range(len(b)):if b[i]>=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=180if AngleX<0 and AngleY > 0:AngleAddPi=360if 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-360Angle=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=30if t<-30 : t=-30Angle=tKalman_Filter(Angle)Angle_last=Anglelcd.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=AngleAngleR=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_LBglobal Angle_lastglobal cishuAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_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=180if AngleX<0 and AngleY > 0:AngleAddPi=360if 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-5Angle=Angle-360#if Angle>10 and Angle<45:#Angle=Angle+10Angle=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=30if t1<-30 : t1=-30Angle=t1Angle_last=AngleHY.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_LBglobal Angle_lastAngleX=0AngleY=0AngleR=0Angle=0AngleAddPi=0Juli=0mic_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=180if AngleX<0 and AngleY > 0:AngleAddPi=360if 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-9Angle=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=30if t1<-30 : t1=-30Angle=t1Angle_last=Anglelcd.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)) #右邊線2lcd.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.1Servo(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的分享

總結

以上是生活随笔為你收集整理的2022年电赛E题声源定位跟踪系统的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。