from mpython import i2c, sleep_ms, MPythonPin, PinMode
from micropython import const
from machine import UART,Pin
import framebuf
import ubinascii
import ustruct
import math
import parrot
import neopixel
import time

MPU_ADDR = const(0x68)

MPU_DEVICE_ID_REG = 0x75
MPU_PWR_MGMT1_REG = 0x6B
MPU_PWR_MGMT2_REG = 0x6C
MPU_SAMPLE_RATE_REG = 0x19
MPU_CFG_REG = 0x1A
MPU_GYRO_CFG_REG = 0x1B
MPU_ACCEL_CFG_REG = 0x1C
MPU_FIFO_EN_REG = 0x23

MPU_INTBP_CFG_REG = 0x37
MPU_INT_EN_REG = 0x38

MPU_ACCEL_XOUTH_REG = 0x3B
MPU_ACCEL_XOUTL_REG = 0x3C
MPU_ACCEL_YOUTH_REG = 0x3D
MPU_ACCEL_YOUTL_REG = 0x3E
MPU_ACCEL_ZOUTH_REG = 0x3F
MPU_ACCEL_ZOUTL_REG = 0x40

MPU_TEMP_OUTH_REG = 0x41
MPU_TEMP_OUTL_REG = 0x42

MPU_GYRO_XOUTH_REG = 0x43
MPU_GYRO_XOUTL_REG = 0x44
MPU_GYRO_YOUTH_REG = 0x45
MPU_GYRO_YOUTL_REG = 0x46
MPU_GYRO_ZOUTH_REG = 0x47
MPU_GYRO_ZOUTL_REG = 0x48

MPU_USER_CTRL_REG = 0x6A

class Color(object):

    def __init__(self, i2c=i2c):
        self.i2c = i2c
        self.I2C_ADR = 0x39
        self.wb_rgb = [0.52,1,1]
        self.i2c.writeto_mem(0x12, 3, b'\x00')
        
    def turn_on(self):
        self.i2c.writeto_mem(0x12, 3, b'\x00')

    def turn_off(self):
        self.i2c.writeto_mem(0x12, 3, b'\x01')

    def setup(self):
        self.i2c.writeto_mem(self.I2C_ADR, 0x80, b'\x03')
        time.sleep_ms(10)
        self.i2c.writeto_mem(self.I2C_ADR, 0x81, b'\xD5')
        self.i2c.writeto_mem(self.I2C_ADR, 0x8F, b'\x01')
        
    def setbw_rgb(self,r,g,b):
        self.wb_rgb = [r,g,b]

    def getRGB(self):
        color = [0, 0, 0]
        buf = self.i2c.readfrom_mem(self.I2C_ADR, 0x96, 6)
        color_r = buf[1]*256 + buf[0]
        color_g = buf[3]*256 + buf[2]
        color_b = buf[5]*256 + buf[4]
        color_r = int(color_r/self.wb_rgb[0])
        color_g = int(color_g/self.wb_rgb[1])
        color_b = int(color_b/self.wb_rgb[2])
        maxColor = max(color_r, color_g, color_b)
        if maxColor > 255:
            scale = 255 / maxColor
            color_r = int(color_r * scale)
            color_g = int(color_g * scale)
            color_b = int(color_b * scale)
        return (color_r, color_g, color_b)

    def getHSV(self):
        rgb = self.getRGB()
        r, g, b = rgb[0], rgb[1], rgb[2]
        r, g, b = r / 255.0, g / 255.0, b / 255.0
        mx = max(r, g, b)
        mn = min(r, g, b)
        df = mx - mn
        if mx == mn:
            h = 0
        elif mx == r:
            h = (60 * ((g - b) / df) + 360) % 360
        elif mx == g:
            h = (60 * ((b - r) / df) + 120) % 360
        elif mx == b:
            h = (60 * ((r - g) / df) + 240) % 360
        if mx == 0:
            s = 0
        else:
            s = df / mx
        v = mx
        return round(h, 1), round(s, 1), round(v, 1)

    def discern(self):
        hsv=self.getHSV()
        if 0<=hsv[1]<=0.3:
            return 'none'
        elif 0<=hsv[0]<=6 or 340<=hsv[0]<=360:
            return 'red'
        elif 6<=hsv[0]<=25:
            return 'orange'
        elif 25<=hsv[0]<=70:
            return 'yellow'
        elif 70<=hsv[0]<=150:
            return 'green'
        elif 150<=hsv[0]<=180:
            return 'cyan'
        elif 180<=hsv[0]<=250:
            return 'blue'
        elif 250<=hsv[0]<=340:
            return 'purple'
        else:
            return 'null'

    def discern_red(self):
        if self.discern() == 'red':
            return True
        else:
            return False

    def discern_orange(self):
        if self.discern() == 'orange':
            return True
        else:
            return False

    def discern_yellow(self):
        if self.discern() == 'yellow':
            return True
        else:
            return False

    def discern_green(self):
        if self.discern() == 'green':
            return True
        else:
            return False

    def discern_cyan(self):
        if self.discern() == 'cyan':
            return True
        else:
            return False

    def discern_blue(self):
        if self.discern() == 'blue':
            return True
        else:
            return False

    def discern_purple(self):
        if self.discern() == 'purple':
            return True
        else:
            return False

