CocoPi/backup/CocoPi.py

'''
=========================================================
    ______                         ______
   /\  _  \                       /\  _  \  __
   \ \ \/\_\    ____    ____   ___\ \ \_\ \/\_\
    \ \ \/_/_  / __ \  / __ \ / __'\ \  __/\/_/_
     \ \ \_\ \/\ \_\ \/\ \__//\ \_\ \ \ \/   /\ \
      \ \____/\ \____/\ \____\ \____/\ \_\   \ \_\
       \/___/  \/___/  \/____/\/___/  \/_/    \/_/
  __
 /\ \       __                           ----------------
 \ \ \     /\_\    ____  __  __  __  _    pi.cocorobo.hk
  \ \ \    \/_/_ /  _  \/\ \/\ \/\ \/ \  ----------------
   \ \ \____ /\ \/\ \/\ \ \ \_\ \/>  </   SYSTEM VERSION
    \ \_____\\ \_\ \_\ \_\ \____//\_/\_\  -- 23.06.01 ---
     \/_____/ \/_/\/_/\/_/\/___/ \//\/_/ ----------------

=========================================================
'''
#########################################################
# ------------------------------------------------------
#|          封装包内容：         |        类名称：      |
#|-------------------------------|----------------------|
#|  2023.02.16:                  |                      |
#|  【添加】                     |                      |
#|  1、AHT20温湿度传感器         |    AHT20             |
#|  2、MPU6050陀螺仪六轴传感器   |    MPU6050           |  
#|  3、板载4颗按钮A、B、C、D     |    BUTTON            |
#|  4、光线强度传感器            |    LIGHTINTENSITY    |
#|  5、板载LED灯!                |    LED               |
#|  6、直流电机                  |    DCMOTOR           |
#|  7、模拟舵机                  |    SERVO             |
#|------------------------------------------------------|
#|  2023.02.27:                  |                      |
#|  【修复完善】                 |    BUTTON、LED       |
#|  【添加】                     |    WIFI              |
#|------------------------------------------------------|
#|  2023.03.09：                 |                      |
#|  【修复完善】                 |    LED               |
#|------------------------------------------------------|
#|  2023.03.14:                  |                      |
#|  【修复完善】                 |    SERVO             |
#|-------------------------------------------------------
#|  2023.06.01:                  |                      |
#|  【替换】                     |    BUTTON            |
#|  【替换】                     |    LED               |
#|  【替换】                     |    DCMOTOR           |
#|  【替换】                     |    SERVO             |
#|  【添加】                     |    RGB               |
#|  【添加】                     |    ADC               |
#|  【添加】                     |    DAC               |
#|  【添加】                     |    INPUT             |
#|  【添加】                     |    OUT               |
#|  【添加】                     |    UvcVideo          |
#|-------------------------------------------------------
#|  2023.06.15:                  |                      |
#|  【更改】                     |    STM8S             |
#|-------------------------------------------------------
#########################################################

from smbus2 import SMBus
import time
import math
from maix import gpio
import os
import SUNXI_GPIO
from maix import camera
import time
import sys
sys.path.append('/root/preset/server')

##################################
#     1、AHT20温湿度传感器       #
##################################

AHT20_I2CADDR = 0x38
AHT20_CMD_SOFTRESET = [0xBA]
AHT20_CMD_INITIALIZE = [0xBE, 0x08, 0x00]
AHT20_CMD_MEASURE = [0xAC, 0x33, 0x00]
AHT20_STATUSBIT_BUSY = 7                    # The 7th bit is the Busy indication bit. 1 = Busy, 0 = not.
AHT20_STATUSBIT_CALIBRATED = 3              # The 3rd bit is the CAL (calibration) Enable bit. 1 = Calibrated, 0 = not

