利用Arduino和ADXL345加速度计测量实时倾角并图形化处理

最新推荐文章于 2024-05-01 13:49:44 发布

leehyukshuai

最新推荐文章于 2024-05-01 13:49:44 发布

阅读量1.2k

点赞数 3

分类专栏： C 文章标签：单片机 python

本文链接：https://blog.csdn.net/leehyukshuai/article/details/128495073

版权

C 专栏收录该内容

17 篇文章 0 订阅

订阅专栏

本文重点说明部分是：I2C串口通信在Arduino平台的使用；利用旋转矩阵进行线性变换。

I2C串口通信在Arduino平台的使用

基础认识

既然是多个部件相互通信，那就首先讲一下I2C通讯的原理（但我讲的并不好，所以引用一下我认为把I2C讲的很透彻的博文链接：How I2C Communication Works & How To Use It with Arduino (howtomechatronics.com)https://howtomechatronics.com/tutorials/arduino/how-i2c-communication-works-and-how-to-use-it-with-arduino/

单字节/多字节读写操作

相信看完这篇文章之后，各位已经对I2C通讯的原理有了一个基本的认识。不过，上述文章美中不足的地方在于对于主从之间单字节或多字节读写方法的讲述并不清晰，问题代码如下：

这个地方，原作者希望读取X_Axis_Register_DATAX0和X_Axis_Register_DATAX1两个寄存器的数据，因此，他在beginTransmission和endTransmission之间向加速度计发送了两个寄存器的地址，并用requestFrom函数读取两个字节。乍一看，这似乎是正确的，但是，按照这种思路，我们把两个寄存器请求的顺序更改一下，先请求DATAX1再请求DATAX2：

但是会发现x1, x2结果完全不正确。问题的原因就在于原文章对I2C多字节读的方式没有正确理解。

如图，想要进行多字节读，只需要发送目标数据的首地址即可，随后读取到的内容就是R[ADDR],R[ADDR+1],R[ADDR+2]...

Arduino的wire库函数和I2C通信时序的一一对应

先放上另一个参考文献，看完之后会对通信时序有一个更深刻的认识：一文看懂I2C协议 - 知乎 (zhihu.com)https://zhuanlan.zhihu.com/p/362287272

然后，我们来具体分析一下Arduino的wire.h库函数和I2C通信之间一一对应的关系：

beginTransmission(int addr)，新建缓存区，存储之后写的内容（信息的第一个字节一定是addr与W标志），等到endTransmission的时候一起发送。

write(...)，把要传送的数据存入缓存区当中，注意缓存区大小只有32 bytes，所以内容不能一次传递太多，否则endTransmission会返回错误。

endTransmission(bool stop = true)，发送开始信号，并把缓存区的内容发送给目标从机，注意，这里的stop参数若为真，则发送完所有内容后会发送Stop信号，关闭连接；否则，不发送Stop信号，不关闭连接状态。（如果是读数据，那么就应当选择false！如果是写数据，那就可以直接用默认参数）

requestFrom(int addr, int num)，发送开始信号，第一个字节是addr与R标志，每收到一个字节，如果累计接收数据大小小于num，则发送ACK信号，直到接收到了num个字节，发送NACK信号告诉从机停止发送信息，并发送Stop信号断开连接。

简单的回顾一下

现在回过头再来看一下刚才的问题代码，分析一下它为什么可以恰好正确运行？

第四行，指示了DATAX0寄存器，而后又write(DATAX1)的效果相当于单字节写，会向DATAX0写入DATAX1的内容，但是，由于DATAX0和DATAX1都是只读的，因此写失败[1]，加速度计仍保持在DATAX0的位置。虽然endTransmission的参数并没有设定为false，但是，参考之前的Figure 41，“This start is either a restart or a stop followed by a start”，也就是说无论restart之前是否有stop信号都是允许的[2]。而后requestFrom函数就会从DATAX0开始，连续读取两个字节的内容。此后的available函数我没有分析出有什么用处，那就直接删掉，避免冗余。