class Ultrasonic(object):
    """
    超声波控制类
    """
    def __init__(self, i2c=i2c):
        self.i2c = i2c

    def distance(self):
        self.i2c.writeto(0x13, b'\x20')
        time.sleep_ms(2)
        buf = self.i2c.readfrom(0x13, 2)
        distance_mm = (buf[0]*256+buf[1])
        return distance_mm


class Line(object):

    def __init__(self, i2c=i2c):
        self.i2c = i2c
        self.I2C_ADR = 0x12
        self.threshold = 100
        
    def read_sensor(self,num):
        self.i2c.writeto(self.I2C_ADR, bytearray([33+num]))
        return self.i2c.readfrom(self.I2C_ADR,1)[0]

    def set_line_threshold(self,data=100):
        self.threshold = data
        
    def touch_line(self,num):
        if self.read_sensor(num)>self.threshold: 
            return True
        else:
            return False




class Motor(object):
    """
    电机控制模块控制类
    """
    def __init__(self, i2c=i2c):
        self.i2c = i2c
        self.I2C_ADR = 0x11

    def motor1(self, speed=100):
        i2c.writeto_mem(self.I2C_ADR, 1, bytearray([speed]))

    def motor2(self, speed=100):
        i2c.writeto_mem(self.I2C_ADR, 2, bytearray([speed]))


class SHT20(object):

    def __init__(self, i2c=i2c):
        self.i2c = i2c

    def temperature(self):
        """
        获取温度
        """
        self.i2c.writeto(0x40, b'\xf3')
        time.sleep_ms(70)
        t = i2c.readfrom(0x40, 2)
        return -46.86 + 175.72 * (t[0] * 256 + t[1]) / 65535

    def humidity(self):
        """
        获取湿度
        """
        self.i2c.writeto(0x40, b'\xf5')
        time.sleep_ms(25)
        t = i2c.readfrom(0x40, 2)
        return -6 + 125 * (t[0] * 256 + t[1]) / 65535


