使用mpu6050通过加速度积分求出位移的代码

以下是一个基本的使用MPU6050通过加速度积分求位移的代码：

#include <Wire.h>
#include <MPU6050.h>

// MPU6050对象
MPU6050 mpu;

// 加速度计的采样频率
const uint16_t sampleRate = 100;

// 加速度计的灵敏度
const uint16_t sensitivity = 16384;

// 累计的位移
float distance = 0;

// 上一次的速度
float lastVelocity = 0;

// 上一次的位移
float lastDistance = 0;

// 上一次的时间戳
uint32_t lastTime = 0;

void setup() {
// 初始化串口
Serial.begin(9600);

// 初始化MPU6050
mpu.initialize();

// 设置加速度计的采样频率
mpu.setRate(sampleRate);

// 设置加速度计的灵敏度
mpu.setFullScaleAccelRange(sensitivity);
}

void loop() {
// 获取加速度计的值
int16_t ax, ay, az;
mpu.getAcceleration(&ax, &ay, &az);

// 计算加速度
float acc = sqrt(ax * ax + ay * ay + az * az) / sensitivity;

// 获取当前时间戳
uint32_t now = millis();

// 计算时间间隔
float dt = (now - lastTime) / 1000.0;

// 计算当前速度
float velocity = lastVelocity + acc * dt;

// 计算当前位移
float thisDistance = lastDistance + velocity * dt;

// 更新累计的位移
distance += thisDistance - lastDistance;

// 更新上一次的速度、位移和时间戳
lastVelocity = velocity;
lastDistance = thisDistance;
lastTime = now;

// 输出当前位移
Serial.println(distance);
}