由于[1][2]两条原因，这段代码没有故障，但是对于程序的兼容性造成了很大的损害，如果用于其他的模块，则很有可能出现故障。

总结一下I2C单/多字节读写的方法

单字节写：举例子，向adxl345的POWER_CTL寄存器写入0x8。

  Wire.beginTransmission(ADXL345);
  Wire.write(POWER_CTR);
  Wire.write(0x8);
  Wire.endTransmission();

多字节读：举例子，向adxl345的OFSX, OFSY, OFSZ写入offset_x, offset_y, offset_z（连续的三个寄存器，存储xyz三个分量上的误差值），注意offset_xyz都是byte类型的数据。

  Wire.beginTransmission(ADXL345);
  Wire.write(OFSX);
  Wire.write(offset_x);
  Wire.write(offset_y);
  Wire.write(offset_z);
  Wire.endTransmission();

其实write函数最好这样子用：write(&data, size)，这样子可以更好控制传输数据内容。

单字节读：举例子，读取adxl345的DEVID值。

  Wire.beginTransmission(ADXL345);
  Wire.write(DEVID);
  Wire.endTransmission(false);
  Wire.requestFrom(ADXL345, 1);
  int device_id = Wire.read();

多字节读：举例子，读取adxl345的DATAX0, DATAX1, DATAY0, DATAY1, DATAZ0, DATAZ1六个寄存器的内容，注意这六个寄存器是连续的，并且由于数据按照小端法存放，acc_x应该等于R[DATAX1] << 8 | R[DATAX0]。

  Wire.beginTransmission(ADXL345);
  Wire.write(DATAX0);
  Wire.endTransmission(false);
  Wire.requestFrom(ADXL345, 6);
  int x, y, z;
  Wire.readBytes((char *)&x, 2);
  Wire.readBytes((char *)&y, 2);
  Wire.readBytes((char *)&z, 2);

这里必须要用到(char *)&x这样的类型转换，否则会报错（int *和char *类型不匹配）。当然，也可以直接如下，都不过是类型转换罢了。

  char buf[6];
  int x, y, z;
  Wire.beginTransmission(ADXL345);
  Wire.write(DATAX0);
  Wire.endTransmission();
  Wire.requestFrom(ADXL345, 6);
  Wire.readBytes(buf, 6);
  x = *(int *)(buf + 0);
  y = *(int *)(buf + 2);
  z = *(int *)(buf + 4);

利用旋转矩阵进行线性变换

如何读取三维加速度的内容别人也有论述，那，我就也不写了趴？

Arduino and MPU6050 Accelerometer and Gyroscope Tutorial (howtomechatronics.com)https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/

使得3D模型跟随旋转的方法分析

在读取到了accx, accy, accz的值之后，考虑如何做出这样子的效果：每当我倾斜加速度计，电脑上都会有一个3D模型按照我的倾斜角度进行响应。

这个地方原作者写了方法，但我懒得看了，原因有二：Processing这个软件连智能提示都没有，而且还要用到java，我并不擅长；刚学完矩阵变换，借此机会练一下手岂不妙哉。

废话不多说，直接开始分析。

以加速度计的板子为参考系，测得重力加速度a=(accx,accy,accz)，（三者范围均为[-256,255]，单位均为LSB，即最小精度值）。而在世界参考系当中，g=(0,0,255)。因此，我们只需要求得一个旋转矩阵R，使得Ra=g即可。不过这个R的取值有无限多个，所以我们只用找最典型的一个就好，即：将a在ag两个向量夹角所构成的平面中旋转至g所在位置。

在看以下公式之前，先自己想一想空间里面一根向量通过旋转与另一根向量重合的场景。