'''
MPU6050类
'''
class MPU6050(object):
    def __init__(self,i2c=i2c):
        self.i2c = i2c
        
    def Write_Mpu6050_REG(self, reg, dat):
        buf = bytearray(1)
        buf[0] = dat
        self.i2c.writeto_mem(MPU_ADDR, reg, buf)
        
    def Read_Mpu6050_REG(self, reg):
        t = self.i2c.readfrom_mem(MPU_ADDR, reg, 1)[0]
        return (t >> 4)*10 +(t % 16)

    def Read_Mpu6050_Len(self, reg, len, buffer):
        self.i2c.readfrom_mem_into(MPU_ADDR, reg, buffer)
        
    ##fsr:0,�250dps;1,�500dps;2,�1000dps;3,�2000dps
    def MPU_Set_Gyro_Fsr(self, fsr):
        return self.Write_Mpu6050_REG(MPU_GYRO_CFG_REG, fsr << 3)
    
    #fsr:0,�2g;1,�4g;2,�8g;3,�16g
    def MPU_Set_Accel_Fsr(self, fsr):
        return self.Write_Mpu6050_REG(MPU_ACCEL_CFG_REG, fsr << 3)
        
    def MPU_Set_LPF(self, lpf):
        if(lpf >= 188):
            data = 1
        elif(lpf >=98):
            data = 2
        elif(lpf >= 42):
            data = 3
        elif(lpf >= 20):
            data = 4
        elif(lpf >= 10):
            data = 5
        else:
            data = 6
        self.Write_Mpu6050_REG(MPU_CFG_REG, data)
    
    #rate:4~1000hz
    def MPU_Set_Rate(self, rate):
        if(rate > 1000):
            rate = 1000
        if(rate < 4):
            rate = 4
        data = int(1000/rate-1)
        datas = self.Write_Mpu6050_REG(MPU_SAMPLE_RATE_REG, data)
        return self.MPU_Set_LPF(rate/2)
        
    def MPU_Init(self):
        self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG, 0x80)
        time.sleep_ms(100)
        self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG, 0x00)
        self.MPU_Set_Gyro_Fsr(3)
        self.MPU_Set_Accel_Fsr(0)
        self.MPU_Set_Rate(50)
        self.Write_Mpu6050_REG(MPU_INT_EN_REG, 0x00)
        self.Write_Mpu6050_REG(MPU_USER_CTRL_REG, 0x00)
        self.Write_Mpu6050_REG(MPU_FIFO_EN_REG, 0x00)
        self.Write_Mpu6050_REG(MPU_INTBP_CFG_REG, 0x80)
        res = self.Read_Mpu6050_REG(MPU_DEVICE_ID_REG)
        if(res == 68):
            self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG,0x01)
            self.Write_Mpu6050_REG(MPU_PWR_MGMT2_REG,0x00)
            self.MPU_Set_Rate(50)
        else:
            return 1
        return 0

    def Get_Gyro_x(self):
        buf = bytearray(2)
        res = self.Read_Mpu6050_Len(MPU_GYRO_XOUTH_REG, 2, buf)
        gx = (buf[0] << 8) | buf[1]
        if(gx >= 0x8000):
            gx = -((65535 - gx) + 1)
        return 2000*gx/32768
    
    def Get_Gyro_y(self):
        buf = bytearray(2)
        res = self.Read_Mpu6050_Len(MPU_GYRO_YOUTH_REG, 2, buf)
        gy = (buf[0] << 8) | buf[1]
        if(gy >= 0x8000):
            gy = -((65535 - gy) + 1)
        return 2000*gy/32768
        
    def Get_Gyro_z(self):
        buf = bytearray(2)
        res = self.Read_Mpu6050_Len(MPU_GYRO_ZOUTH_REG, 2, buf)
        gz = (buf[0] << 8) | buf[1]
        if(gz >= 0x8000):
            gz = -((65535 - gz) + 1)
        return 2000*gz/32768
        
    def Get_Accel_x(self):
        buf = bytearray(2)
        res = self.Read_Mpu6050_Len(MPU_ACCEL_XOUTH_REG, 2, buf)
        ax = (buf[0] << 8) | buf[1]
        if(ax >= 0x8000):
            ax = -((65535 - ax) + 1)
        return 2*9.8*ax/32768
        
    def Get_Accel_y(self):
        buf = bytearray(2)
        res = self.Read_Mpu6050_Len(MPU_ACCEL_YOUTH_REG, 2, buf)
        ay = (buf[0] << 8) | buf[1]
        if(ay >= 0x8000):
            ay = -((65535 - ay) + 1)
        return 2*9.8*ay/32768
        
    def Get_Accel_z(self):
        buf = bytearray(2)
        res = self.Read_Mpu6050_Len(MPU_ACCEL_ZOUTH_REG, 2, buf)
        az = (buf[0] << 8) | buf[1]
        if(az >= 0x8000):
            az = -((65535 - az) + 1)
        return 2*9.8*az/32768

