SLRC-2019 / Mbed 2 deprecated speed_robot

code for the speed robot

Dependencies:   MPU6050-DMP mbed PololuQTRSensors vl53l0x

gyro.h@8:d7941bcd9981, 2019-10-01 (annotated)

Committer:
deepanaishtaweera174
Date:
Tue Oct 01 12:33:33 2019 +0000
Revision:
8:d7941bcd9981
Parent:
7:fd80a0d11658 
hey

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 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 7:fd80a0d11658 43 zAngle = (int)ypr[0] * 180 / M_PI;
deepanaishtaweera174 7:fd80a0d11658 44 xAngle = (int)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 }