因此，考虑三个正交向量a，a叉乘g，a叉乘(a叉乘b)经过旋转矩阵作用后的结果：（为什么用叉乘？因为两个向量叉乘的结果垂直于两个向量，这样方便构造正交矩阵。）（为什么要构造正交矩阵？因为正交矩阵的逆好求，直接转置即可。）

$\begin{cases} Ra=g \\ R(a\times g)=(a\times g) \\ R(a\times(a\times b)) = b\times(a\times b) \end{cases}$

其中a和g都是已知的，那么，有：

$R\left [ a\,\,\,a\times g\,\,\,a\times(a\times b) \right ] = \left [ b\,\,\,a\times g\,\,\,b\times(a\times b) \right ]$

$R= \left [ b\,\,\,a\times g\,\,\,b\times(a\times b) \right ]\left [ a\,\,\,a\times g\,\,\,a\times(a\times b) \right ] ^{-1}$

将a，b，a叉乘g，a叉乘(a叉乘b)，b叉乘(a叉乘b)进行单位化，由于旋转矩阵作用在向量上并不会改变长度，因此等式仍然成立。并且单位化后，[a a叉乘g a叉乘(a叉乘b)]为正交矩阵，逆就等于转置。

$R= \left [ b\,\,\,a\times g\,\,\,b\times(a\times b) \right ]\left [ a\,\,\,a\times g\,\,\,a\times(a\times b) \right ] ^T$

这样子就得到了旋转矩阵R，完事。不得不感叹线性代数没有白学。（虽然成绩很差，恼）

随便给出一个测量结果(71, 174, 171)，先求出旋转矩阵R，然后将R分别作用于一个空间正方形的四个顶点(1,1,0),(1,-1,0),(-1,1,0),(-1,-1,0)，就可以得到旋转后的顶点坐标(0.83894433, 0.6053002 , 0.96425312), ( 1.06770912, -0.83406498, -0.40537988), (-0.83894433, -0.6053002 , -0.96425312), (-1.06770912, 0.83406498, 0.40537988)。

然后，利用matplotlib就可以做出数据可视化的效果：

代码就不做过多解释了，简单叙述一下：process模块建立与arduino的串口通讯，调用rotate模块计算出旋转矩阵，并实时在matplotlib的视图中进行渲染。

全部代码

/* manual.txt */
连接线路图：
UNO   :   ADXL345
5V    -   VCC
GND   -   GND
A4    -   SDA
A5    -   SCL

/* accel.ino */
#include <Wire.h>
#include <math.h>
#define ADXL345 0x53
#define R_CTR 0x2d
#define MEASURE_MODE 0x8
#define R_FMT 0x31
#define R_XYZ 0x32
#define R_OFFSET 0x1e

int RANGE = 2;  // 2, 4, 8, 16 (g)

void init_wire();
void set_calibration();
void set_range();

void setup() {
  Serial.begin(9600);
  init_wire();
}

void loop() {
  Wire.beginTransmission(ADXL345);
  Wire.write(R_XYZ);  // specify register 0x32, which is R_X0
  Wire.endTransmission();
  Wire.requestFrom(ADXL345, 6);  // ask for 6 bytes begin from R_X0
  int x, y, z;
  Wire.readBytes((char *)&x, 2);
  Wire.readBytes((char *)&y, 2);
  Wire.readBytes((char *)&z, 2);
  Serial.print(x);
  Serial.print(" ");
  Serial.print(y);
  Serial.print(" ");
  Serial.print(z);
  Serial.print("\n");
  delay(100);
}

void init_wire() {
  Wire.begin();
  /* Set Measure mode */
  Wire.beginTransmission(ADXL345);  // begin to send message to ADXL345
  Wire.write(R_CTR);                // choose to access POWER_CTR register,
  Wire.write(MEASURE_MODE);         // and select the sensor mode to measure mode
  Wire.endTransmission();           // end sending message to ADXL345

  set_range();
  set_calibration();
}