'''

千里马，小方舟AI类

'''
class K210_AI(object):
    def __init__(self, uart2_rx0=33, uart2_tx0=32): #RX:P0,TX:P1
        self.uart = UART(2, baudrate=115200, rx=uart2_rx0,tx=uart2_tx0,timeout=1000)
        self.ai_read_data = []  #起始x,y,w,h
        
    def AI_Uart_CMD(self,cmd,cmd_data=[0,0,0,0],str_buf=0):   #================#
        check_sum = 0
        CMD_TEMP = [0xAA, 0x55,cmd]
        CMD_TEMP.extend(cmd_data)
        for i in range(4-len(cmd_data)):
            CMD_TEMP.append(0)
        for i in range(len(CMD_TEMP)):
            check_sum = check_sum+CMD_TEMP[i]
        #print_x16(CMD_TEMP)
        self.uart.write(bytes(CMD_TEMP))
        if str_buf:
            self.uart.write(str_buf)
            for i in range(len(str_buf)):
                check_sum = check_sum + ord(str_buf[i])
            self.uart.write(bytes([(check_sum>>8)&0xFF,check_sum&0xFF]))
        
    
    def print_x16(self,date):
        for i in range(len(date)):
            print('{:2x}'.format(date[i]),end=' ')
        print('')
    
    #运行模式切换cmd:0x10，data：（0:颜色, 1:二维码, 2:人脸, 3: 20分类）    
    #中心方框对象学习，cmd:0x11
    #清除学习数据，cmd:0x12
    #TF卡模型加载，cmd:0x1A  data:(mode：4-分类模型，5-检测模型)  str:模型名字，带后缀
    
    #学习数据获取，cmd:0x20，data:(学习对象ID号)
    
    #屏幕旋转，      cmd:0x30  data:(0 or 1)
    #屏幕摄像头旋转，cmd:0x31  data:(0 or 1)
    #LCD字符串颜色， cmd:0x32  data:(color:[r,g,b])
    #LCD字符显示清除 cmd:0x33  
    #拍照存储到TF卡，cmd:0x3A  data:(mode: 0 or 1)
    #录像存储到TF卡，cmd:0x3B  data:(time: 1秒,max:500s)
    #LCD字符串显示， cmd:0x3C  data:(x,y)  str:显示字符串
    
    #颜色传感器使能，cmd:0x41  data:(0:启动4红外, 1:启动颜色)
    #小车扬声器使能，cmd:0x42  data:(0:关闭, 1:启动)
    #蜂鸣器模拟钢琴，cmd:0x45  data:(pitch:音调,  time=1==4/1拍==0.125秒)
    #K210开发板彩灯，cmd:0X46  data:(num,r,g,b)
    def AI_WaitForARP(self,cmd,data=[0,0,0,0],str_buf=0): #================#
        self.AI_Uart_CMD(cmd,data,str_buf)
        wait_time = 0
        if cmd&0xF0 == 0x20 or cmd&0x0F > 0x09:
            while (not self.uart.any()):
                wait_time = wait_time+1
                time.sleep_ms(10)
                if wait_time>300:
                    print("UART_NO_ACK_ERR")
                    break
            else:
                res=False
                time.sleep_ms(10)
                TEMP = self.uart.read(9)
                #self.print_x16(TEMP)
                if len(TEMP) > 3:
                    if TEMP[0]==0x55 and TEMP[1]==0xAA:
                        if TEMP[2]==cmd:
                            #self.print_x16(TEMP[2:])
                            res = True
                            self.ai_read_data = list(TEMP[3:]) #起始x,y,w,h
                        elif cmd&0x0F > 0x09:
                            #print("uart_cmd_sent_failed")
                            res = self.AI_WaitForARP(cmd,data,str_buf)
                return res   #返回False代表无识别数据#
    

