190919_imu_test2.py

190919_imu_test2.py

from machine import I2C
i2c = I2C(I2C.I2C0, freq=100000, scl=28, sda=29)
devices = i2c.scan()
print(devices)

import lcd  #for test
AXP192_ADDR=0x34
Backlight_ADDR=0x91
level=50
val = (level+7) << 4
i2c.writeto_mem(AXP192_ADDR, Backlight_ADDR,int(val))


import KPU as kpu
import sensor, image, time,lcd,machine
from machine import I2C
MPU6886_ADDRESS=0x68
MPU6886_WHOAMI=0x75
MPU6886_ACCEL_INTEL_CTRL=  0x69
MPU6886_SMPLRT_DIV=0x19
MPU6886_INT_PIN_CFG=   0x37
MPU6886_INT_ENABLE=0x38
MPU6886_ACCEL_XOUT_H=  0x3B
MPU6886_TEMP_OUT_H=0x41
MPU6886_GYRO_XOUT_H=   0x43
MPU6886_USER_CTRL= 0x6A
MPU6886_PWR_MGMT_1=0x6B
MPU6886_PWR_MGMT_2=0x6C
MPU6886_CONFIG=0x1A
MPU6886_GYRO_CONFIG=   0x1B
MPU6886_ACCEL_CONFIG=  0x1C
MPU6886_ACCEL_CONFIG2= 0x1D
MPU6886_FIFO_EN=   0x23

i2c = I2C(I2C.I2C0, freq=100000, scl=28, sda=29)

AXP192_ADDR=0x34
Backlight_ADDR=0x91

i2c.writeto_mem(AXP192_ADDR, Backlight_ADDR,0xa0)  # maximum


def write_i2c(address, value):
    i2c.writeto_mem(MPU6886_ADDRESS, address, bytearray([value]))
    time.sleep_ms(10)

write_i2c(MPU6886_PWR_MGMT_1, 0x00)
write_i2c(MPU6886_PWR_MGMT_1, 0x01<<7)
write_i2c(MPU6886_PWR_MGMT_1,0x01<<0)
write_i2c(MPU6886_ACCEL_CONFIG,0x10)
write_i2c(MPU6886_GYRO_CONFIG,0x18)
write_i2c(MPU6886_CONFIG,0x01)
write_i2c(MPU6886_SMPLRT_DIV,0x05)
write_i2c(MPU6886_INT_ENABLE,0x00)
write_i2c(MPU6886_ACCEL_CONFIG2,0x00)
write_i2c(MPU6886_USER_CTRL,0x00)
write_i2c(MPU6886_FIFO_EN,0x00)
write_i2c(MPU6886_INT_PIN_CFG,0x22)
write_i2c(MPU6886_INT_ENABLE,0x01)


fm.register(board_info.BUTTON_A, fm.fpioa.GPIO1)
but_a=GPIO(GPIO.GPIO1, GPIO.IN, GPIO.PULL_UP)

fm.register(board_info.BUTTON_B, fm.fpioa.GPIO2)
but_b = GPIO(GPIO.GPIO2, GPIO.IN, GPIO.PULL_UP)

but_a_pressed = 0
but_b_pressed = 0





def read_imu():
    aRes=255/4096/2
    offset=128
    accel = i2c.readfrom_mem(MPU6886_ADDRESS, MPU6886_ACCEL_XOUT_H, 6)
    accel_x = (accel[0]<<8|accel[1])
    accel_y = (accel[2]<<8|accel[3])
    accel_z = (accel[4]<<8|accel[5])
    if accel_x>32768:
        accel_x=accel_x-65536
    if accel_y>32768:
        accel_y=accel_y-65536
    if accel_z>32768:
        accel_z=accel_z-65536
    ax=int(accel_x*aRes+offset)
    if ax<0: ax=0
    if ax>255: ax=255
    ay=int(accel_y*aRes+offset)
    if ay<0: ay=0
    if ay>255: ay=255
    az=int(accel_z*aRes+offset)
    if az<0: az=0
    if az>255: az=255
    accel_array = [ay,az,ax]
#    print(accel_array);
#    time.sleep_ms(1)
    return accel_array

cnt=0
mode=0
save_flg=0
pic_no=0
accel_array_zero=(0,0,0)


w_size=14
lcd.init()
dummyImage = image.Image()
dummyImage = dummyImage.resize(w_size, w_size)
image_data_array = []

task = kpu.load(0x00300000)

while(True):
    accel_array = read_imu()
    w=cnt%w_size
    h=int(cnt/w_size)
    dummyImage.set_pixel(w, h, accel_array)

    img3=dummyImage.resize(130,130)

    dummyImage.pix_to_ai()
    fmap=kpu.forward(task,dummyImage)

    plist=fmap[:]
    pmax=max(plist)
    max_index=plist.index(pmax)
    print(max_index,"_",pmax)
    #lcd.draw_string(50,50,"%d: %.3f"%(max_index,pmax),scale=2,lcd.WHITE,lcd.BLACK)

    if max_index==0:
        img3.draw_string(0,50,"sit",color=(200,200,200), scale=2)
    elif max_index==1:
        img3.draw_string(0,50,"walk",color=(200,200,200), scale=2)
    elif max_index==2:
        img3.draw_string(0,50,"USA!USA!",color=(200,200,200), scale=2)
    elif max_index==3:
        img3.draw_string(0,50,"Upper", color=(200,200,200),scale=2)
    elif max_index==4:
        img3.draw_string(0,50,"Sleep",color=(200,200,200), scale=2)
    lcd.display(img3)
    cnt=cnt+1
    if cnt>dummyImage.width()*dummyImage.height():
        cnt=0
        pic_no+=1