【二级项目】利用树莓派(以及Orange pi)陀螺仪控制Unity制作体感游戏玩具


00:00/00:00

项目前提

原本是想要打造一个类似任天堂Switch的双人对战体感游戏,但是最后变成制作单人体感游戏了。不过技术差不多是类似的。

Raspberry pi(树莓派)是一款微型的卡片式电脑,基于ARM(ARM也是单片机!确信!),能运行Linux系统,虽然项目原本要求采用单片机。至于为什么用树莓....因为我个人非常讨厌C,能用python为啥不用呢!

MPU6050是一款非常常用的陀螺仪/加速度计 模块,总之四轴非常常用。它和控制板之间采取I2C通信。Raspberry本身就带有I2C支持,这和STM32,51之类的是一样的(所以才是创客神器嘛!)

这个项目有很多技术来源于:http://atceiling.blogspot.jp/2017/02/raspberry-pi-mpu-6050.html

接线

I2C总线是由Philips公司开发的一种简单、双向二线制同步串行总线。它只需要两根线即可在连接于总线上的器件之间传送信息。

MPU-6050模块 Raspberry pi引脚
VCC 1
SDA 3
SCL 5
GND 6

这里需要注意的是,树莓派的GPIO口的排列顺序是

1 2

3 4

5 6..

跟以前用过的芯片都不太一样,这点要注意。给MPU6050供电采用的是3.3V

开启树莓派I2C

# sudo raspi-config

进入系统后,输入以上指令进入树莓派设置。选择5->I2C打开I2C,也不知道为啥我的raspiconfig和别人不太一样,结果花了很长时间,偶然才找到了。因为不在root模式下,所以采取sudo获取权限进行命令。

$ sudo apt-get install i2c-tools

设置好I2C后,首先需要进行一轮测试,这个软件其实我的树莓买来的时候已经帮忙装好了。

$ sudo i2cdetect -y 1

这是利用i2c-tools测试i2c设备,当显示68的时候,证明I2c正常。

python-GPIO并不能读取I2c,所以必须要下载python读取I2c的程序:

$ sudo apt-get install python-smbus

完成以上步骤,python就能正常读取I2C了,接下来进行下一步陀螺仪的调试:

#!/usr/bin/python
import smbus
import math

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

bus = smbus.SMBus(1) 
address = 0x68       # This is the address value read via the i2cdetect command

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

print "gyro data"
print "---------"

gyro_xout = read_word_2c(0x43)
gyro_yout = read_word_2c(0x45)
gyro_zout = read_word_2c(0x47)

print "gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131)
print "gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131)
print "gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131)

print
print "accelerometer data"
print "------------------"

accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)

accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0

print "accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled
print "accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled
print "accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled

print "x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
print "y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

运行后发现,程序通过I2C从MPU6050模块中读取了0x43-0x47,0x3b-0x3d等6个内存地址(这就是六轴陀螺仪加速度计的六轴输出!)然后通过姿势解算,获得了陀螺仪的X,Y角度(亏我还研究了很久如何解算姿势)

接下来就要考虑,怎么把X,Y传出去。

增加TCP通信

TCP通信是非常熟悉的内容了~不过,这也是我第一次做Python的TCP通信,上次的其实是伟斌做的。

添加后的 结果就像这样~

#i/usr/bin/env python
# -*- coding: utf-8 -*-
#Programmer: APPIX
#Date 2017-09-27

import socket
import threading
import time
import smbus
import math

#电源管理寄存器
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

#MPU6050数据处理
def read_byte(adr):
    return bus.read_byte_data(address,adr)

def read_word(adr):
    high = bus.read_byte_data(address,adr)
    low = bus.read_byte_data(address,adr+1)
    val = (high << 8)+low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val)+1)
    else:
        return val

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x,dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y,dist(x,z))
    return math.degrees(radians)