void set_range() {
  /* Set range */
  Wire.beginTransmission(ADXL345);  // begin to send message to ADXL345
  Wire.write(R_FMT);                // choose to access DATA_FORMAT register
  char buf[100];
  switch (RANGE) {
  default:
    RANGE = 2;
    sprintf(buf, "Unsupported range: %d, set to default range (-2g,2g)\n", RANGE);
    Serial.print(buf);
  case 2: Wire.write(0); break;
  case 4: Wire.write(1); break;
  case 8: Wire.write(2); break;
  case 16: Wire.write(3); break;
  }
  Wire.endTransmission();  // end sending message to ADXL345
}

void set_calibration() {
  Wire.beginTransmission(ADXL345);
  Wire.write(R_OFFSET);
  Wire.write((byte)0);
  Wire.write((byte)0);
  Wire.write((byte)0);
  Wire.endTransmission();
  /* Measure the offset */
  int n = 100;
  double sx = 0, sy = 0, sz = 0;
  for (int i = 0; i < n; ++i) {
    Wire.beginTransmission(ADXL345);
    Wire.write(R_XYZ);  // specify register 0x32, which is R_X0
    Wire.endTransmission();
    Wire.requestFrom(ADXL345, 6);  // ask for 6 bytes begin from R_X0
    int x, y, z;
    Wire.readBytes((char *)&x, 2);
    Wire.readBytes((char *)&y, 2);
    Wire.readBytes((char *)&z, 2);
    sx += x; sy += y; sz += z;
    delay(10);
  }
  /* Calibrate the offset */
  // 10-bit resolution - 1-bit sign = 9-bit data
  byte ox, oy, oz;
  double scale = 64.0 / ((1 << 9) / RANGE);
  ox = round((0 - sx / n) * scale);
  oy = round((0 - sy / n) * scale);
  oz = round((255 - sz / n) * scale);
  Wire.beginTransmission(ADXL345);
  Wire.write(R_OFFSET);
  Wire.write(ox);
  Wire.write(oy);
  Wire.write(oz);
  Wire.endTransmission();
  delay(20);
}

# process.py
import matplotlib.pyplot as plt
import serial
import rotate

fig = plt.figure()
axe = plt.axes(projection='3d')

vertices = [[1, 1, 0], [1, -1, 0], [-1, -1, 0], [-1, 1, 0]]

se = serial.Serial(port = "COM7", baudrate = 9600)

while True:
    data = se.readline().decode()
    a = list(map(int, data.split()))
    R = rotate.rotation(a, [0,0,1])
    new_vertices = [rotate.rotate(R, v) for v in vertices]
    print(a)
    print(new_vertices)
    axe.clear()
    axe.set_xlim((-1.5,1.5))
    axe.set_ylim((-1.5,1.5))
    axe.set_zlim((-1.5,1.5))
    for v1 in new_vertices:
        for v2 in new_vertices:
            plt.plot([v1[0],v2[0]], [v1[1],v2[1]], [v1[2],v2[2]], color='c')
    plt.pause(0.005)

# rotate.py
import numpy as np

# 将R作用于向量a
def rotate(R, a):
    return np.array(a * R.T).flatten()

# 计算出从a到b的旋转矩阵
def rotation(a, b):
    cross = lambda x,y:np.cross(x,y)
    a = a / np.linalg.norm(a)
    b = b / np.linalg.norm(b)
    if ((a == b).all()):
        return np.identity(3)
    ab = cross(a, b)
    aab = cross(a, ab)
    bab = cross(b, ab)
    ab = ab / np.linalg.norm(ab)
    aab = aab / np.linalg.norm(aab)
    bab = bab / np.linalg.norm(bab)
    B = np.matrix([b, ab, bab]).T
    AT = np.matrix([a, ab, aab])
    return B * AT

如果发现错误之处，还望不吝指出。感谢阅读到这里！~