class AHT20:
    # Usage: AHT20 crc8 checker. 
    # A total of 6 * 8 bits data need to check. G(x) = x8 + x5 + x4 + 1 -> 0x131(0x31), Initial value = 0xFF. No XOROUT. 
    N_DATA = 6
    # 1 * 8 bits CRC
    N_CRC = 1
    # Initial value. Equal to bit negation the first data (status of AHT20)
    INIT = 0xFF
    # Useful value to help calculate
    LAST_8_bit = 0xFF
    # Devide number retrieve from CRC-8 MAXIM G(x) = x8 + x5 + x4 + 1
    CRC_DEVIDE_NUMBER = 0x131
    
    # I2C communication driver for AHT20, using only smbus2
    def __init__(self, BusNum=2):
        # Initialize AHT20
        self.BusNum = BusNum
        self.cmd_soft_reset()

        # Check for calibration, if not done then do and wait 10 ms
        if not self.get_status_calibrated == 1:
            self.cmd_initialize()
            while not self.get_status_calibrated() == 1:
                time.sleep(0.01)
                
    def get_normalized_bit(self,value, bit_index):
        # Return only one bit from value indicated in bit_index
        return (value >> bit_index) & 1
        
    def cmd_soft_reset(self):
        # Send the command to soft reset
        with SMBus(self.BusNum) as i2c_bus:
            i2c_bus.write_i2c_block_data(AHT20_I2CADDR, 0x0, AHT20_CMD_SOFTRESET)
        time.sleep(0.04)    # Wait 40 ms after poweron
        return True

    def cmd_initialize(self):
        # Send the command to initialize (calibrate)
        with SMBus(self.BusNum) as i2c_bus:
            i2c_bus.write_i2c_block_data(AHT20_I2CADDR, 0x0 , AHT20_CMD_INITIALIZE)
        return True

    def cmd_measure(self):
        # Send the command to measure
        with SMBus(self.BusNum) as i2c_bus:
            i2c_bus.write_i2c_block_data(AHT20_I2CADDR, 0, AHT20_CMD_MEASURE)
        time.sleep(0.08)    # Wait 80 ms after measure
        return True

    def get_status(self):
        # Get the full status byte
        with SMBus(self.BusNum) as i2c_bus:
            return i2c_bus.read_i2c_block_data(AHT20_I2CADDR, 0x0, 1)[0]
        return True

    def get_status_calibrated(self):
        # Get the calibrated bit
        return self.get_normalized_bit(self.get_status(), AHT20_STATUSBIT_CALIBRATED)

    def get_status_busy(self):
        # Get the busy bit
        return self.get_normalized_bit(self.get_status(), AHT20_STATUSBIT_BUSY)
            
    def get_measure(self):
        # Get the full measure

        # Command a measure
        self.cmd_measure()

        # Check if busy bit = 0, otherwise wait 80 ms and retry
        while self.get_status_busy() == 1:
            time.sleep(0.08) # Wait 80 ns
        
        # TODO: do CRC check

        # Read data and return it
        with SMBus(self.BusNum) as i2c_bus:
            return i2c_bus.read_i2c_block_data(AHT20_I2CADDR, 0x0, 7)

    def mod2_division_8bits(self,a, b, number_of_bytes, init_value):
        "calculate mod2 division in 8 bits. a mod b. init_value is for crc8 init value."
        head_of_a = 0x80
        # Processiong a
        a = a << 8
        # Preprocessing head_of_a
        for i in range(0, number_of_bytes):
            head_of_a = head_of_a << 8
            b = b << 8
            init_value = init_value << 8
        a = a ^ init_value
        while (head_of_a > 0x80):
            # Find a 1
            if (head_of_a & a):
                head_of_a = head_of_a >> 1
                b = b >> 1
                a = a ^ b
            else:
                head_of_a = head_of_a >> 1
                b = b >> 1
            # This will show calculate the remainder
            # print("a:{0}\thead of a:{1}\tb:{2}".format(
            #     bin(a), bin(head_of_a), bin(b)))
        return a

    def AHT20_crc8_calculate(self,all_data_int):
        init_value = INIT
        # Preprocess all the data and CRCCode from AHT20
        data_from_AHT20 = 0x00
        # Preprocessing the first data (status)
        # print(bin(data_from_AHT20))
        for i_data in range(0, len(all_data_int)):
            data_from_AHT20 = (data_from_AHT20 << 8) | all_data_int[i_data]
        # print(bin(data_from_AHT20))
        mod_value = self.mod2_division_8bits(
            data_from_AHT20, CRC_DEVIDE_NUMBER, len(all_data_int), init_value)
        # print(mod_value)
        return mod_value


    def AHT20_crc8_check(self,all_data_int):
        """
        The input data shoule be:
        Status Humidity0 Humidity1 Humidity2|Temperature0 Temperature1 Temperature2 CRCCode.
        In python's int64.
        """
        mod_value = self.AHT20_crc8_calculate(all_data_int[:-1])
        if (mod_value == all_data_int[-1]):
            return True
        else:
            return False

    def get_measure_CRC8(self):
        """
        This function will calculate crc8 code with G(x) = x8 + x5 + x4 + 1 -> 0x131(0x31), Initial value = 0xFF. No XOROUT.
        return: all_data (1 bytes status + 2.5 byes humidity + 2.5 bytes temperature + 1 bytes crc8 code), isCRC8_pass
        """
        all_data = self.get_measure()
        isCRC8_pass = self.AHT20_crc8_check(all_data)

        return all_data, isCRC8_pass

    def get_temperature(self):
        # Get a measure, select proper bytes, return converted data
        measure = self.get_measure()
        measure = ((measure[3] & 0xF) << 16) | (measure[4] << 8) | measure[5]
        measure = measure / (pow(2,20))*200-50
        return measure
        
    def get_temperature_crc8(self):
        isCRC8Pass = False
        while (not isCRC8Pass): 
            measure, isCRC8Pass = self.get_measure_CRC8()
            time.sleep(80 * 10**-3)
        measure = ((measure[3] & 0xF) << 16) | (measure[4] << 8) | measure[5]
        measure = measure / (pow(2,20))*200-50
        return measure

    def get_humidity(self):
        # Get a measure, select proper bytes, return converted data
        measure = self.get_measure()
        measure = (measure[1] << 12) | (measure[2] << 4) | (measure[3] >> 4)
        measure = measure * 100 / pow(2,20)
        return measure

    def get_humidity_crc8(self):
        isCRC8Pass = False
        while (not isCRC8Pass): 
            measure, isCRC8Pass = self.get_measure_CRC8()
            time.sleep(80 * 10**-3)
        measure = (measure[1] << 12) | (measure[2] << 4) | (measure[3] >> 4)
        measure = measure * 100 / pow(2,20)
        return measure