###############################
    def get_ai_data(self):      #获取当前识别数据
        return self.AI_WaitForARP(0x21)
        
    def get_class_id(self):      #获取当前分类ID
        return self.ai_read_data[4]
        
    def get_class_value(self):    #获取准确度
        return self.ai_read_data[5]
    
    def learn_from_center(self):   #从中心框中学习
        self.AI_WaitForARP(0x11)
    
    def clean_data(self):     #清除学习数据
        self.AI_WaitForARP(0X12)

    def get_id_data(self, id):   #获取对象ID 数据
        return self.AI_WaitForARP(0x20, [id])
    
    def get_model_data(self):   #获取对象model数据
        self.AI_WaitForARP(0x20, [0])
        return self.ai_read_data
    
    def get_center_coord(self):  #获取学习数据中心坐标(x0,y0,w,h)
        if not self.ai_read_data:
            return None
        return [int(self.ai_read_data[0] + self.ai_read_data[2]/2),int(self.ai_read_data[1] + self.ai_read_data[3]/2),self.ai_read_data[2],self.ai_read_data[3]]

    def get_rect_coord(self):  #获取学习数据方框左上及右下坐标(x1,y1,x2,y2)
        if not self.ai_read_data:
            return None
        return [self.ai_read_data[0],self.ai_read_data[1],self.ai_read_data[0] + self.ai_read_data[2],self.ai_read_data[1] + self.ai_read_data[3]]
     

##############################
    def mode_change(self,mode): #运行任务模式切换
        self.AI_Uart_CMD(0x10,[mode,0,0,0])
        
    def add_sample_img(self,class_id):
        self.AI_WaitForARP(0x13,[class_id,0,0,0]) #通用模型添加样本图片
        
    def knn_model_train(self): #通用模型训练
        self.AI_WaitForARP(0x14,[0,0,0,0])
        
    def loading_sd_model(self,mode,model_name,class_name): #SD卡加载模型 mode: 4-分类模型，5-检测模型
        mode = [mode]
        if model_name.endswith(".kmodel"):
            mode.append(1)
        else:
            mode.append(0)
        self.AI_WaitForARP(0x1A,mode,model_name[0:-7]+':'+class_name)
    
    def load_knn_model(self,class_name): #加载通用学习模型
        self.AI_WaitForARP(0x1B,[0,len(class_name.split(',')),0,0],class_name)
        
    def save_knn_file(self,name): #储存通用模型学习文件
        self.AI_WaitForARP(0x1B,[1,0,0,0],name)
        
    def load_knn_file(self,file_name,class_name): #加载通用模型学习文件
        self.AI_WaitForARP(0x1B,[2,len(class_name.split(',')),0,0],file_name+':'+class_name)
        
#############################


    def lcd_rotation(self): #LCD屏幕旋转（千里马）
        self.AI_Uart_CMD(0x30)
        
    def lcd_senser_rotation(self): #LCD跟摄像头同时旋转（小方舟）
        self.AI_Uart_CMD(0x31)
        
    def lcd_display_color(self,r=0,g=128,b=255): #lcd字符显示颜色
        self.AI_Uart_CMD(0x32,[r,g,b,0])
        
    def lcd_display_clear(self): #清除LCD显示
        self.AI_Uart_CMD(0x33)
        
    def picture_capture(self,mode=0): #拍摄图片 mode:(0:320*240, 1:244*244(模型训练))
        self.AI_WaitForARP(0x3A,[mode,0,0,0])

    def video_capture(self,time): #录制视频
        self.AI_WaitForARP(0x3B,[int(time/2),0,0,0])
    
    def lcd_display_str(self,x,y,str_buf): #lcd字符串显示
        self.AI_WaitForARP(0x3C,[x,y,0,0],str_buf)
        
#############################
        
    def buzzer_piano_ring(self,pitch,time): #time=1==4/1拍，==0.125秒
        self.AI_Uart_CMD(0x45,[pitch,time])
        
    def set_ws2812_rgb(self,num,r,g,b): #默认开发板2个彩灯
        self.AI_Uart_CMD(0x46,[num,r,g,b])
