用ICM20602与Arduino做运动控制的代码

当使用ICM20602与Arduino进行运动控制时，你可以使用以下代码作为参考：

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps20.h>

// 初始化 I2C 总线
#define SDA_PIN 21
#define SCL_PIN 22

// 初始化 MPU6050
MPU6050 mpu;

// MPU6050 解算器对象
Quaternion q;

// 初始化 DMP
#define INTERRUPT_PIN 19
bool dmpReady = false;  
volatile bool mpuInterrupt = false;
void dmpDataReady() {
  mpuInterrupt = true;
}

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

  // 初始化 I2C 总线
  Wire.begin(SDA_PIN, SCL_PIN);

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

  // 验证 MPU6050 是否正常连接
  Serial.println(mpu.testConnection() ? "MPU6050 连接成功" : "MPU6050 连接失败");

  // 加载 DMP 固件
  uint8_t devStatus = mpu.dmpInitialize();

  // 启动 DMP，并设置采样率
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);
  
  if (devStatus == 0) {
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    dmpReady = true;
    Serial.println("DMP 已启动");
  } else {
    Serial.print("DMP 初始化失败 (code ");
    Serial.print(devStatus);
    Serial.println(")");
  }
}

void loop() {
  // 等待 MPU6050 中断触发
  if (!mpuInterrupt && fifoCount < packetSize) {
    return;
  }

  // 重置中断标志位
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // 获取FIFO缓冲区中的数据
  fifoCount = mpu.getFIFOCount();
  
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
    mpu.resetFIFO();
    Serial.println("FIFO 已重置");
  } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;

    // 解析DMP数据
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    
    // 获取姿态角度
    float yaw = atan2(2*(q.w*q.z + q.x*q.y), 1-2*(q.y*q.y + q.z*q.z));
    float pitch = asin(2*(q.w*q.y - q.z*q.x));
    float roll = atan2(2*(q.w*q.x + q.y*q.z), 1-2*(q.x*q.x + q.y*q.y));

    // 输出姿态角度
    Serial.print("Yaw: ");
    Serial.print(yaw * 180/M_PI);
    Serial.print("\tPitch: ");
    Serial.print(pitch * 180/M_PI);
    Serial.print("\tRoll: ");
    Serial.println(roll * 180/M_PI);
  }
}

这段代码使用了 I2Cdev 库和 MPU6050 库来与 ICM20602进行通信。首先，我们初始化了串口和 I2C 总线，并验证了 ICM20602 是否成功连接。然后，我们加载了 DMP 固件，并启动了 DMP。在主循环中，我们等待 MPU6050 中断触发，并从 FIFO 缓冲区读取数据。通过解析 DMP 数据，我们可以得到姿态角度（Yaw、Pitch和Roll），并将其输出到串口。

请注意，以上的代码是使用 MPU6050 库进行开发的，因为 ICM20602 和 MPU6050 具有相同的寄存器映射，所以可以直接使用。如果你使用的是其他的库或者驱动程序，请根据其提供的 API 进行相应的修改。

同时，在使用这段代码之前，你需要先安装 I2Cdev 和 MPU6050 库。可以通过 Arduino 库管理器进行安装，或者从以下链接下载并手动安装：

I2Cdev 库：https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev
MPU6050 库：https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

希望能对你有所帮助！如果有任何问题，请随时提问。