##################################
#   2、MPU6050陀螺仪六轴传感器   #
##################################

SLAVE_ADDR = 0x68

PWR_MGMT_1 = 0x6B
PWR_MGMT_2 = 0x6C
WHO_AM_I = 0x75

GYRO_X = 0x43
GYRO_Y = 0x45
GYRO_Z = 0x47

ACCL_X = 0x3B
ACCL_Y = 0x3D
ACCL_Z = 0x3F

class MPU6050:
    def __init__(self, BusNum=2):
        # Initialize mpu6050
        self.BusNum = BusNum
        self.cmd_soft_reset()
        
    def cmd_soft_reset(self):
        # Send the command to soft reset
        with SMBus(self.BusNum) as bus:
            bus.write_byte_data(SLAVE_ADDR, PWR_MGMT_1, 0)
        time.sleep(0.04)    # Wait 40 ms after poweron

    def read_byte(self,addr):
        with SMBus(self.BusNum) as bus:
            return bus.read_byte_data(SLAVE_ADDR,addr)

    def read_word(self,addr):
        with SMBus(self.BusNum) as bus:
            h = bus.read_byte_data(SLAVE_ADDR, addr)
            l = bus.read_byte_data(SLAVE_ADDR, addr+1)
        val = (h << 8) + l
        return val

    def read_word_i2c(self,addr):
        val = self.read_word(addr)
        if (val >= 0x8000):
            return -((65535 - val) + 1)
        else:
            return val

    def dist(self,x, y):
        return math.sqrt((x*x) + (y*y))

    def get_x_rotat(self,x,y, z):
        rad = math.atan2(y,self.dist(x, z))
        return math.degrees(rad)

    def get_y_rotat(self,x, y, z):
        rad = math.atan2(x,self.dist(y, z))
        return -math.degrees(rad)

    def read_gyro(self):
        GYR_X = self.read_word_i2c(GYRO_X)
        GYR_Y = self.read_word_i2c(GYRO_Y)
        GYR_Z = self.read_word_i2c(GYRO_Z)

        #print ("GYRO -> X:{:04.2f} Y:{:04.2f} Z:{:04.2f}".format((GYR_X/131), (GYR_Y/131), (GYR_Z/131)))
        return (GYR_X/131), (GYR_Y/131), (GYR_Z/131)

    def read_acc(self):
        ACC_X = self.read_word_i2c(ACCL_X)
        ACC_Y = self.read_word_i2c(ACCL_Y)
        ACC_Z = self.read_word_i2c(ACCL_Z)

        CALC_ACC_X = ACC_X/16384.0
        CALC_ACC_Y = ACC_Y/16384.0
        CALC_ACC_Z = ACC_Z/16384.0

        #print ("ACCL -> X:{:04.2f} Y:{:04.2f} Z:{:04.2f}".format(CALC_ACC_X, CALC_ACC_Y, CALC_ACC_Z))
        #print("ROTATE -> X:{:04.2f} Y:{:04.2f}\n".format(get_x_rotat(CALC_ACC_X, CALC_ACC_Y, CALC_ACC_Z), get_y_rotat(CALC_ACC_X, CALC_ACC_Y, CALC_ACC_Z)))
        return CALC_ACC_X, CALC_ACC_Y, CALC_ACC_Z,self.get_x_rotat(CALC_ACC_X, CALC_ACC_Y, CALC_ACC_Z),self.get_y_rotat(CALC_ACC_X, CALC_ACC_Y, CALC_ACC_Z)
    
    def get_gyro_x(self):
        return self.read_gyro()[0]
    
    def get_gyro_y(self):
        return self.read_gyro()[1]
    
    def get_gyro_z(self):
        return self.read_gyro()[2]
    
    def get_acc_x(self):
        return self.read_acc()[0]
    
    def get_acc_y(self):
        return self.read_acc()[1]
    
    def get_acc_z(self):
        return self.read_acc()[2]
    
    def get_angle_x(self):
        return self.read_acc()[3]
    
    def get_angle_y(self):
        return self.read_acc()[4]

'''
##################################
#    3、板载4颗按钮A、B、C、D    #
##################################
class BUTTON:
    import os
    def __init__(self, gpioId):
        sendMsg_1='echo '
        sendMsg_2=' > /sys/class/gpio/export'
        sendMsg_3=' > /sys/class/gpio/unexport'
        sendMsg_4='echo "in" > /sys/class/gpio/gpio'
        sendMsg_5='/direction'
        sendMsg_6='/sys/class/gpio/gpio'
        sendMsg_7='/value'
        self.gpio=str(224+gpioId)
        self.msgStart=sendMsg_1+self.gpio+sendMsg_2
        self.msgMode=sendMsg_4+self.gpio+sendMsg_5
        self.msgDel=sendMsg_1+self.gpio+sendMsg_3
        self.msgGet=sendMsg_6+self.gpio+sendMsg_7

    def is_pressed(self):
        import os
        if(os.access(self.msgGet, os.F_OK) is False):
            os.system(self.msgStart)
        os.system(self.msgMode)
        with open(self.msgGet, "rb") as self.file:
            self.getValue=int(self.file.read())
        if self.getValue != 1:
            return True
        else:
            return False
        
    def __del__(self):
        os.system(self.msgDel)
'''

