Both sides previous revision Previous revision | Next revision Both sides next revision | ||
fabricademy2017:students:nuria.robles:week_9 [2018/02/15 23:40] nuriafablab_gmail.com |
fabricademy2017:students:nuria.robles:week_9 [2018/05/12 00:15] nuriafablab_gmail.com |
||
---|---|---|---|
Line 38: | Line 38: | ||
{{ :fabricademy2017:students:nuria.robles:week9_etextile_wearablesii:lilypad_arduino_processing.png?nolink&600x613 }} | {{ :fabricademy2017:students:nuria.robles:week9_etextile_wearablesii:lilypad_arduino_processing.png?nolink&600x613 }} | ||
+ | |||
+ | Here is te arduino code : | ||
+ | |||
+ | <code> | ||
+ | #define OUTPUT_TEAPOT | ||
+ | |||
+ | #define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards | ||
+ | #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 | ||
+ | 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) | ||
+ | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE | ||
+ | Wire.begin(); | ||
+ | Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties | ||
+ | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE | ||
+ | Fastwire::setup(400, true); | ||
+ | #endif | ||
+ | |||
+ | // 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 Arduino | ||
+ | // 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(); | ||
+ | pinMode(INTERRUPT_PIN, INPUT); | ||
+ | |||
+ | // verify connection | ||
+ | Serial.println(F("Testing device connections...")); | ||
+ | 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 | ||
+ | |||
+ | // load and configure the DMP | ||
+ | Serial.println(F("Initializing DMP...")); | ||
+ | devStatus = mpu.dmpInitialize(); | ||
+ | |||
+ | // supply your own gyro offsets here, scaled for min sensitivity | ||
+ | mpu.setXGyroOffset(220); | ||
+ | mpu.setYGyroOffset(76); | ||
+ | mpu.setZGyroOffset(-85); | ||
+ | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip | ||
+ | |||
+ | // 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(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); | ||
+ | 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); | ||
+ | } | ||
+ | |||
+ | // ================================================================ | ||
+ | // === 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 | ||
+ | // . | ||
+ | // . | ||
+ | // . | ||
+ | } | ||
+ | |||
+ | // 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.dmpGetLinearAccel(&aaReal, &aa, &gravity); | ||
+ | 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); | ||
+ | } | ||
+ | } | ||
+ | </code> | ||
A video of the circuit working can be watched here: | A video of the circuit working can be watched here: |