#tcp循环读取陀螺仪数据并发送到客户端
def tcplink(sock,addr):
    print('Accept new connection from %s,%s...' % addr);
   # sock.send(b'Welcome!!!');
   # bus = smbus.SMBus(1)
    while True:
        address=0x68
        bus.write_byte_data(address,power_mgmt_1,0)
        gyro_xout = read_word_2c(0x43)
        gyro_yout = read_word_2c(0x45)
        gyro_zout = read_word_2c(0x47)
        accel_xout = read_word_2c(0x3b)
        accel_yout = read_word_2c(0x3d)
        accel_zout = read_word_2c(0x3f)

        accel_xout_scaled = accel_xout / 16384.0
        accel_yout_scaled = accel_yout / 16384.0
        accel_zout_scaled = accel_zout / 16384.0
        outxr = get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
        outyr = get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
        outstr= '%.2f;%.2f;' %(outxr,outyr)
        print outstr
        if (outxr<=-70):
            break;
        sock.send(b'%s' %outstr)
        time.sleep(0.1);
       # data = sock.recv(1024);
       # time.sleep(1);
       # if not data or data.decode('utf-8')=='exit':
       #     break;
       # sock.send(b'  sendsucceed>>%s\n' %data);
       # print('client>> %s' %data);


    sock.close();
    print('Connection from %s:%s closed' % addr);

bus = smbus.SMBus(1)

address=0x68
if __name__=="__main__":
    s = socket.socket(socket.AF_INET,socket.SOCK_STREAM);
    s.bind(('192.168.191.2',9090));
    s.listen(8);
    print("waiting for connection...");

    while True:
        sock,addr=s.accept();
        t = threading.Thread(target=tcplink,args=(sock,addr));
        t.start();
        break;

通过对Python语言TCP通信的研究,将程序改成这样。那就和以前一一样,将陀螺仪的数据以“x;y;”格式发到TCP上了~

软件客户端接收TCP数据

将以前图象识别项目用过的程序稍微改了改,变成了通过TCP收到的角度改变3D模型的欧拉角的程序。

点击这里查看该项目细节

测试效果如下:

 

制作Unity端适宜的游戏范例

做了两个,一个是飞机游戏,另一个是New game动画里宁宁做的平衡球。

移植到Orange Pi zero

弄了块国产的开发板~59块钱买不了吃亏买不了上当。叫香橙派zero,CPU是全志H2,这板子便宜是便宜,实际使用发热也可以接受,建议买256mb的版本,因为我买的512性能太好了没必要......但是,这板子给的资料真的很少.....非常少,官方给的系统甚至wifi都不能用,刷了第三方的armbian系统。

armbian下载地址

采用TTL连接到电脑,默认账号root密码1234,第一次登陆要改密码。

处理了很久的wifi问题,这板子要用

nmtui

来连接wifi,其他的都和树莓一样了,不过这板子的板载wifi也有问题,虽然连得上,但是偶尔会有不接受发送数据的情况,就ping不通。安装i2ctools和python.smbus

利用i2cdetect -a

查看,发现该设备有两个i2c口,连接MPU6050后,发现是接在0口的。利用nano粘贴之前在树莓做好的服务端代码,注意,因为i2c接口不同,源程序修改为:

bus = smbus.SMBus(0)

从0口读取数据。用TCP调试助手作为客户端连接orange pi,成功输出数据。

最后,将orange pi zero(用快递箱)包装成飞机的样子(真丑!不管了!),客户端换成我们用Unity制作的体感飞机游戏运行。

这就是最终的结果啦~

扩展

新增卡尔曼滤波

# -*- coding=utf-8 -*-
import numpy as np
#初始化数据
now=np.zeros(3) #当前观测值
#初始化卡尔曼滤波存放矩阵
xhat=np.zeros(3) 		#后向估计
P=np.zeros(3)	 		#后向误差
xhatminus=np.zeros(3)	#先验概率
Pminus=np.zeros(3)		#先验误差
K=np.zeros(3)
Q=np.array([1e-5,1e-5,1e-5]) #协方差
R=np.array([0.1**2,0.1**2,0.1**2])
#初始猜测值
P=[1.,1.,1.]
#卡尔曼滤波器,在实际使用中,每一个通信循环跑一次,修正三轴加速度计计算结果
for i in range(1,100):
  if(i<50):
    now=[0.2,0.2,0.2]+np.random.normal(1.,0.1,3)
  else:
    now=[1.2,1.2,1.2]+np.random.normal(1.,0.1,3)
  xhatminus=xhat
  Pminus=P+Q
  K=Pminus/(Pminus+R)
  xhat=xhatminus+K*(now-xhatminus)
  print "当前测量值"+str(now)+"|滤波结果"+str(xhat)
  P=(1-K)*Pminus

 


想要成为自己的未来