##################################
#    3、板载4颗按钮A、B、C、D    #
##################################
class BUTTON:
    def __init__(self, gpioId):
        self.gpio=224+gpioId
        SUNXI_GPIO.setcfg(self.gpio, SUNXI_GPIO.IN)

    def is_pressed(self):
        self.getValue=SUNXI_GPIO.input(self.gpio)
        if self.getValue != 1:
            return True
        else:
            return False

##################################
#       4、光线强度传感器        #
##################################
class LIGHTINTENSITY:
    def __init__(self, addr=b"0x05070080") -> None:
        self.addr = addr
        self.path = "/sys/class/sunxi_dump/dump"
        self.file = open(self.path, "wb+")
        self.last = self.value()
    def value(self):
        self.file.write(b"0x05070080")
        self.file.seek(0)
        return int(self.file.read()[:-1], 16)
    def __del__(self):
        try:
            if self.file:
                self.file.close()
                del self.file
        except Exception as e:
            pass
'''
##################################
#         5、板载LED灯           #
##################################
class LED:
    import os
    def __init__(self, gpioId=14):
        sendMsg_1='echo '
        sendMsg_2=' > /sys/class/gpio/export'
        sendMsg_3=' > /sys/class/gpio/unexport'
        sendMsg_4='echo "out" > /sys/class/gpio/gpio'
        sendMsg_5='/direction'
        self.sendMsg_6=' > /sys/class/gpio/gpio'
        self.sendMsg_7='/value'
        self.sendMsg_8='echo '
        self.gpio=str(224+gpioId)
        self.msgStart=sendMsg_1+self.gpio+sendMsg_2
        self.msgMode=sendMsg_4+self.gpio+sendMsg_5
        self.msgDel=sendMsg_1+self.gpio+sendMsg_3
        self.msgGet=self.sendMsg_6+self.gpio+self.sendMsg_7

    def out(self,value):
        import os
        os.system(self.msgStart)
        os.system(self.msgMode)
        if value==0 or value==1:
            self.value=value
        else:
            self.value=0
        self.msgSet=self.sendMsg_8+str(self.value)+self.sendMsg_6+self.gpio+self.sendMsg_7
        os.system(self.msgSet)

    def __del__(self):
        os.system(self.msgDel)
'''
##################################
#         5、板载LED灯           #
##################################
class LED:
    def __init__(self, gpioId=69):
        self.gpioId=gpioId
        SUNXI_GPIO.setcfg(self.gpioId, SUNXI_GPIO.OUT)
    def out(self,value):
        if value==0 or value==1:
            self.value=1-value
        else:
            self.value=0
        SUNXI_GPIO.output(self.gpioId,self.value)
        
