【Follow me Season 2 Episode 4】Task 2: IMU knowledge + IMU data reading + IMU control cube

HonestQiao

【Follow me Season 2 Episode 4】Task 2: IMU knowledge + IMU data reading + IMU control cube [Copy link]

 

The Arduino Nano RP2040 Connect has an onboard IMU sensor that integrates a speedometer and a gyroscope, and can be used to obtain and analyze motion status.

In this article, let’s study the use of this IMU sensor.

1. Understanding the onboard IMU sensor

From the official information of Arduino Nano RP2040 Connect, we can know that the onboard IMU sensor is LSM6DSOXTR:

Its specific location is shown in the figure below:

LSM6DSOXTR is produced by ST. Its functions can be understood from ST official data:

IMU sensors sense movement in three-dimensional space by detecting acceleration and angular velocity: [Reference: What is IMU? IMU (Inertial Measurement Unit) is a… | Author: Dr. Barak Or | Towards Data Science ]

2. Read data from IMU sensor

In the micropython environment adapted for Arduino Nano RP2040 Connect, the driver of this sensor is provided by default and can be called easily.

From the Pinout, we can see that it uses IIC connection:

To read data from it, use the following code:

import time
from machine import Pin, I2C
from lsm6dsox import LSM6DSOX

i2c = I2C(0, scl=Pin(13), sda=Pin(12))
lsm = LSM6DSOX(i2c)

while True:
    accx, accy, accz = lsm.accel()
    print(f"accel: x:{accx:.2f}m/s2, y:{accy:.2f}m/s2, z{accz:.2f}m/s2")
    gyrox, gyroy, gyroz = lsm.gyro()
    print(f"gyro: x:{gyrox:.2f}°/s, y:{gyroy:.2f}°/s, z{gyroz:.2f}°/s")
    print("")
    time.sleep(0.5)

The logic of the above code is as follows:

1. Introduce the required module: LSM6DSOX
2. Instantiate the IIC interface
3. Instantiate the sensor object LSM6DSOX
4. Read the acceleration and angular velocity values in the loop and output them

After running, the output is as follows:

If you pick up the board at this time, you will see that the upper value changes rapidly.

You can try different ways of exercise and observe the patterns of numerical changes.

Refer to the following figure, moving along different axes, you can observe better rules:

3. Controlling the Cube Using IMU Data

1. Arduino Nano RP2040 Connect Run the program:

Just reading the output data from the IMU is a bit boring. You can use a computer to read the IMU data and control the presentation of a cube in the program.

In order to allow the computer program to better obtain IMU data, the program running on the development board is slightly modified:

import time
from machine import Pin, I2C
from lsm6dsox import LSM6DSOX

i2c = I2C(0, scl=Pin(13), sda=Pin(12))
lsm = LSM6DSOX(i2c)

while True:
    accx, accy, accz = lsm.accel()
    print(f"accel: x:{accx:.2f}m/s2, y:{accy:.2f}m/s2, z{accz:.2f}m/s2", end="")
    gyrox, gyroy, gyroz = lsm.gyro()
    print(f"\tgyro: x:{gyrox:.2f}°/s, y:{gyroy:.2f}°/s, z{gyroz:.2f}°/s")
    #print("")
    time.sleep(0.1)

Output the data in one line to facilitate reading and parsing on the computer. The actual output data is as follows:

accel: x:-0.16m/s2, y:-0.40m/s2, z0.91m/s2    gyro: x:0.06°/s, y:0.31°/s, z0.12°/s

To read the data output by the Arduino Nano RP2040 Connect on a computer, you can use the USB serial port directly, but you cannot use it at the same time as the micropython editor.

Save the above program to main.py on the Arduino Nano RP2040 Connect, then disconnect the device serial port connection of the IDE, and re-plug the development board to start and run automatically.

At this point, use the serial port tool to view the output of Arduino Nano RP2040 Connect:

This way the development board is ready to work.

After ensuring normal output, close the serial port tool so that subsequent programs can use the serial port and read data, otherwise there will be conflicts.

2. Computer Programming

On a computer, using Python, you can quickly write a program that reads data from the serial port, parses the data, and then controls the cube.

To use Python on a computer to render 3D objects and control them, you need to use matplotlib, mpl_toolkits, and numpy libraries. To read data from the serial port, you also need pyserial, which can be installed using the following command:

pip install numpy
pip install matplotlib
pip install mpl_toolkits
pip install pyserial

Then write the corresponding program:

import serial
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import re
import time

# 串口配置
port = '/dev/cu.usbmodem14201'  # 根据实际情况修改串口号
baud_rate = 115200

