|
|
楼主 |
发表于 2019-9-9 16:29:09
|
显示全部楼层
- /*
- 【Arduino】108种传感器模块系列实验（资料+代码+图形+仿真）
- 实验一百一十二： MPU-6050模块 三轴加速度 电子陀螺仪6DOF GY-521传感器
- 项目：测试MPU6050示例
- Arduino------MPU 6050
- 5V-------------VCC
- GND-----------GND
- A4-----------SDA IIC 数据线
- A5-----------SCL IIC 时钟线
- D2-----------INT 中断脚
- */
- #include "I2Cdev.h"
- #include "MPU6050_6Axis_MotionApps20.h"
- //#include "MPU6050.h"
- #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- #include "Wire.h"
- #endif
- MPU6050 mpu;
- #define OUTPUT_READABLE_YAWPITCHROLL
- #define LED_PIN 13
- bool blinkState = false;
- // MPU control/status vars
- bool dmpReady = false;
- uint8_t mpuIntStatus;
- uint8_t devStatus;
- uint16_t packetSize;
- uint16_t fifoCount;
- uint8_t fifoBuffer[64];
- Quaternion q;
- VectorInt16 aa;
- VectorInt16 aaReal;
- VectorInt16 aaWorld;
- VectorFloat gravity;
- float euler[3];
- float ypr[3];
- uint8_t teapotPacket[14] = { ', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
- volatile bool mpuInterrupt = false;
- void dmpDataReady() {
- mpuInterrupt = true;
- }
- void setup() {
- #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- Wire.begin();
- TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
- #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
- Fastwire::setup(400, true);
- #endif
- Serial.begin(115200);
- while (!Serial);
- Serial.println(F("Initializing I2C devices..."));
- mpu.initialize();
- Serial.println(F("Testing device connections..."));
- Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
- Serial.println(F("\nSend any character to begin DMP programming and demo: "));
- while (Serial.available() && Serial.read());
- while (!Serial.available());
- while (Serial.available() && Serial.read());
- Serial.println(F("Initializing DMP..."));
- devStatus = mpu.dmpInitialize();
- mpu.setXGyroOffset(220);
- mpu.setYGyroOffset(76);
- mpu.setZGyroOffset(-85);
- mpu.setZAccelOffset(1788);
- if (devStatus == 0) {
-
- Serial.println(F("Enabling DMP..."));
- mpu.setDMPEnabled(true);
- Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
- attachInterrupt(0, dmpDataReady, RISING);
- mpuIntStatus = mpu.getIntStatus();
- Serial.println(F("DMP ready! Waiting for first interrupt..."));
- dmpReady = true;
- packetSize = mpu.dmpGetFIFOPacketSize();
- }
- else {
-
- Serial.print(F("DMP Initialization failed (code "));
- Serial.print(devStatus);
- Serial.println(F(")"));
- }
- // configure LED for output
- pinMode(LED_PIN, OUTPUT);
- }
- void loop() {
- if (!dmpReady) return;
- while (!mpuInterrupt && fifoCount < packetSize) {
- }
- mpuInterrupt = false;
- mpuIntStatus = mpu.getIntStatus();
- fifoCount = mpu.getFIFOCount();
- if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
-
- mpu.resetFIFO();
- Serial.println(F("FIFO overflow!"));
- } else if (mpuIntStatus & 0x02) {
- while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
- mpu.getFIFOBytes(fifoBuffer, packetSize);
-
- fifoCount -= packetSize;
- #ifdef OUTPUT_READABLE_QUATERNION
-
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- Serial.print("quat\t");
- Serial.print(q.w);
- Serial.print("\t");
- Serial.print(q.x);
- Serial.print("\t");
- Serial.print(q.y);
- Serial.print("\t");
- Serial.println(q.z);
- #endif
- #ifdef OUTPUT_READABLE_EULER
-
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetEuler(euler, &q);
- Serial.print("euler\t");
- Serial.print(euler[0] * 180/M_PI);
- Serial.print("\t");
- Serial.print(euler[1] * 180/M_PI);
- Serial.print("\t");
- Serial.println(euler[2] * 180/M_PI);
- #endif
- #ifdef OUTPUT_READABLE_YAWPITCHROLL
-
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
- Serial.print("ypr\t");
- Serial.print(ypr[0] * 180/M_PI);
- Serial.print("\t");
- Serial.print(ypr[1] * 180/M_PI);
- Serial.print("\t");
- Serial.println(ypr[2] * 180/M_PI);
- #endif
- #ifdef OUTPUT_READABLE_REALACCEL
-
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetAccel(&aa, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
- Serial.print("areal\t");
- Serial.print(aaReal.x);
- Serial.print("\t");
- Serial.print(aaReal.y);
- Serial.print("\t");
- Serial.println(aaReal.z);
- #endif
- #ifdef OUTPUT_READABLE_WORLDACCEL
-
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetAccel(&aa, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
- mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
- Serial.print("aworld\t");
- Serial.print(aaWorld.x);
- Serial.print("\t");
- Serial.print(aaWorld.y);
- Serial.print("\t");
- Serial.println(aaWorld.z);
- #endif
-
- #ifdef OUTPUT_TEAPOT
-
- teapotPacket[2] = fifoBuffer[0];
- teapotPacket[3] = fifoBuffer[1];
- teapotPacket[4] = fifoBuffer[4];
- teapotPacket[5] = fifoBuffer[5];
- teapotPacket[6] = fifoBuffer[8];
- teapotPacket[7] = fifoBuffer[9];
- teapotPacket[8] = fifoBuffer[12];
- teapotPacket[9] = fifoBuffer[13];
- Serial.write(teapotPacket, 14);
- teapotPacket[11]++;
- #endif
- blinkState = !blinkState;
- digitalWrite(LED_PIN, blinkState);
- delay(1000);
- }
- }
|
|
楼主: eagler8
谢谢鼓励！可以考虑啊，目前还在赶时间，争取先把这个系列的实验都做完，大概还有三十种前后......