'''
##################################
#          6、直流电机           #
##################################
class DCMOTOR:
    def __init__(self,dcMotorID="M1"):
        self.motorId=dcMotorID
        os.system('i2cset -y 2 0x40 0x00 0x00')         #初始化
        #设定频率freq=50,预分频prescale=int(25000000.0 / 4096.0 / freq + 0.5)
        os.system('i2cset -y 2 0x40 0x00 0x10')         #设定pca9685为睡眠模式
        os.system('i2cset -y 2 0x40 0xfe 0x7a')         #设定预分频
        os.system('i2cset -y 2 0x40 0x00 0x00')         #重新初始化
        time.sleep(0.01)
        os.system('i2cset -y 2 0x40 0x00 0xa1')         #设定pca9685为活跃模式
        
    def numberMap(self,value, leftMin, leftMax, rightMin, rightMax):
        leftSpan = leftMax - leftMin
        rightSpan = rightMax - rightMin
        valueScaled = float(value - leftMin) / float(leftSpan)
        return rightMin + (valueScaled * rightSpan)

    def dcMotorOffRegisterValue(self,dcMotorSetSpeed):
        dcMotorOffRegister = int(self.numberMap(dcMotorSetSpeed,0,255,0,4095))
        dcMotorOffRegister_L = hex(dcMotorOffRegister & 0xff)
        dcMotorOffRegister_H = hex((dcMotorOffRegister >> 8) & 0xff)
        return dcMotorOffRegister_L, dcMotorOffRegister_H
    
    def dcMotorControl(self,dcMotorSetSpeed=0,dcMotorSetRotationDirection=True):
        if self.motorId=="M1":
            if dcMotorSetRotationDirection == True:
                dcMotorOffRegisterValue6_L ='i2cset -y 2 0x40 0x20 '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[0])
                dcMotorOffRegisterValue6_H ='i2cset -y 2 0x40 0x21 '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[1])
                os.system('i2cset -y 2 0x40 0x1e 0x00') #设定6通道ON_L为0
                os.system('i2cset -y 2 0x40 0x1f 0x00') #设定6通道ON_H为0
                os.system(dcMotorOffRegisterValue6_L)   #设定6通道OFF_L为速度寄存器值低位
                os.system(dcMotorOffRegisterValue6_H)   #设定6通道OFF_H为速度寄存器值高位
                os.system('i2cset -y 2 0x40 0x22 0x00') #设定7通道ON_L为0
                os.system('i2cset -y 2 0x40 0x23 0x00') #设定7通道ON_H为0
                os.system('i2cset -y 2 0x40 0x24 0x00') #设定7通道OFF_L为0
                os.system('i2cset -y 2 0x40 0x25 0x00') #设定7通道OFF_H为0
            elif dcMotorSetRotationDirection == False:
                dcMotorOffRegisterValue7_L ='i2cset -y 2 0x40 0x24 '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[0])
                dcMotorOffRegisterValue7_H ='i2cset -y 2 0x40 0x25 '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[1])
                os.system('i2cset -y 2 0x40 0x1e 0x00') #设定6通道ON_L为0
                os.system('i2cset -y 2 0x40 0x1f 0x00') #设定6通道ON_H为0
                os.system('i2cset -y 2 0x40 0x20 0x00') #设定6通道OFF_L为0
                os.system('i2cset -y 2 0x40 0x21 0x00') #设定6通道OFF_H为0
                os.system('i2cset -y 2 0x40 0x22 0x00') #设定7通道ON_L为0
                os.system('i2cset -y 2 0x40 0x23 0x00') #设定7通道ON_H为0
                os.system(dcMotorOffRegisterValue7_L)   #设定7通道OFF_L为速度寄存器值低位
                os.system(dcMotorOffRegisterValue7_H)   #设定7通道OFF_H为速度寄存器值高位
            else:
                pass
        elif self.motorId=="M2":
            if dcMotorSetRotationDirection == True:
                dcMotorOffRegisterValue9_L ='i2cset -y 2 0x40 0x2c '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[0])
                dcMotorOffRegisterValue9_H ='i2cset -y 2 0x40 0x2d '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[1])
                os.system('i2cset -y 2 0x40 0x2a 0x00') #设定9通道ON_L为0
                os.system('i2cset -y 2 0x40 0x2b 0x00') #设定9通道ON_H为0
                os.system(dcMotorOffRegisterValue9_L)   #设定9通道OFF_L为速度寄存器值低位
                os.system(dcMotorOffRegisterValue9_H)   #设定9通道OFF_H为速度寄存器值高位
                os.system('i2cset -y 2 0x40 0x2e 0x00') #设定10通道ON_L为0
                os.system('i2cset -y 2 0x40 0x2f 0x00') #设定10通道ON_H为0
                os.system('i2cset -y 2 0x40 0x30 0x00') #设定10通道OFF_L为0
                os.system('i2cset -y 2 0x40 0x31 0x00') #设定10通道OFF_H为0
            elif dcMotorSetRotationDirection == False:
                dcMotorOffRegisterValue10_L ='i2cset -y 2 0x40 0x30 '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[0])
                dcMotorOffRegisterValue10_H ='i2cset -y 2 0x40 0x31 '+str(self.dcMotorOffRegisterValue(dcMotorSetSpeed)[1])
                os.system('i2cset -y 2 0x40 0x2a 0x00') #设定9通道ON_L为0
                os.system('i2cset -y 2 0x40 0x2b 0x00') #设定9通道ON_H为0
                os.system('i2cset -y 2 0x40 0x2c 0x00') #设定9通道OFF_L为0
                os.system('i2cset -y 2 0x40 0x2d 0x00') #设定9通道OFF_H为0
                os.system('i2cset -y 2 0x40 0x2e 0x00') #设定10通道ON_L为0
                os.system('i2cset -y 2 0x40 0x2f 0x00') #设定10通道ON_H为0
                os.system(dcMotorOffRegisterValue10_L)  #设定10通道OFF_L为速度寄存器值低位
                os.system(dcMotorOffRegisterValue10_H)  #设定10通道OFF_H为速度寄存器值高位
            else:
                pass
        else:
            pass

    def __del__(self):
        if self.motorId=="M1":
            os.system('i2cset -y 2 0x40 0x1e 0x00') #设定6通道ON_L为0
            os.system('i2cset -y 2 0x40 0x1f 0x00') #设定6通道ON_H为0
            os.system('i2cset -y 2 0x40 0x20 0x00') #设定6通道OFF_L为0
            os.system('i2cset -y 2 0x40 0x21 0x00') #设定6通道OFF_H为0
            os.system('i2cset -y 2 0x40 0x22 0x00') #设定7通道ON_L为0
            os.system('i2cset -y 2 0x40 0x23 0x00') #设定7通道ON_H为0
            os.system('i2cset -y 2 0x40 0x24 0x00') #设定7通道OFF_L为0
            os.system('i2cset -y 2 0x40 0x25 0x00') #设定7通道OFF_H为0
        elif self.motorId=="M2":
            os.system('i2cset -y 2 0x40 0x2a 0x00') #设定9通道ON_L为0
            os.system('i2cset -y 2 0x40 0x2b 0x00') #设定9通道ON_H为0
            os.system('i2cset -y 2 0x40 0x2c 0x00') #设定9通道OFF_L为0
            os.system('i2cset -y 2 0x40 0x2d 0x00') #设定9通道OFF_H为0
            os.system('i2cset -y 2 0x40 0x2e 0x00') #设定10通道ON_L为0
            os.system('i2cset -y 2 0x40 0x2f 0x00') #设定10通道ON_H为0
            os.system('i2cset -y 2 0x40 0x30 0x00') #设定10通道OFF_H为0
            os.system('i2cset -y 2 0x40 0x31 0x00') #设定10通道OFF_L为0
        else:
            pass
'''