#
#
#
#
#
#
#千里马
class Car(object):

    def __init__(self,i2c=i2c):
        self.i2c=i2c
        self.I2C_ADR=0x10
        self.I2C_ADR2=0x39
        self.buf1=bytes(1)
        self.buf4=bytes(2)
        self.threshold=100
        self.wb_rgb = [1,1.7,2]
        self.i2c.writeto_mem(0x10, 3, b'\x00')

    def light_switch(self,switch):
        self.i2c.writeto_mem(0x10, 3, bytearray([switch]))

    def forward(self,speed=100):
        if speed<0:
            speed=0-speed
        parrot.set_speed(parrot.MOTOR_1, speed)   
        parrot.set_speed(parrot.MOTOR_2, speed)

    def backward(self,speed=-100):
        if speed>0:
            speed=0-speed
        parrot.set_speed(parrot.MOTOR_1, speed)
        parrot.set_speed(parrot.MOTOR_2, speed)

    def turn_l(self,r_speed=100,l_speed=0,):
        parrot.set_speed(parrot.MOTOR_1, l_speed)   
        parrot.set_speed(parrot.MOTOR_2, r_speed)

    def turn_r(self,l_speed=100,r_speed=0):
        parrot.set_speed(parrot.MOTOR_1, l_speed)   
        parrot.set_speed(parrot.MOTOR_2, r_speed)

    def stop(self):
        parrot.set_speed(parrot.MOTOR_1, 0)
        parrot.set_speed(parrot.MOTOR_2, 0)

    def set_motor_speed(self,l_speed,r_speed):
        parrot.set_speed(parrot.MOTOR_1, l_speed)
        parrot.set_speed(parrot.MOTOR_2, r_speed)
    
    def read_sensor(self,num):
        self.i2c.writeto(self.I2C_ADR, bytearray([33+num]))
        return self.i2c.readfrom(self.I2C_ADR,1)[0]

    def set_line_threshold(self,data=100):
        self.threshold = data
        
    def touch_line(self,num):
        if self.read_sensor(num)>self.threshold: 
            return True
        else:
            return False

    def colorsenser(self):
        self.i2c.writeto_mem(self.I2C_ADR2, 0x80, b'\x03')
        time.sleep_ms(10)
        self.i2c.writeto_mem(self.I2C_ADR2, 0x81, b'\xD5') 
        self.i2c.writeto_mem(self.I2C_ADR2, 0x8F, b'\x01')
        self.light_switch(0)
    
    def setbw_rgb(self,r,g,b):
        self.wb_rgb = [r,g,b]

    def getRGB(self):
        color = [0, 0, 0]
        buf=self.i2c.readfrom_mem(self.I2C_ADR2, 0x96, 6)
        color_r=buf[1]*256 +buf[0]
        color_g=buf[3]*256 +buf[2]
        color_b=buf[5]*256 +buf[4]
        color_r=int(color_r/self.wb_rgb[0])
        color_g=int(color_g/self.wb_rgb[1])
        color_b=int(color_b/self.wb_rgb[2])
        maxColor = max(color_r, color_g, color_b)
        if maxColor > 255:
            scale = 255 / maxColor
            color_r = int(color_r * scale)
            color_g = int(color_g * scale)
            color_b = int(color_b * scale)
        return (color_r,color_g,color_b)

    def getHSV(self):
        rgb = self.getRGB()
        r, g, b = rgb[0], rgb[1], rgb[2]
        r, g, b = r / 255.0, g / 255.0, b / 255.0
        mx = max(r, g, b)
        mn = min(r, g, b)
        df = mx - mn
        if mx == mn:
            h = 0
        elif mx == r:
            h = (60 * ((g - b) / df) + 360) % 360
        elif mx == g:
            h = (60 * ((b - r) / df) + 120) % 360
        elif mx == b:
            h = (60 * ((r - g) / df) + 240) % 360
        if mx == 0:
            s = 0
        else:
            s = df / mx
        v = mx
        return round(h, 1), round(s, 1), round(v, 1)
    
    def discern(self):
        hsv=self.getHSV()
        if 0<=hsv[1]<=0.3:
            return 'none'
        elif 0<=hsv[0]<=6 or 340<=hsv[0]<=360:
            return 'red'
        elif 6<=hsv[0]<=25:
            return 'orange'
        elif 25<=hsv[0]<=70:
            return 'yellow'
        elif 70<=hsv[0]<=150:
            return 'green'
        elif 150<=hsv[0]<=180:
            return 'cyan'
        elif 180<=hsv[0]<=250:
            return 'blue'
        elif 250<=hsv[0]<=340:
            return 'purple'
        else:
            return 'null'

    def discern_red(self):
        if self.discern()=='red':
            return True
        else:
            return False

    def discern_orange(self):
        if self.discern()=='orange':
            return True
        else:
            return False

    def discern_yellow(self):
        if self.discern()=='yellow':
            return True
        else:
            return False

    def discern_green(self):
        if self.discern()=='green':
            return True
        else:
            return False

    def discern_cyan(self):
        if self.discern()=='cyan':
            return True
        else:
            return False

    def discern_blue(self):
        if self.discern()=='blue':
            return True
        else:
            return False

    def discern_purple(self):
        if self.discern()=='purple':
            return True
        else:
            return False

    def distance(self):
        self.i2c.writeto(0x13, b'\x20')
        buf = self.i2c.readfrom(0x13,2)
        distance_mm = (buf[0]*256+buf[1])        
        return distance_mm

