SLRC-2019 / Mbed 2 deprecated speed_robot

code for the speed robot

Dependencies:   MPU6050-DMP mbed PololuQTRSensors vl53l0x

gyro.h@0:c3a27228c31b, 2019-09-30 (annotated)

Committer:
deepanaishtaweera174
Date:
Mon Sep 30 08:00:47 2019 +0000
Revision:
0:c3a27228c31b
Child:
1:b188e27eb7da
working gyro code

Who changed what in which revision?

UserRevisionLine numberNew 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 0:c3a27228c31b 7 devStatus = mpu.dmpInitialize();
deepanaishtaweera174 0:c3a27228c31b 8 PrintSerial("Dmp Initialized!");
deepanaishtaweera174 0:c3a27228c31b 9
deepanaishtaweera174 0:c3a27228c31b 10 mpu.setXGyroOffset(98);
deepanaishtaweera174 0:c3a27228c31b 11 mpu.setYGyroOffset(-24);
deepanaishtaweera174 0:c3a27228c31b 12 mpu.setZGyroOffset(0);
deepanaishtaweera174 0:c3a27228c31b 13 mpu.setZAccelOffset(1788);
deepanaishtaweera174 0:c3a27228c31b 14
deepanaishtaweera174 0:c3a27228c31b 15 if (devStatus == 0)
deepanaishtaweera174 0:c3a27228c31b 16 {
deepanaishtaweera174 0:c3a27228c31b 17 mpu.setDMPEnabled(true);
deepanaishtaweera174 0:c3a27228c31b 18 dmpInterrupt.rise(&DmpDataReady);
deepanaishtaweera174 0:c3a27228c31b 19
deepanaishtaweera174 0:c3a27228c31b 20 mpuIntStatus = mpu.getIntStatus();
deepanaishtaweera174 0:c3a27228c31b 21 dmpReady = true;
deepanaishtaweera174 0:c3a27228c31b 22 packetSize = mpu.dmpGetFIFOPacketSize();
deepanaishtaweera174 0:c3a27228c31b 23 // mpu.resetFIFO();
deepanaishtaweera174 0:c3a27228c31b 24 // fifoCount = mpu.getFIFOCount();
deepanaishtaweera174 0:c3a27228c31b 25 PrintSerial("DMP Began");
deepanaishtaweera174 0:c3a27228c31b 26 }
deepanaishtaweera174 0:c3a27228c31b 27 else
deepanaishtaweera174 0:c3a27228c31b 28 {
deepanaishtaweera174 0:c3a27228c31b 29 }
deepanaishtaweera174 0:c3a27228c31b 30 }
deepanaishtaweera174 0:c3a27228c31b 31
deepanaishtaweera174 0:c3a27228c31b 32 void DmpDataReady()
deepanaishtaweera174 0:c3a27228c31b 33 {
deepanaishtaweera174 0:c3a27228c31b 34 mpuInterrupt = true;
deepanaishtaweera174 0:c3a27228c31b 35 }
deepanaishtaweera174 0:c3a27228c31b 36
deepanaishtaweera174 0:c3a27228c31b 37 void DMPMath()
deepanaishtaweera174 0:c3a27228c31b 38 {
deepanaishtaweera174 0:c3a27228c31b 39 mpu.dmpGetQuaternion(&q, fifoBuffer);
deepanaishtaweera174 0:c3a27228c31b 40 mpu.dmpGetGravity(&gravity, &q);
deepanaishtaweera174 0:c3a27228c31b 41 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
deepanaishtaweera174 0:c3a27228c31b 42 zAngle = ypr[0] * 180 / M_PI;
deepanaishtaweera174 0:c3a27228c31b 43 xAngle = ypr[1] * 180 / M_PI;
deepanaishtaweera174 0:c3a27228c31b 44 if (zAngle < 0)
deepanaishtaweera174 0:c3a27228c31b 45 {
deepanaishtaweera174 0:c3a27228c31b 46 zAngle += 360;
deepanaishtaweera174 0:c3a27228c31b 47 }
deepanaishtaweera174 0:c3a27228c31b 48 if (xAngle < 0)
deepanaishtaweera174 0:c3a27228c31b 49 {
deepanaishtaweera174 0:c3a27228c31b 50 xAngle += 360;
deepanaishtaweera174 0:c3a27228c31b 51 }
deepanaishtaweera174 0:c3a27228c31b 52 }
deepanaishtaweera174 0:c3a27228c31b 53
deepanaishtaweera174 0:c3a27228c31b 54 void GetAngles(bool silentMode)
deepanaishtaweera174 0:c3a27228c31b 55 {
deepanaishtaweera174 0:c3a27228c31b 56 if (!dmpReady)
deepanaishtaweera174 0:c3a27228c31b 57 return;
deepanaishtaweera174 0:c3a27228c31b 58 while (!mpuInterrupt && fifoCount < packetSize)
deepanaishtaweera174 0:c3a27228c31b 59 {
deepanaishtaweera174 0:c3a27228c31b 60 if (mpuInterrupt && fifoCount < packetSize)
deepanaishtaweera174 0:c3a27228c31b 61 {
deepanaishtaweera174 0:c3a27228c31b 62 fifoCount = mpu.getFIFOCount();
deepanaishtaweera174 0:c3a27228c31b 63 }
deepanaishtaweera174 0:c3a27228c31b 64 }
deepanaishtaweera174 0:c3a27228c31b 65
deepanaishtaweera174 0:c3a27228c31b 66 mpuInterrupt = false;
deepanaishtaweera174 0:c3a27228c31b 67 mpuIntStatus = mpu.getIntStatus();
deepanaishtaweera174 0:c3a27228c31b 68 fifoCount = mpu.getFIFOCount();
deepanaishtaweera174 0:c3a27228c31b 69 if ((mpuIntStatus & 0x10) || fifoCount == 1024)
deepanaishtaweera174 0:c3a27228c31b 70 {
deepanaishtaweera174 0:c3a27228c31b 71 mpu.resetFIFO();
deepanaishtaweera174 0:c3a27228c31b 72 fifoCount = mpu.getFIFOCount();
deepanaishtaweera174 0:c3a27228c31b 73 while (!mpuInterrupt)
deepanaishtaweera174 0:c3a27228c31b 74 {
deepanaishtaweera174 0:c3a27228c31b 75 }
deepanaishtaweera174 0:c3a27228c31b 76 }
deepanaishtaweera174 0:c3a27228c31b 77 if (mpuIntStatus & 0x02)
deepanaishtaweera174 0:c3a27228c31b 78 {
deepanaishtaweera174 0:c3a27228c31b 79 while (fifoCount < packetSize)
deepanaishtaweera174 0:c3a27228c31b 80 fifoCount = mpu.getFIFOCount();
deepanaishtaweera174 0:c3a27228c31b 81 mpu.getFIFOBytes(fifoBuffer, packetSize);
deepanaishtaweera174 0:c3a27228c31b 82 fifoCount -= packetSize;
deepanaishtaweera174 0:c3a27228c31b 83 DMPMath();
deepanaishtaweera174 0:c3a27228c31b 84 if (!silentMode)
deepanaishtaweera174 0:c3a27228c31b 85 {
deepanaishtaweera174 0:c3a27228c31b 86 printf("%d\n",(int)zAngle);
deepanaishtaweera174 0:c3a27228c31b 87 }
deepanaishtaweera174 0:c3a27228c31b 88 }
deepanaishtaweera174 0:c3a27228c31b 89 }