'''
##################################
#          7、模拟舵机           #
##################################
class SERVO:
    def __init__(self,servoID="S1"):
        self.servoId=servoID
        os.system('i2cset -y 2 0x40 0x00 0x00')     #初始化
        #设定频率freq=50,预分频prescale=int(25000000.0 / 4096.0 / freq + 0.5)
        os.system('i2cset -y 2 0x40 0x00 0x10')     #设定pca9685为睡眠模式
        os.system('i2cset -y 2 0x40 0xfe 0x7a')     #设定预分频
        os.system('i2cset -y 2 0x40 0x00 0x00')     #重新初始化
        time.sleep(0.01)
        os.system('i2cset -y 2 0x40 0x00 0xa1')     #设定pca9685为活跃模式

    def servoOffRegisterValue(self,servoSetDegree):
        servoPwmus = (servoSetDegree * 2000 / 180 + 500)    # 0.6 ~ 2.4——【2023.03.14更改：0.5~2.5】
        servoOffRegister = int(servoPwmus * 4096 / 20000)
        servoOffRegister_L = hex(servoOffRegister & 0xff)
        servoOffRegister_H = hex((servoOffRegister >> 8) & 0xff)
        return servoOffRegister_L, servoOffRegister_H

    def servoControl(self,servoSetDegree=0):
        if self.servoId=="S1":
            servoOffRegisterCmd5_L ="i2cset -y 2 0x40 0x1c "+str(self.servoOffRegisterValue(servoSetDegree)[0])
            servoOffRegisterCmd5_H ="i2cset -y 2 0x40 0x1d "+str(self.servoOffRegisterValue(servoSetDegree)[1])
            os.system('i2cset -y 2 0x40 0x1a 0x00') #设定5通道ON_L为0
            os.system('i2cset -y 2 0x40 0x1b 0x00') #设定5通道ON_H为0
            os.system(servoOffRegisterCmd5_L)       #设定5通道OFF_L为
            os.system(servoOffRegisterCmd5_H)       #设定5通道OFF_H为
        elif self.servoId=="S2":
            servoOffRegisterCmd11_L ="i2cset -y 2 0x40 0x34 "+str(self.servoOffRegisterValue(servoSetDegree)[0])
            servoOffRegisterCmd11_H ="i2cset -y 2 0x40 0x35 "+str(self.servoOffRegisterValue(servoSetDegree)[1])
            os.system('i2cset -y 2 0x40 0x32 0x00') #设定11通道ON_L为0
            os.system('i2cset -y 2 0x40 0x33 0x00') #设定11通道ON_H为0
            os.system(servoOffRegisterCmd11_L)      #设定11通道OFF_L为
            os.system(servoOffRegisterCmd11_H)      #设定11通道OFF_H为
        else:
            pass

    def __del__(self):
        time.sleep(0.01)
'''        

