/* * Connection of MPU9150 to Unity, * We use the DMP funrctions to obtain the rotation of the MPU9150. * Make sure that the pinConfig Array below is exactly the same as in the Unity files. * * Adapted by Gidi van Liempd (gidi@geedesign.com) Jan 2013 * Gidi: This is example MPU6050_DMP6 (can be found in https://github.com/sparkfun/MPU-9150_Breakout/tree/master/firmware ) * combined with my sketch "NunchukConnectionToUnity2", which uses code for a direct connection to Unity, * provided in the zip file "Connecting Arduino to Unity.zip" by Tiuri de Jong * https://code.google.com/p/unity-arduino-serial-connection/ (and adapted by me into a library connUnity). * * Note: since we use the serial connection to Unity, all serial.print function calls in the loop * except the ones defined by ConnUnity must be commented out!! */ // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation // is used in I2Cdev.h #include "Wire.h" // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files // for both classes must be in the include path of your project #include "I2Cdev.h" // (Gidi:) replaced this: //#include "MPU6050_6Axis_MotionApps20.h" // by #include "MPU6050_9Axis_MotionApps41.h" // (but I don't see any difference, except with the latter the frequency of the blinking led is lower?) //#include "MPU6050.h" // not necessary if using MotionApps include file #include // Gidi: connUnity is the class that will connect us to Unity char pinConfig[] = { // array with the virtual pin configuration of the Arduino. Don't forget to change in Flash/Unity. // // (Gidi) I did not change this, but we only use the first digital pin as an input, the rest is irrelevant // // COPY-PASTE PIN CONFIGURATION BELOW ALSO TO FLASH/UNITY -- // 'i' for in 'o' for out or 'p' for pwm 'i', 'i', 'i', 'i', 'i', 'i', 'o', 'o', 'o', 'o', 'o', 'o' // ---------------------------------------------------- }; // I create a connUnity instance which uses the pinConfig as defined above, // and FAKE Digital pins (true) and FAKE Analog pins (true) // i.e., I pass the values of other computations as if these are actual values of the pins defined ConnUnity connUnity = ConnUnity(true, true, pinConfig); // class default I2C address is 0x68 // 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; // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual // quaternion components in a [w, x, y, z] format (not best for parsing // on a remote host such as Processing or something though) #define OUTPUT_READABLE_QUATERNION // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles // (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 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ // pitch/roll angles (in degrees) calculated from the quaternions coming // from the FIFO. Note this also requires gravity vector calculations. // Also note that yaw/pitch/roll angles suffer from gimbal lock (for // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) //#define OUTPUT_READABLE_YAWPITCHROLL // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration // components with gravity removed. This acceleration reference frame is // not compensated for orientation, so +X is always +X according to the // sensor, just without the effects of gravity. If you want acceleration // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. //#define OUTPUT_READABLE_REALACCEL // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration // components with gravity removed and adjusted for the world frame of // reference (yaw is relative to initial orientation, since no magnetometer // is present in this case). Could be quite handy in some cases. //#define OUTPUT_READABLE_WORLDACCEL // uncomment "OUTPUT_TEAPOT" if you want output that matches the // format used for the InvenSense teapot demo //#define OUTPUT_TEAPOT /* ========================================================================= NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch depends on the MPU-6050's INT pin being connected to the Arduino's external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is digital I/O pin 2. Remark Gidi: on the Teensy2.0 and Teensy2.0++ interrupt #0 is the same pin as one of SDA or SCL - and apparently it works without connecting the INT pin on the MPU-9150 breakout board?? (I even tested it on the UNO: also works without this pin connected?) Later I realized that i can set another interrupt, e.g. interupt 2 on the Teensy2.0 (= pin 7 on the Teensy2.0). I tried it, but saw no change in behavior in Unity * ========================================================================= */ #define LED_PIN 11 // (Arduino is 13, Teensy2.0 is 11, Teensy2.0++ is 6) bool blinkState = false; #define BUTTON_PIN 3 // the actual pin a pushbutton is attached, we'll map it to (virtual) digital input 0 // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector // packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; // ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================ volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { mpuInterrupt = true; } // ================================================================ // === INITIAL SETUP === // ================================================================ 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); // Gidi: for the connection to Unity I use 57600 (because it worked before) Serial.begin(57600); // start serial port // Gidi: commented out the next while loop because it gave errors and I am not using Arduino Leonardo //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 // 38400 or slower in these cases, or use some kind of external separate // crystal solution for the UART timer. // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); // verify connection Serial.println(F("Testing device connections...")); Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // wait for ready delay(10000); // (Gidi) I added this, just to be sure //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 // load and configure the DMP //Serial.println(F("Initializing DMP...")); 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...")); mpu.setDMPEnabled(true); // enable Arduino interrupt detection //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); attachInterrupt(0, dmpDataReady, RISING); // (Gidi) if you use the interrupts, you can set this to interrupt 2 for the Teensy2.0 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...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 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(")")); } // configure LED for output pinMode(LED_PIN, OUTPUT); // configure an actual input for a button pinMode(BUTTON_PIN, INPUT); } // ================================================================ // === MAIN PROGRAM LOOP === // ================================================================ void loop() { // if programming failed, don't try to do anything if (!dmpReady) return; // wait for MPU interrupt or extra packet(s) available while (!mpuInterrupt && fifoCount < packetSize) { // other program behavior stuff here // if you are really paranoid you can frequently test in between other // stuff to see if mpuInterrupt is true, and if so, "break;" from the // while() loop to immediately process the MPU data // . // . // . // Gidi: N.B. I noticed that the program never actually gets into this // while loop (because I did not connect the interrupt pin ;-)), // so I placed my code outside of this loop. // Later I tested it on the Uno and on the Teensy2.0 with the interrupt pin // attached, and I was able to put my communication code inside this loop } // reset interrupt flag and get INT_STATUS byte mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus(); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); Serial.println(F("FIFO overflow!")); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & 0x02) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; #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); #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); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees 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); #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); 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); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); 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); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; Serial.write(teapotPacket, 14); teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } // +++++++++++++++++++++++++++++++++++ // Gidi: with no interrupt pin attached, // I placed my code to communicate with Unity here // // process inputs/outputs receiveQuaternionData(); // communicate with Flash/Unity connUnity.processSerial(); // +++++++++++++++++++++++++++++++++++ } void receiveQuaternionData() { // pass the Quaternion values as fake analog signals, and scale them to integers 0..1000 connUnity.readVirtualAnalog(0, (int) (q.x * 1000)); connUnity.readVirtualAnalog(1, (int) (q.y * 1000)); connUnity.readVirtualAnalog(2, (int) (q.z * 1000)); connUnity.readVirtualAnalog(3, (int) (q.w * 1000)); connUnity.readVirtualDigital(0,digitalRead(BUTTON_PIN)); // pass the button value to Unity }