K210 Bit麦克风阵列声源定位

话不多，只甩代码

from Maix import MIC_ARRAY as mic
from machine import Timer, PWM
import lcd, time
from Maix import GPIO
from fpioa_manager import fm
import math, image



#激光控制引脚
LIGHT_PIN = 24

#MIC引脚
MIC_LED_CLK = 27
MIC_LED_DAT = 28
MIC_DAT0 = 29
MIC_DAT1 = 30
MIC_DAT2 = 31
MIC_DAT3 = 32
MIC_WS = 33
MIC_CLK = 34

#舵机引脚
SERVO_PIN1 = 22
SERVO_PIN2 = 23

class Servo:
    def __init__(self):
        #初始化定时器
        self.__tim_servo1 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
        self.__tim_servo2 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
        #初始化PWM
        self.__prv_servo1 = PWM(self.__tim_servo1, freq=50, duty=0, pin=SERVO_PIN1)
        self.__prv_servo2 = PWM(self.__tim_servo2, freq=50, duty=0, pin=SERVO_PIN2)

        self.angle(1,0)
        self.angle(2,0)

    def angle(self, servo, angle):
        """舵机角度控制"""
        try:
            if servo == 1:
                self.__prv_servo1.duty((angle+90)/180*10+2.5)
            else:
                self.__prv_servo2.duty((angle+90)/180*10+2.5)
        except Exception as e:
            print(e)

    def set_angle(self, x, y):
        """设置x,y"""
        self.angle(2,-x)
        self.angle(1,-y)



#卡尔曼
class Kalman:
    def __init__(self, last_err, current_err, x_hat, kgain, proc, mea):
        self.kf_last_err = last_err                                 #上次的协方差
        self.kf_current_err = current_err                           #本次的协方差
        self.kf_x_hat = x_hat                                       #卡尔曼滤波的计算值，即为后验最优值
        self.kf_kgain = kgain                                       #卡尔曼增益系数
        self.kf_proc = proc                                         #过程噪声
        self.kf_mea = mea                                           #测量噪声

    def kalman(self, value):
        """计算卡尔曼滤波后数据"""
        kf_x_hat = self.kf_x_hat                                    #当前先验预测值 = 上一次最优值
        self.kf_current_err = self.kf_last_err+self.kf_proc         #本次的协方差矩阵
        self.kf_kgain = self.kf_current_err/(self.kf_current_err+self.kf_mea)#卡尔曼增益系数计算
        out = kf_x_hat+self.kf_kgain*(value-kf_x_hat)               #当前最优值
        self.kf_x_hat = out                                         #更新最优值
        self.kf_last_err = (1-self.kf_kgain)*self.kf_current_err    #更新协方差矩阵
        return out

#麦克风
class Micro:
    def __init__(self):
        self.angle_last = 0
        self.sound_list = []
        self.kal_ang_x = Kalman(0.1, 0, 0, 0.5, 0.01, 0.8)
        self.kal_ang_y = Kalman(0.1, 0, 0, 0.5, 0.01, 0.8)
        self.kal_ang = Kalman(0.1, 0, 0, 1, 0.01, 0.1)


    def get_sound_degree(self):
        """获取12个麦克风的声音强度"""
        return mic.get_dir(mic.get_map())

    def print_sound_list(self):
        """打印12个麦克风强度"""
        print("{:^6},{:^6},{:^6},{:^6},{:^6},{:^6},{:^6},{:^6},{:^6},{:^6},{:^6},{:^6}".format(
            self.sound_list[0],self.sound_list[1],self.sound_list[2],self.sound_list[3],
            self.sound_list[4],self.sound_list[5],self.sound_list[6],self.sound_list[7],
            self.sound_list[8],self.sound_list[9],self.sound_list[10],self.sound_list[11]))

    def get_mic_data(self):
        """读取x,y轴坐标和角度数据"""
        angle = 0
        anglex = 0 #X坐标
        angley = 0 #Y坐标
        angler = 0 #强度
        ang_pi = 0
        data_list = []

        #获取12个声音强度数据
        self.sound_list = self.get_sound_degree()
        #self.print_sound_list()

        for index, sound in enumerate(self.sound_list):
            #if sound >= 2:
                #anglex += sound * math.sin(index * math.pi/6)
                #angley += sound * math.cos(index * math.pi/6)
            anglex += sound * math.sin(index * math.pi/6)
            angley += sound * math.cos(index * math.pi/6)

        #计算坐标转换值
        anglex = round(anglex,6)
        angley = round(angley,6)

        #使用卡尔曼平滑x,y坐标数据
        anglex = self.kal_ang_x.kalman(anglex)
        angley = self.kal_ang_y.kalman(angley)

        if angley < 0:
            ang_pi = 180
        if anglex < 0 and angley > 0:
            ang_pi = 360

        #参数修正
        if anglex != 0 or angley != 0:
            if angley == 0:
                #填补X轴角度
                angle = 90 if anglex > 0 else 270
            else:
                #计算角度
                angle = ang_pi+round(math.degrees(math.atan(anglex/angley)),4)

            #卡尔曼滤波，使角度数据更稳定
            angle = self.kal_ang.kalman(angle)-180

            data_list.append(anglex)     #X坐标
            data_list.append(angley)     #Y坐标
            data_list.append(angle)     #角度

        return data_list


    def get_mic(self):
        strength = 0

        #获取数据
        mic_data = self.get_mic_data()

        if mic_data:
            #计算强度
            strength = round(math.sqrt(mic_data[0]*mic_data[0]+mic_data[1]*mic_data[1]),4)
            return [mic_data[0],mic_data[1],mic_data[2],strength]
        else:
            return []


#初始化红外激光笔控制引脚
fm.register(LIGHT_PIN, fm.fpioa.GPIO0)
laser_pin = GPIO(GPIO.GPIO0, GPIO.OUT)

#初始化舵机
servo = Servo()

#初始化模块
lcd.init(type=1,freq=15000000,color=lcd.WHITE,invert=0,lcd_type=0)

#初始化麦克风
mic.deinit()
mic.init(   i2s_d0=MIC_DAT0,
            i2s_d1=MIC_DAT1,
            i2s_d2=MIC_DAT2,
            i2s_d3=MIC_DAT3,
            i2s_ws=MIC_WS,
            i2s_sclk=MIC_CLK,
            sk9822_dat=MIC_LED_DAT,
            sk9822_clk=MIC_LED_CLK)

#mic.set_led([0,0,0,0,0,0,0,0,0,0,0,0],(0,0,255))

miccro = Micro()

laser_pin.value(1)

servo_ang = 0

#设置屏幕方向
lcd.rotation(1)

#创建图片
img = image.Image()



while True:
    pass
    data = miccro.get_mic()
    mic.set_led(miccro.sound_list,(0,0,255))
    if data:
        #print(miccro.sound_list)
        #print("X坐标={:^6.2f}, Y坐标={:^6.2f}, 角度={:^6.2f}, 强度={:^6.2}".format(data[0],data[1],data[2],data[3]))
        #舵机设置角度
        if data[3] > 5: #如果强度大于5
            servo_ang = data[2]
            servo.set_angle(servo_ang,0)
    #显示
    img.draw_rectangle((0,0,img.width(),img.height()),color=(255,255,255),thickness=1,fill=True)
    img.draw_string(60, 100, "Sound Ang:{:^3.1f}".format(servo_ang), color=(255,0,0),scale=2)
    lcd.display(img)

mic.deinit()