##################################
#          7、QMI8658            #
##################################
class QMI8658(object):
    def __init__(self, smbus=2, address=0X6B):
        self._address = address
        import smbus2
        self._bus = smbus2.SMBus(smbus)
        bRet = self.WhoAmI()
        if bRet:
            self.Read_Revision()
        else:
            return None
        self.Config_apply()

    def _read_byte(self, cmd):
        rec = self._bus.read_i2c_block_data(int(self._address), int(cmd), 1)
        return rec[0]

    def _read_block(self, reg, length=1):
        rec = self._bus.read_i2c_block_data(int(self._address), int(reg), length)
        return rec

    def _read_u16(self, cmd):
        LSB = self._bus.read_i2c_block_data(int(self._address), int(cmd), 1)
        MSB = self._bus.read_i2c_block_data(int(self._address), int(cmd)+1, 1)
        return (MSB[0] << 8) + LSB[0]

    def _write_byte(self, cmd, val):
        self._bus.write_i2c_block_data(int(self._address), int(cmd), bytes([int(val)]))

    def WhoAmI(self):
        bRet = False
        if (0x05) == self._read_byte(0x00):
            bRet = True
        return bRet

    def Read_Revision(self):
        return self._read_byte(0x01)

    def Config_apply(self):
        # REG CTRL1
        self._write_byte(0x02, 0x60)
        # REG CTRL2 : QMI8658AccRange_8g  and QMI8658AccOdr_1000Hz
        self._write_byte(0x03, 0x23)
        # REG CTRL3 : QMI8658GyrRange_512dps and QMI8658GyrOdr_1000Hz
        self._write_byte(0x04, 0x53)
        # REG CTRL4 : No
        self._write_byte(0x05, 0x00)
        # REG CTRL5 : Enable Gyroscope And Accelerometer Low-Pass Filter
        self._write_byte(0x06, 0x11)
        # REG CTRL6 : Disables Motion on Demand.
        self._write_byte(0x07, 0x00)
        # REG CTRL7 : Enable Gyroscope And Accelerometer
        self._write_byte(0x08, 0x03)

    def Read_Raw_XYZ(self):
        xyz = [0, 0, 0, 0, 0, 0]
        raw_timestamp = self._read_block(0x30, 3)
        raw_acc_xyz = self._read_block(0x35, 6)
        raw_gyro_xyz = self._read_block(0x3b, 6)
        raw_xyz = self._read_block(0x35, 12)
        timestamp = (raw_timestamp[2] << 16) | (
            raw_timestamp[1] << 8) | (raw_timestamp[0])
        for i in range(6):
            # xyz[i]=(raw_acc_xyz[(i*2)+1]<<8)|(raw_acc_xyz[i*2])
            # xyz[i+3]=(raw_gyro_xyz[((i+3)*2)+1]<<8)|(raw_gyro_xyz[(i+3)*2])
            xyz[i] = (raw_xyz[(i*2)+1] << 8) | (raw_xyz[i*2])
            if xyz[i] >= 32767:
                xyz[i] = xyz[i]-65535
        return xyz

    def Read_XYZ(self):
        xyz = [0, 0, 0, 0, 0, 0]
        raw_xyz = self.Read_Raw_XYZ()
        #QMI8658AccRange_8g
        acc_lsb_div = (1 << 12)
        #QMI8658GyrRange_512dps
        gyro_lsb_div = 64
        for i in range(3):
            xyz[i] = raw_xyz[i]/acc_lsb_div  # (acc_lsb_div/1000.0)
            xyz[i+3] = raw_xyz[i+3]*1.0/gyro_lsb_div
        return xyz
        
import smbus2
import time

class stm8s(object):

    bus = smbus2.SMBus(2) # 2 indicates /dev/i2c-2
    address = 0x50

    # 0x00 # 触发配置
    # 0x01 # 重置配置
    # 0x02 # pwm0 历史配置
    # 0x03 # pwm1 历史配置

    # 引脚配置模式有 1. pwm 2. gpio ouput gpio input 3. adc 4.ws2812_singe 5 ws2812_multi
    def __init__(self):
        self.reset()
        time.sleep(0.05)
        pass
    
    def clear(self):
        self.write(1, 1)
        self.reset()
        time.sleep(0.05)
    
    def write(self, addr, val):
        for i in range(0, 3):
            try:
                self.bus.write_byte_data(self.address, addr, val)
                time.sleep(0.001) # 1ms
                # print(addr, val) # debug
                return True
            except Exception:
                time.sleep(0.01)
                continue
        return False

    def read(self, addr):
        for i in range(0, 3):
            try:
                tmp = self.bus.read_byte_data(self.address, addr)
                time.sleep(0.001) # 1ms
                # print(addr, tmp) # debug
                return tmp
            except Exception:
                time.sleep(0.01)
                continue
        return None

    def reset(self):
        self.write(0, 1)
        time.sleep(0.05) # 重启并配置需要时间

    def dump(self):
        for i in range(0, 32):
            print(i, self.read(i))

class singleRgb(stm8s):
    def __init__(self):
        self.valR=0
        self.valG=0
        self.valB=0
        self.brightness=255
        self.show()
    
    def setColor(self,r,g,b):
        if(r>=0 and r<=255):
            self.valR=int(r*self.brightness/255)
        else:
            self.valR=0
        if(g>=0 and g<=255):
            self.valG=int(g*self.brightness/255)
        else:
            self.valG=0
        if(b>=0 and b<=255):
            self.valB=int(b*self.brightness/255)
        else:
            self.valB=0
            
    def setBrightness(self,brightness):
        if(brightness>=0 and brightness<=255):
            self.brightness=brightness
        else:
            self.brightness=0
    
    def show(self):
        self.write(31, self.valB)
        self.write(30, self.valG)
        self.write(29, self.valR)
        self.write(28, 6)

class dcMotor(stm8s):
    def __init__(self,id):
        self.id=id

    def m1a(self,val=0):
        self.write(4,5)    #M1A
        self.write(5,0)
        self.write(6,0)
        self.write(7,val)
        
    def m1b(self,val=0):
        self.write(8,5)    #M1B
        self.write(9,0)
        self.write(10,0)
        self.write(11,val)
        
    def m2a(self,val=0):
        self.write(12,5)    #M2A
        self.write(13,0)
        self.write(14,0)
        self.write(15,val)
        
    def m2b(self,val=0):
        self.write(16,5)    #M2B
        self.write(17,0)
        self.write(18,0)
        self.write(19,val)
        
    def dcMotorCtrl(self,dir,speed):
        if(0<=speed and speed<=255):
            if(self.id ==1):                #Motor1
                if(dir==0):
                    self.m1a(speed)
                    self.m1b(0)
                else:
                    self.m1a(255-speed)
                    self.m1b(1)
            elif(self.id ==2):
                if(dir==0):
                    self.m2a(speed)
                    self.m2b(0)
                else:
                    self.m2a(255-speed)
                    self.m2b(1)
        else:
            pass
            