#MP3模块
class JQ6500_MP3(object):
    def __init__(self, uart2_rx0=18, uart2_tx0=19): #RX:P0,TX:P1
        self.uart = UART(2, baudrate=9600, rx=uart2_rx0,tx=uart2_tx0,timeout=300)
        self.ai_read_data = []  #起始x,y,w,h
        
    def mp3_Uart_CMD(self,cmd,cmd_data=[]):   #================#
        cmd_temp = [0x7E,cmd[0],cmd[1]]
        cmd_temp.extend(cmd_data)
        cmd_temp.append(0xEF)
        #print_x16(CMD_TEMP)
        self.uart.write(bytes(cmd_temp))
        #time.sleep_ms(10)
        #print(self.uart.read())
        
    def MP3_WaitForARP(self,cmd,data=[]): #================#
        if self.uart.any():
            self.uart.read()
        self.mp3_Uart_CMD(cmd,data)
        wait_time = 0
        if cmd[1] > 0x40:
            while (not self.uart.any()):
                wait_time = wait_time+1
                time.sleep_ms(10)
                if wait_time>100:
                    print("UART_NO_ACK_ERR")
                    break
            else:
                return self.uart.read()   
        
    def print_x16(self,date):
        for i in range(len(date)):
            print('{:2x}'.format(date[i]),end=' ')
        print('')
        
    def sd_device_set(self):    #播放设备选择
        self.mp3_Uart_CMD([3,9],[1])
    
    def music_file_select(self,folder=1,file=1):    #MP3选择，文件夹01~99，文件001~999
        self.mp3_Uart_CMD([4,18],[folder,file])
        
    def music_golbal_select(self,num=0):    #MP3曲目全盘选择
        self.mp3_Uart_CMD([4,3],[num>>8,num&0xFF])
        
    def music_next(self):    #下一曲
        self.mp3_Uart_CMD([2,1])
        
    def music_prev(self):    #上一曲
        self.mp3_Uart_CMD([2,2])
        
    def music_sound_vol(self,data):  #音量大小0~30
        self.mp3_Uart_CMD([3,6],[data])
        
    def music_eq_set(self,mode=0):    #音乐EQ模式选择0~5
        self.mp3_Uart_CMD([3,7],[mode])
    
    def music_play_mode(self,mode=0):    #播放模式0~4：全部循环，文件夹循环，单曲循环，单曲循环，单曲播放
        self.mp3_Uart_CMD([3,17],[mode])
        
    def music_play(self):    #播放
        self.mp3_Uart_CMD([2,13])
        
    def music_pause(self):    #暂停
        self.mp3_Uart_CMD([2,14])
        
    def music_mode_change(self,mode):
        p16 = MPythonPin(16, PinMode.OUT)
        p16.write_digital(mode)