# 初始化串口
ser = serial.Serial(port, baud_rate, timeout=1)

# 初始化3D图形
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

# 固定位置
x, y, z = 0, 0, 0
roll, pitch, yaw = 0, 0, 0

# 更新3D物体的位置和姿态
def update_3d_object(ax, x, y, z, roll, pitch, yaw):
    ax.clear()
    ax.set_xlim([-10, 10])
    ax.set_ylim([-10, 10])
    ax.set_zlim([-10, 10])

    # 绘制3D物体（这里用一个更大的立方体表示）
    size = 2  # 立方体的边长
    cube = np.array([
        [x-size, y-size, z-size],
        [x+size, y-size, z-size],
        [x+size, y+size, z-size],
        [x-size, y+size, z-size],
        [x-size, y-size, z+size],
        [x+size, y-size, z+size],
        [x+size, y+size, z+size],
        [x-size, y+size, z+size]
    ])

    # 应用旋转矩阵
    R = rotation_matrix(roll, pitch, yaw)
    cube_rotated = np.dot(cube, R.T)

    # 绘制立方体
    colors = ['r', 'g', 'b', 'c', 'm', 'y']  # 不同的颜色
    for i in range(4):
        ax.plot([cube_rotated[i, 0], cube_rotated[(i+1) % 4, 0]],
                [cube_rotated[i, 1], cube_rotated[(i+1) % 4, 1]],
                [cube_rotated[i, 2], cube_rotated[(i+1) % 4, 2]], colors[i % 6])
        ax.plot([cube_rotated[i+4, 0], cube_rotated[(i+1) % 4 + 4, 0]],
                [cube_rotated[i+4, 1], cube_rotated[(i+1) % 4 + 4, 1]],
                [cube_rotated[i+4, 2], cube_rotated[(i+1) % 4 + 4, 2]], colors[i % 6])
        ax.plot([cube_rotated[i, 0], cube_rotated[i+4, 0]],
                [cube_rotated[i, 1], cube_rotated[i+4, 1]],
                [cube_rotated[i, 2], cube_rotated[i+4, 2]], colors[i % 6])

    plt.draw()
    plt.pause(0.001)

# 旋转矩阵
def rotation_matrix(roll, pitch, yaw):
    Rx = np.array([[1, 0, 0],
                   [0, np.cos(roll), -np.sin(roll)],
                   [0, np.sin(roll), np.cos(roll)]])
    Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)],
                   [0, 1, 0],
                   [-np.sin(pitch), 0, np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0],
                   [np.sin(yaw), np.cos(yaw), 0],
                   [0, 0, 1]])
    return np.dot(Rz, np.dot(Ry, Rx))

# 读取并解析串口数据
def read_imu_data():
    global roll, pitch, yaw
    if ser.in_waiting > 0:
        data = ser.readline().decode('utf-8').strip()
        match = re.match(r'accel: x:(.*?)m/s2, y:(.*?)m/s2, z(.*?)m/s2\t?gyro: x:(.*?)°/s, y:(.*?)°/s, z(.*?)°/s', data)
        if match:
            accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z = map(float, match.groups())
            # 仅根据角速度更新姿态
            dt = 0.1  # 假设采样间隔为0.1秒
            roll += gyro_x * dt
            pitch += gyro_y * dt
            yaw += gyro_z * dt
            print(data)
            print("")

# 主循环
while True:
    read_imu_data()
    update_3d_object(ax, x, y, z, roll, pitch, yaw)
    time.sleep(0.01)

The specific logic of the above program will not be explained in detail. Those who are interested can study it.

The focus is on the processing in the final main loop, which first reads the IMU data, which is the line of data output previously, and then uses the read data to update the display of the 3D cube, and then continues with a delay.

Running the above program, the specific effects are as follows:

The video is as follows:

iShot_2024-11-22_17.19.00

Refer to the previous test, the effect will be better if you move along different axes.

IV. Summary

The above just demonstrates the simple use of the LSM6DSOXTR IMU sensor on the Arduino Nano RP2040 Connect.

Furthermore, the IMU sensor can also be used for vibration detection, gesture recognition, step counting, tilt detection, free fall, wake-up, single and double clicks, etc., and its uses are very wide.

wangerxian

6Ah, if you replace the cube with an object, it will make more sense.

HonestQiao

wangerxian posted on 2024-11-22 17:51 6Ah, if the cube is replaced with an object, it will feel better.

For example, if you change to an airplane, you can directly play the flight simulation.

秦天qintian0303

One more sensor will complete it. The first three sensors are pretty good.