class multiFuncGpio(stm8s):
    def __init__(self,id=0,mode=0):
        self.id=id
        self.rgbId=0
        self.brightness=255
        self.mode=mode
        self.write(20+self.id*4,self.mode)
        self.reset()
        time.sleep(0.05)
    ##################################################
    # mode |  0  |  1  |  2  |  3  |  4  |  5  |  6  |      
    #-------------------------------------------------
    # Func |free |servo|INPUT| OUT | ADC | PWM | BEEP|
    ##################################################
    # mode=0，为空闲状态，引脚不执行任何功能
    # mode=1，为控制舵机模式，值控制范围为0°~180°
    # mode=2，为数字输入模式，返回值0或1
    # mode=3，为数字输出模式，输出值为0或1
    # mode=4，为ADC采样模式，返回值为0~1023
    # mode=5，为PWM输出模式，输出值范围为0~255
    # mode=6，为控制蜂鸣器模式，控制范围为20hz~12000hz
    # mode=7，为控制灯带模式，暂未实现
    # id=0,控制多功能引脚1，向20号寄存器写入模式数据
    # id=1,控制多功能引脚2，向24号寄存器写入模式数据
    
    def servoCtrl(self,angle):
        if ((angle>=0) and (angle<=180)):
            self.write(23+self.id*4,angle)
        else:
            pass
            
    def digitalRead(self):
        try:
            self.gpioVal= self.read(23+ self.id*4)
            if(self.gpioVal != None):
                return 1-self.gpioVal
        except:
            print("ERROR 233")
    
    def digitalWrite(self,val):
        if ((val>=0) and (val<=1)):
            self.write(23+self.id*4,val)
        else:
            pass
            
    def analogRead(self):
        try:
            self.adcValH= self.read(22+ self.id*4)
            self.adcValL= self.read(23+ self.id*4)
            if (self.adcValH != None and self.adcValL != None):
            # adcValH adcValL 大小端合并 16bit
                self.adcVal = self.adcValL + (self.adcValH << 8)
                return self.adcVal
        except:
            print("ERROR 2333")
        
    def analogWrite(self,val):
        if ((val>=0) and (val<=255)):
            self.write(23+self.id*4,val)
        else:
            pass
            
    
    def beep(self,frequency):
        if ((val>=20) and (val<=12000)):
            self.write(22+self.id*4,val>>8)
            self.write(23+self.id*4,val&0b11111111)
        else:
            pass
        
    def pixelInit_(self):
        for i in range(16):
            self.setPixelColor(i,0,0,0)
            self.pixelShow()
    
    def setBrightness(self,brightness):
        if(brightness>=0 and brightness<=255):
            self.brightness=brightness
        else:
            self.brightness=0
    
    def setPixelColor(self,rgbId,r,g,b):
        if(rgbId >=0 and rgbId <=15):
            self.rgbId=rgbId    #rgbId范围：0~15
        else:
            self.rgbId=0
        if(r>=0 and r<=255):
            self.valR=int(r*self.brightness/255)
        else:
            self.valR=0
        if(g>=0 and g<=255):
            self.valG=int(g*self.brightness/255)
        else:
            self.valG=0
        if(b>=0 and b<=255):
            self.valB=int(b*self.brightness/255)
        else:
            self.valB=0
    
    #self.id的值为0或1，用于控制灯带缓冲区（S1或者S2引脚）
    def pixelShow(self):
        self.write(32+ self.id*48+self.rgbId*3, self.valR)    
        self.write(33+ self.id*48+self.rgbId*3, self.valG)
        self.write(34+ self.id*48+self.rgbId*3, self.valB)
        

class UvcVideo(camera.MaixVideo):

    def __init__(self, source="/dev/videoX"):
        self.source = source
        super(UvcVideo, self).__init__()
        import os, time
        usb_path = '/sys/devices/platform/soc/usbc0/otg_role'
        # 确认 usb_path 内容为 usb_host 如果不是，自动设置为 usb_host
        if os.popen('cat %s' % usb_path).read().strip() != 'usb_host':
            os.system('echo "usb_host" > %s' % usb_path)
            time.sleep(2) # 要设置 2s 左右才能初始化工作，否则会报 VIDIOC_S_FMT 失败。

    def config(self, size=None, video=2, horizontal=0, vertical=0):
        if size == None:
            size = (320, 240)
        super(UvcVideo, self).config(size)
        print('[camera] config input size(%d, %d, %d)' %
            (self.width(), self.height(), video))
        if self.cam:
            self.cam = None
        try:
            import _coco_camera
            self.cam = _coco_camera.Camera(self.width(), self.height(), video, horizontal, vertical)
        except Exception as e:
            print(e)
            self.cam = None

    def read(self):
        if self.cam == None:
            self.config()
        if self.cam:
            ret, frame = self.cam.read()
            if ret:
                return frame  # bytes
            else:
                try:
                    self.config()
                except Exception as e:
                    print(e)
        return None

    def __del__(self):
        if self.cam:
            self.cam = None