code for the speed robot
Dependencies: MPU6050-DMP mbed PololuQTRSensors vl53l0x
gyro.h@1:b188e27eb7da, 2019-09-30 (annotated)
- Committer:
- deepanaishtaweera174
- Date:
- Mon Sep 30 08:43:34 2019 +0000
- Revision:
- 1:b188e27eb7da
- Parent:
- 0:c3a27228c31b
- Child:
- 7:fd80a0d11658
added tof code;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
deepanaishtaweera174 | 0:c3a27228c31b | 1 | void DmpDataReady(); |
deepanaishtaweera174 | 0:c3a27228c31b | 2 | |
deepanaishtaweera174 | 0:c3a27228c31b | 3 | void InitMPU() |
deepanaishtaweera174 | 0:c3a27228c31b | 4 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 5 | InterruptIn dmpInterrupt(INTERRUPT_PIN); |
deepanaishtaweera174 | 0:c3a27228c31b | 6 | mpu.initialize(); |
deepanaishtaweera174 | 1:b188e27eb7da | 7 | wait(0.2); |
deepanaishtaweera174 | 0:c3a27228c31b | 8 | devStatus = mpu.dmpInitialize(); |
deepanaishtaweera174 | 0:c3a27228c31b | 9 | PrintSerial("Dmp Initialized!"); |
deepanaishtaweera174 | 0:c3a27228c31b | 10 | |
deepanaishtaweera174 | 0:c3a27228c31b | 11 | mpu.setXGyroOffset(98); |
deepanaishtaweera174 | 0:c3a27228c31b | 12 | mpu.setYGyroOffset(-24); |
deepanaishtaweera174 | 0:c3a27228c31b | 13 | mpu.setZGyroOffset(0); |
deepanaishtaweera174 | 0:c3a27228c31b | 14 | mpu.setZAccelOffset(1788); |
deepanaishtaweera174 | 0:c3a27228c31b | 15 | |
deepanaishtaweera174 | 0:c3a27228c31b | 16 | if (devStatus == 0) |
deepanaishtaweera174 | 0:c3a27228c31b | 17 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 18 | mpu.setDMPEnabled(true); |
deepanaishtaweera174 | 0:c3a27228c31b | 19 | dmpInterrupt.rise(&DmpDataReady); |
deepanaishtaweera174 | 0:c3a27228c31b | 20 | |
deepanaishtaweera174 | 0:c3a27228c31b | 21 | mpuIntStatus = mpu.getIntStatus(); |
deepanaishtaweera174 | 0:c3a27228c31b | 22 | dmpReady = true; |
deepanaishtaweera174 | 0:c3a27228c31b | 23 | packetSize = mpu.dmpGetFIFOPacketSize(); |
deepanaishtaweera174 | 0:c3a27228c31b | 24 | // mpu.resetFIFO(); |
deepanaishtaweera174 | 0:c3a27228c31b | 25 | // fifoCount = mpu.getFIFOCount(); |
deepanaishtaweera174 | 0:c3a27228c31b | 26 | PrintSerial("DMP Began"); |
deepanaishtaweera174 | 0:c3a27228c31b | 27 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 28 | else |
deepanaishtaweera174 | 0:c3a27228c31b | 29 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 30 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 31 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 32 | |
deepanaishtaweera174 | 0:c3a27228c31b | 33 | void DmpDataReady() |
deepanaishtaweera174 | 0:c3a27228c31b | 34 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 35 | mpuInterrupt = true; |
deepanaishtaweera174 | 0:c3a27228c31b | 36 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 37 | |
deepanaishtaweera174 | 0:c3a27228c31b | 38 | void DMPMath() |
deepanaishtaweera174 | 0:c3a27228c31b | 39 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 40 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
deepanaishtaweera174 | 0:c3a27228c31b | 41 | mpu.dmpGetGravity(&gravity, &q); |
deepanaishtaweera174 | 0:c3a27228c31b | 42 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); |
deepanaishtaweera174 | 0:c3a27228c31b | 43 | zAngle = ypr[0] * 180 / M_PI; |
deepanaishtaweera174 | 0:c3a27228c31b | 44 | xAngle = ypr[1] * 180 / M_PI; |
deepanaishtaweera174 | 0:c3a27228c31b | 45 | if (zAngle < 0) |
deepanaishtaweera174 | 0:c3a27228c31b | 46 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 47 | zAngle += 360; |
deepanaishtaweera174 | 0:c3a27228c31b | 48 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 49 | if (xAngle < 0) |
deepanaishtaweera174 | 0:c3a27228c31b | 50 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 51 | xAngle += 360; |
deepanaishtaweera174 | 0:c3a27228c31b | 52 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 53 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 54 | |
deepanaishtaweera174 | 0:c3a27228c31b | 55 | void GetAngles(bool silentMode) |
deepanaishtaweera174 | 0:c3a27228c31b | 56 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 57 | if (!dmpReady) |
deepanaishtaweera174 | 0:c3a27228c31b | 58 | return; |
deepanaishtaweera174 | 0:c3a27228c31b | 59 | while (!mpuInterrupt && fifoCount < packetSize) |
deepanaishtaweera174 | 0:c3a27228c31b | 60 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 61 | if (mpuInterrupt && fifoCount < packetSize) |
deepanaishtaweera174 | 0:c3a27228c31b | 62 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 63 | fifoCount = mpu.getFIFOCount(); |
deepanaishtaweera174 | 0:c3a27228c31b | 64 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 65 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 66 | |
deepanaishtaweera174 | 0:c3a27228c31b | 67 | mpuInterrupt = false; |
deepanaishtaweera174 | 0:c3a27228c31b | 68 | mpuIntStatus = mpu.getIntStatus(); |
deepanaishtaweera174 | 0:c3a27228c31b | 69 | fifoCount = mpu.getFIFOCount(); |
deepanaishtaweera174 | 0:c3a27228c31b | 70 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) |
deepanaishtaweera174 | 0:c3a27228c31b | 71 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 72 | mpu.resetFIFO(); |
deepanaishtaweera174 | 0:c3a27228c31b | 73 | fifoCount = mpu.getFIFOCount(); |
deepanaishtaweera174 | 0:c3a27228c31b | 74 | while (!mpuInterrupt) |
deepanaishtaweera174 | 0:c3a27228c31b | 75 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 76 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 77 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 78 | if (mpuIntStatus & 0x02) |
deepanaishtaweera174 | 0:c3a27228c31b | 79 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 80 | while (fifoCount < packetSize) |
deepanaishtaweera174 | 0:c3a27228c31b | 81 | fifoCount = mpu.getFIFOCount(); |
deepanaishtaweera174 | 0:c3a27228c31b | 82 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
deepanaishtaweera174 | 0:c3a27228c31b | 83 | fifoCount -= packetSize; |
deepanaishtaweera174 | 0:c3a27228c31b | 84 | DMPMath(); |
deepanaishtaweera174 | 0:c3a27228c31b | 85 | if (!silentMode) |
deepanaishtaweera174 | 0:c3a27228c31b | 86 | { |
deepanaishtaweera174 | 0:c3a27228c31b | 87 | printf("%d\n",(int)zAngle); |
deepanaishtaweera174 | 0:c3a27228c31b | 88 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 89 | } |
deepanaishtaweera174 | 0:c3a27228c31b | 90 | } |