![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
code for the speed robot
Dependencies: MPU6050-DMP mbed PololuQTRSensors vl53l0x
Diff: gyro.h
- Revision:
- 0:c3a27228c31b
- Child:
- 1:b188e27eb7da
diff -r 000000000000 -r c3a27228c31b gyro.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/gyro.h Mon Sep 30 08:00:47 2019 +0000 @@ -0,0 +1,89 @@ +void DmpDataReady(); + +void InitMPU() +{ + InterruptIn dmpInterrupt(INTERRUPT_PIN); + mpu.initialize(); + devStatus = mpu.dmpInitialize(); + PrintSerial("Dmp Initialized!"); + + mpu.setXGyroOffset(98); + mpu.setYGyroOffset(-24); + mpu.setZGyroOffset(0); + mpu.setZAccelOffset(1788); + + if (devStatus == 0) + { + mpu.setDMPEnabled(true); + dmpInterrupt.rise(&DmpDataReady); + + mpuIntStatus = mpu.getIntStatus(); + dmpReady = true; + packetSize = mpu.dmpGetFIFOPacketSize(); + // mpu.resetFIFO(); + // fifoCount = mpu.getFIFOCount(); + PrintSerial("DMP Began"); + } + else + { + } +} + +void DmpDataReady() +{ + mpuInterrupt = true; +} + +void DMPMath() +{ + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + zAngle = ypr[0] * 180 / M_PI; + xAngle = ypr[1] * 180 / M_PI; + if (zAngle < 0) + { + zAngle += 360; + } + if (xAngle < 0) + { + xAngle += 360; + } +} + +void GetAngles(bool silentMode) +{ + if (!dmpReady) + return; + while (!mpuInterrupt && fifoCount < packetSize) + { + if (mpuInterrupt && fifoCount < packetSize) + { + fifoCount = mpu.getFIFOCount(); + } + } + + mpuInterrupt = false; + mpuIntStatus = mpu.getIntStatus(); + fifoCount = mpu.getFIFOCount(); + if ((mpuIntStatus & 0x10) || fifoCount == 1024) + { + mpu.resetFIFO(); + fifoCount = mpu.getFIFOCount(); + while (!mpuInterrupt) + { + } + } + if (mpuIntStatus & 0x02) + { + while (fifoCount < packetSize) + fifoCount = mpu.getFIFOCount(); + mpu.getFIFOBytes(fifoBuffer, packetSize); + fifoCount -= packetSize; + DMPMath(); + if (!silentMode) + { + printf("%d\n",(int)zAngle); + } + } +} \ No newline at end of file