![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: main.cpp
- Revision:
- 1:ec0a08108442
- Parent:
- 0:8d2c753a96e7
- Child:
- 2:17b18ea93551
--- a/main.cpp Tue Nov 19 19:10:30 2013 +0000 +++ b/main.cpp Sat Nov 23 16:47:59 2013 +0000 @@ -54,6 +54,7 @@ // specific I2C addresses may be passed as a parameter here // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) // AD0 high = 0x69 + MPU6050 mpu; /* ========================================================================= @@ -74,7 +75,6 @@ http://code.google.com/p/arduino/issues/detail?id=958 * ========================================================================= */ - const float M_PI = 3.14159265; // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual @@ -86,7 +86,7 @@ // (in degrees) calculated from the quaternions coming from the FIFO. // Note that Euler angles suffer from gimbal lock (for more info, see // http://en.wikipedia.org/wiki/Gimbal_lock) -#define OUTPUT_READABLE_EULER +//#define OUTPUT_READABLE_EULER // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ // pitch/roll angles (in degrees) calculated from the quaternions coming @@ -112,11 +112,6 @@ // format used for the InvenSense teapot demo //#define OUTPUT_TEAPOT - - -//#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) -//bool blinkState = false; - // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU @@ -138,9 +133,7 @@ uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; DigitalOut led1(LED1); - InterruptIn checkpin(p29); - Serial pc(USBTX, USBRX); // ================================================================ @@ -167,15 +160,6 @@ // ================================================================ void setup() { - // join I2C bus (I2Cdev library doesn't do this automatically) - //Wire.begin(); - - // initialize serial communication - // (115200 chosen because it is required for Teapot Demo output, but it's - // really up to you depending on your project) - //Serial.begin(115200); - //while (!Serial); // wait for Leonardo enumeration, others continue immediately - // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to // the baud timing being too misaligned with processor ticks. You must use @@ -183,46 +167,37 @@ // crystal solution for the UART timer. // initialize device - //Serial.println(F("Initializing I2C devices...")); - //pc.baud(921600); pc.printf("Initializing I2C devices...\r\n"); mpu.initialize(); // verify connection - //Serial.println(F("Testing device connections...")); pc.printf("Testing device connections...\r\n"); if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n"); else pc.printf("MPU6050 connection failed\r\n"); - //Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // wait for ready //Serial.println(F("\nSend any character to begin DMP programming and demo: ")); //while (Serial.available() && Serial.read()); // empty buffer //while (!Serial.available()); // wait for data //while (Serial.available() && Serial.read()); // empty buffer again - wait(2);// Added for test. TODO: Remove this if test goes well. // load and configure the DMP - //Serial.println(F("Initializing DMP...")); pc.printf("Initializing DMP...\r\n"); devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready - //Serial.println(F("Enabling DMP...")); pc.printf("Enabling DMP...\r\n"); mpu.setDMPEnabled(true); // enable Arduino interrupt detection - //Serial.println(F("Enabling interrupt detection...")); pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n"); checkpin.rise(&dmpDataReady); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it - //Serial.println(F("DMP ready! Waiting for first interrupt...")); pc.printf("DMP ready! Waiting for first interrupt...\r\n"); dmpReady = true; @@ -233,9 +208,7 @@ // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) - //Serial.print(F("DMP Initialization failed (code ")); - //Serial.print(devStatus); - //Serial.println(F(")")); + pc.printf("DDMP Initialization failed (code "); pc.printf("%d", devStatus); pc.printf(")\r\n"); @@ -253,7 +226,6 @@ // if programming failed, don't try to do anything if (!dmpReady) return; - mpuInterrupt = true; // wait for MPU interrupt or extra packet(s) available while (!mpuInterrupt && fifoCount < packetSize) { // other program behavior stuff here @@ -296,26 +268,17 @@ #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z 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); + printf("quat\t"); + printf("%f\t", q.w); + printf("%f\t", q.x); + printf("%f\t", q.y); + printf("%f\t\r\n", q.z); #endif #ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees 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); printf("euler\t"); printf("%f\t", euler[0] * 180/M_PI); printf("%f\t", euler[1] * 180/M_PI); @@ -327,12 +290,6 @@ 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); printf("ypr\t"); printf("%f\t", ypr[0] * 180/M_PI); printf("%f\t", ypr[1] * 180/M_PI); @@ -345,12 +302,10 @@ 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); + printf("areal\t"); + printf("%f\t", aaReal.x); + printf("%f\t", aaReal.y); + printf("%f\t\r\n", aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL @@ -360,12 +315,10 @@ mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); 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); + printf("aworld\t"); + printf("%f\t", aaWorld.x); + printf("%f\t", aaWorld.y); + printf("%f\t\r\n", aaWorld.z); #endif #ifdef OUTPUT_TEAPOT @@ -378,13 +331,13 @@ teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; - //Serial.write(teapotPacket, 14); + for (int i = 0; i < 14; ++i) { + pc.send(teapotPacket[i]); + } teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif // blink LED to indicate activity - //blinkState = !blinkState; - //digitalWrite(LED_PIN, blinkState); led1 = !led1; } } \ No newline at end of file