main.cpp

00001 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
00002 // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
00003 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
00004 //
00005 // Changelog:
00006 //     2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
00007 //     2012-06-20 - improved FIFO overflow handling and simplified read process
00008 //     2012-06-19 - completely rearranged DMP initialization code and simplification
00009 //     2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
00010 //     2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
00011 //     2012-06-05 - add gravity-compensated initial reference frame acceleration output
00012 //                - add 3D math helper file to DMP6 example sketch
00013 //                - add Euler output and Yaw/Pitch/Roll output formats
00014 //     2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
00015 //     2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
00016 //     2012-05-30 - basic DMP initialization working
00017 
00018 /* ============================================
00019 I2Cdev device library code is placed under the MIT license
00020 Copyright (c) 2012 Jeff Rowberg
00021 
00022 Permission is hereby granted, free of charge, to any person obtaining a copy
00023 of this software and associated documentation files (the "Software"), to deal
00024 in the Software without restriction, including without limitation the rights
00025 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00026 copies of the Software, and to permit persons to whom the Software is
00027 furnished to do so, subject to the following conditions:
00028 
00029 The above copyright notice and this permission notice shall be included in
00030 all copies or substantial portions of the Software.
00031 
00032 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00033 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00034 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00035 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00036 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00037 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00038 THE SOFTWARE.
00039 ===============================================
00040 */
00041 
00042 // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
00043 // is used in I2Cdev.h
00044 //#include "Wire.h"
00045 
00046 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
00047 // for both classes must be in the include path of your project
00048 #include "I2Cdev.h"
00049 
00050 #include "MPU6050_6Axis_MotionApps20.h"
00051 #include "MPU6051_6Axis_MotionApps20.h"
00052 //#include "MPU6050.h" // not necessary if using MotionApps include file
00053 
00054 // class default I2C address is 0x68
00055 // specific I2C addresses may be passed as a parameter here
00056 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
00057 // AD0 high = 0x69
00058 
00059 MPU6050 mpu;
00060 MPU6051 mpu2;
00061 
00062 /* =========================================================================
00063    NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
00064    depends on the MPU-6050's INT pin being connected to the Arduino's
00065    external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
00066    digital I/O pin 2.
00067  * ========================================================================= */
00068 
00069 /* =========================================================================
00070    NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
00071    when using Serial.write(buf, len). The Teapot output uses this method.
00072    The solution requires a modification to the Arduino USBAPI.h file, which
00073    is fortunately simple, but annoying. This will be fixed in the next IDE
00074    release. For more info, see these links:
00075 
00076    http://arduino.cc/forum/index.php/topic,109987.0.html
00077    http://code.google.com/p/arduino/issues/detail?id=958
00078  * ========================================================================= */
00079 
00080 const float M_PI = 3.14159265;
00081 
00082 //uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
00083 // quaternion components in a [w, x, y, z] format (not best for parsing
00084 // on a remote host such as Processing or something though)
00085 #define OUTPUT_READABLE_QUATERNION
00086 
00087 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
00088 // (in degrees) calculated from the quaternions coming from the FIFO.
00089 // Note that Euler angles suffer from gimbal lock (for more info, see
00090 // http://en.wikipedia.org/wiki/Gimbal_lock)
00091 //#define OUTPUT_READABLE_EULER
00092 
00093 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
00094 // pitch/roll angles (in degrees) calculated from the quaternions coming
00095 // from the FIFO. Note this also requires gravity vector calculations.
00096 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
00097 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
00098 #define OUTPUT_READABLE_YAWPITCHROLL
00099 
00100 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
00101 // components with gravity removed. This acceleration reference frame is
00102 // not compensated for orientation, so +X is always +X according to the
00103 // sensor, just without the effects of gravity. If you want acceleration
00104 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
00105 //#define OUTPUT_READABLE_REALACCEL
00106 
00107 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
00108 // components with gravity removed and adjusted for the world frame of
00109 // reference (yaw is relative to initial orientation, since no magnetometer
00110 // is present in this case). Could be quite handy in some cases.
00111 //#define OUTPUT_READABLE_WORLDACCEL
00112 
00113 // uncomment "OUTPUT_TEAPOT" if you want output that matches the
00114 // format used for the InvenSense teapot demo
00115 //#define OUTPUT_TEAPOT
00116 
00117 // MPU control/status vars
00118 bool dmpReady = false;  // set true if DMP init was successful
00119 uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
00120 uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
00121 uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
00122 uint16_t fifoCount;     // count of all bytes currently in FIFO
00123 uint8_t fifoBuffer[128]; // FIFO storage buffer
00124 
00125 bool dmpReady2 = false;  // set true if DMP init was successful
00126 uint8_t mpuIntStatus2;   // holds actual interrupt status byte from MPU
00127 uint8_t devStatus2;      // return status after each device operation (0 = success, !0 = error)
00128 uint16_t packetSize2;    // expected DMP packet size (default is 42 bytes)
00129 uint16_t fifoCount2;     // count of all bytes currently in FIFO
00130 uint8_t fifoBuffer2[128]; // FIFO storage buffer
00131 
00132 // orientation/motion vars
00133 Quaternion q;           // [w, x, y, z]         quaternion container
00134 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
00135 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
00136 VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
00137 VectorFloat gravity;    // [x, y, z]            gravity vector
00138 float euler[3];         // [psi, theta, phi]    Euler angle container
00139 float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
00140 
00141 
00142 // packet structure for InvenSense teapot demo
00143 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
00144 
00145 DigitalOut led1(PTE3);
00146 InterruptIn checkpin(PTD7);  //PTD7, PTD5
00147 InterruptIn checkpin2(PTD5);
00148 Serial pc(PTA2,PTA1);
00149 DigitalOut btSwitch(PTE30); 
00150 DigitalOut imuSwitch(PTD6); //PTD6, PTD4 
00151 DigitalOut imu2Switch(PTD4);
00152 //int counter = 0;
00153 // ================================================================
00154 // ===               INTERRUPT DETECTION ROUTINE                ===
00155 // ================================================================
00156 
00157 volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
00158 void dmpDataReady() {
00159     mpuInterrupt = true;
00160 }
00161 volatile bool mpuInterrupt2 = false;     // indicates whether MPU interrupt pin has gone high
00162 void dmpDataReady2() {
00163     mpuInterrupt2 = true;
00164 }
00165 
00166 void setup();
00167 void loop();
00168 void loop2();
00169 Timer t;
00170 int main() {
00171     setup();
00172     t.start();
00173     pc.baud(115200);
00174     while(1) {
00175       //  pc.printf("Loopy loop\n\r");
00176       //pc.printf("%f\t",t.read());
00177         loop();
00178         //pc.printf("\t");
00179         loop2();
00180         //pc.printf("\n\r");
00181     }
00182 }
00183 
00184 // ================================================================
00185 // ===                      INITIAL SETUP                       ===
00186 // ================================================================
00187 
00188 void setup() {
00189     // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
00190     // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
00191     // the baud timing being too misaligned with processor ticks. You must use
00192     // 38400 or slower in these cases, or use some kind of external separate
00193     // crystal solution for the UART timer.
00194 btSwitch = 1;
00195 imuSwitch = 1;
00196 imu2Switch = 1;
00197 wait_ms(200);
00198 while(!mpu.testConnection()){
00199     imuSwitch = 0;
00200     wait_ms(100);
00201     imuSwitch = 1;
00202     wait_ms(200);
00203     led1 = !led1;
00204 }
00205 while(!mpu2.testConnection()){
00206     imu2Switch = 0;
00207     wait_ms(100);
00208     imu2Switch = 1;
00209     wait_ms(200);
00210     led1 = !led1;
00211 }
00212 pc.baud(115200);
00213     // initialize device
00214     pc.printf("Initializing I2C devices...\r\n");
00215     mpu.initialize();
00216 
00217     // verify connection
00218     pc.printf("Testing device connections...\r\n");
00219     if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
00220     else pc.printf("MPU6050 connection failed\r\n");
00221     
00222     // load and configure the DMP
00223     pc.printf("Initializing DMP...\r\n");
00224     devStatus = mpu.dmpInitialize();
00225     
00226     // make sure it worked (returns 0 if so)
00227     if (devStatus == 0) {
00228         // turn on the DMP, now that it's ready
00229         pc.printf("Enabling DMP...\r\n");
00230         mpu.setDMPEnabled(true);
00231 
00232         // enable Arduino interrupt detection
00233         pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
00234         checkpin.rise(&dmpDataReady);
00235         
00236         mpuIntStatus = mpu.getIntStatus();
00237 
00238         // set our DMP Ready flag so the main loop() function knows it's okay to use it
00239         pc.printf("DMP ready! Waiting for first interrupt...\r\n");
00240         dmpReady = true;
00241 
00242         // get expected DMP packet size for later comparison
00243         packetSize = mpu.dmpGetFIFOPacketSize();
00244     } else {
00245         // ERROR!
00246         // 1 = initial memory load failed
00247         // 2 = DMP configuration updates failed
00248         // (if it's going to break, usually the code will be 1)
00249         
00250         pc.printf("DDMP Initialization failed (code ");
00251         pc.printf("%d", devStatus);
00252         pc.printf(")\r\n");
00253     }
00254  // initialize device
00255     pc.printf("Initializing I2C devices...\r\n");
00256     mpu2.initialize();
00257 
00258     // verify connection
00259     pc.printf("Testing device connections...\r\n");
00260     if (mpu2.testConnection()) pc.printf("MPU6050 connection successful\r\n");
00261     else pc.printf("MPU6050 connection failed\r\n");
00262     
00263     // load and configure the DMP
00264     pc.printf("Initializing DMP...\r\n");
00265     devStatus2 = mpu2.dmpInitialize();
00266     
00267     // make sure it worked (returns 0 if so)
00268     if (devStatus2 == 0) {
00269         // turn on the DMP, now that it's ready
00270         pc.printf("Enabling DMP...\r\n");
00271         mpu2.setDMPEnabled(true);
00272 
00273         // enable Arduino interrupt detection
00274         pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
00275         checkpin2.rise(&dmpDataReady2);
00276         
00277         mpuIntStatus2 = mpu2.getIntStatus();
00278 
00279         // set our DMP Ready flag so the main loop() function knows it's okay to use it
00280         pc.printf("DMP ready! Waiting for first interrupt...\r\n");
00281         dmpReady2 = true;
00282 
00283         // get expected DMP packet size for later comparison
00284         packetSize2 = mpu2.dmpGetFIFOPacketSize();
00285     } else {
00286         // ERROR!
00287         // 1 = initial memory load failed
00288         // 2 = DMP configuration updates failed
00289         // (if it's going to break, usually the code will be 1)
00290         
00291         pc.printf("DDMP Initialization failed (code ");
00292         pc.printf("%d", devStatus2);
00293         pc.printf(")\r\n");
00294     }
00295 }
00296 
00297 
00298 
00299 // ================================================================
00300 // ===                    MAIN PROGRAM LOOP                     ===
00301 // ================================================================
00302 
00303 void loop() {
00304     // if programming failed, don't try to do anything
00305     if (!dmpReady) return;
00306 
00307     // wait for MPU interrupt or extra packet(s) available
00308     while (!mpuInterrupt && fifoCount < packetSize) {
00309         // other program behavior stuff here
00310         // .
00311         // .
00312         // .
00313         // if you are really paranoid you can frequently test in between other
00314         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
00315         // while() loop to immediately process the MPU data
00316         // .
00317         // .
00318         // .
00319         //led1 = !led1;
00320         //wait_ms(100);
00321     }
00322 
00323     // reset interrupt flag and get INT_STATUS byte
00324     mpuInterrupt = false;
00325     mpuIntStatus = mpu.getIntStatus();
00326 
00327     // get current FIFO count
00328     fifoCount = mpu.getFIFOCount();
00329 
00330     // check for overflow (this should never happen unless our code is too inefficient)
00331     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00332         // reset so we can continue cleanly
00333         mpu.resetFIFO();
00334         //Serial.println(F("FIFO overflow!"));
00335 
00336     // otherwise, check for DMP data ready interrupt (this should happen frequently)
00337     } else if (mpuIntStatus & 0x02) {
00338         // wait for correct available data length, should be a VERY short wait
00339         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00340 
00341         // read a packet from FIFO
00342         mpu.getFIFOBytes(fifoBuffer, packetSize);
00343         
00344         // track FIFO count here in case there is > 1 packet available
00345         // (this lets us immediately read more without waiting for an interrupt)
00346         fifoCount -= packetSize;
00347 
00348                     
00349             printf("\n\r%.4f\t",t.read());
00350         #ifdef OUTPUT_READABLE_QUATERNION
00351             // display quaternion values in easy matrix form: w x y z
00352             mpu.dmpGetQuaternion(&q, fifoBuffer);
00353             printf("quat\t");
00354             printf("%f\t", q.w);
00355             printf("%f\t", q.x);
00356             printf("%f\t", q.y);
00357             printf("%f\t", q.z);
00358         #endif
00359 
00360         #ifdef OUTPUT_READABLE_EULER
00361             // display Euler angles in degrees
00362             mpu.dmpGetQuaternion(&q, fifoBuffer);
00363             mpu.dmpGetEuler(euler, &q);
00364             printf("euler\t");
00365             printf("%f\t", euler[0] * 180/M_PI);
00366             printf("%f\t", euler[1] * 180/M_PI);
00367             printf("%f\t\", euler[2] * 180/M_PI);
00368         #endif
00369 
00370         #ifdef OUTPUT_READABLE_YAWPITCHROLL
00371             // display Euler angles in degrees
00372             mpu.dmpGetQuaternion(&q, fifoBuffer);
00373             mpu.dmpGetGravity(&gravity, &q);
00374             mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
00375             printf("ypr\t");
00376             printf("%.4f\t", ypr[0] * 180/M_PI);
00377             printf("%.4f\t", ypr[1] * 180/M_PI);
00378             printf("%.4f\t", ypr[2] * 180/M_PI);
00379         #endif
00380 
00381         #ifdef OUTPUT_READABLE_REALACCEL
00382             // display real acceleration, adjusted to remove gravity
00383             mpu.dmpGetQuaternion(&q, fifoBuffer);
00384             mpu.dmpGetAccel(&aa, fifoBuffer);
00385             mpu.dmpGetGravity(&gravity, &q);
00386             mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
00387             printf("areal\t");
00388             printf("%f.4\t", aaReal.x);
00389             printf("%f.4\t", aaReal.y);
00390             printf("%f.4\t", aaReal.z);
00391         #endif
00392 
00393         #ifdef OUTPUT_READABLE_WORLDACCEL
00394             // display initial world-frame acceleration, adjusted to remove gravity
00395             // and rotated based on known orientation from quaternion
00396             mpu.dmpGetQuaternion(&q, fifoBuffer);
00397             mpu.dmpGetAccel(&aa, fifoBuffer);
00398             mpu.dmpGetGravity(&gravity, &q);
00399             mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
00400             printf("aworld\t");
00401             printf("%f.4\t", aaWorld.x);
00402             printf("%f.4\t", aaWorld.y);
00403             printf("%f.4\t", aaWorld.z);
00404         #endif
00405     
00406         #ifdef OUTPUT_TEAPOT
00407             // display quaternion values in InvenSense Teapot demo format:
00408             teapotPacket[2] = fifoBuffer[0];
00409             teapotPacket[3] = fifoBuffer[1];
00410             teapotPacket[4] = fifoBuffer[4];
00411             teapotPacket[5] = fifoBuffer[5];
00412             teapotPacket[6] = fifoBuffer[8];
00413             teapotPacket[7] = fifoBuffer[9];
00414             teapotPacket[8] = fifoBuffer[12];
00415             teapotPacket[9] = fifoBuffer[13];
00416             for (int i = 0; i < 14; ++i) {
00417                 pc.send(teapotPacket[i]);
00418             }
00419             teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
00420         #endif
00421 
00422         // blink LED to indicate activity
00423         //led1 = !led1;
00424     }
00425 }
00426 
00427 
00428 void loop2() {
00429     // if programming failed, don't try to do anything
00430     if (!dmpReady2) return;
00431 
00432     // wait for MPU interrupt or extra packet(s) available
00433     while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
00434         // other program behavior stuff here
00435         // .
00436         // .
00437         // .
00438         // if you are really paranoid you can frequently test in between other
00439         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
00440         // while() loop to immediately process the MPU data
00441         // .
00442         // .
00443         // .
00444         //led1 = !led1;
00445         //wait_ms(100);
00446     }
00447 
00448     // reset interrupt flag and get INT_STATUS byte
00449     mpuInterrupt2 = false;
00450     mpuIntStatus2 = mpu2.getIntStatus();
00451 
00452     // get current FIFO count
00453     fifoCount2 = mpu2.getFIFOCount();
00454 
00455     // check for overflow (this should never happen unless our code is too inefficient)
00456     if ((mpuIntStatus2 & 0x10) || fifoCount2 == 1024) {
00457         // reset so we can continue cleanly
00458         mpu2.resetFIFO();
00459         //Serial.println(F("FIFO overflow!"));
00460 
00461     // otherwise, check for DMP data ready interrupt (this should happen frequently)
00462     } else if (mpuIntStatus2 & 0x02) {
00463         // wait for correct available data length, should be a VERY short wait
00464         while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
00465 
00466         // read a packet from FIFO
00467         mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
00468         
00469         // track FIFO count here in case there is > 1 packet available
00470         // (this lets us immediately read more without waiting for an interrupt)
00471         fifoCount2 -= packetSize2;
00472 
00473         #ifdef OUTPUT_READABLE_QUATERNION
00474             // display quaternion values in easy matrix form: w x y z
00475             mpu2.dmpGetQuaternion(&q, fifoBuffer2);
00476             printf("quat\t");
00477             printf("%.4f\t", q.w);
00478             printf("%.4f\t", q.x);
00479             printf("%.4f\t", q.y);
00480             printf("%.4f\t", q.z);
00481         #endif
00482 
00483         #ifdef OUTPUT_READABLE_EULER
00484             // display Euler angles in degrees
00485             mpu2.dmpGetQuaternion(&q, fifoBuffer2);
00486             mpu2.dmpGetEuler(euler, &q);
00487             printf("euler\t");
00488             printf("%.4f\t", euler2[0] * 180/M_PI);
00489             printf("%.4f\t", euler2[1] * 180/M_PI);
00490             printf("%.4f\t", euler2[2] * 180/M_PI);
00491         #endif
00492 
00493         #ifdef OUTPUT_READABLE_YAWPITCHROLL
00494             // display Euler angles in degrees
00495             mpu2.dmpGetQuaternion(&q, fifoBuffer2);
00496             mpu2.dmpGetGravity(&gravity, &q);
00497             mpu2.dmpGetYawPitchRoll(ypr, &q, &gravity);
00498             printf("ypr\t");
00499             printf("%.4f\t", ypr[0] * 180/M_PI);
00500             printf("%.4f\t", ypr[1] * 180/M_PI);
00501             printf("%.4f\t", ypr[2] * 180/M_PI);
00502         #endif
00503 
00504         #ifdef OUTPUT_READABLE_REALACCEL
00505             // display real acceleration, adjusted to remove gravity
00506             mpu.dmpGetQuaternion(&q, fifoBuffer);
00507             mpu.dmpGetAccel(&aa, fifoBuffer);
00508             mpu.dmpGetGravity(&gravity, &q);
00509             mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
00510             printf("areal\t");
00511             printf("%.4f\t", aaReal.x);
00512             printf("%.4f\t", aaReal.y);
00513             printf("%.4f\t\r\n", aaReal.z);
00514         #endif
00515 
00516         #ifdef OUTPUT_READABLE_WORLDACCEL
00517             // display initial world-frame acceleration, adjusted to remove gravity
00518             // and rotated based on known orientation from quaternion
00519             mpu.dmpGetQuaternion(&q, fifoBuffer);
00520             mpu.dmpGetAccel(&aa, fifoBuffer);
00521             mpu.dmpGetGravity(&gravity, &q);
00522             mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
00523             printf("aworld\t");
00524             printf("%f\t", aaWorld.x);
00525             printf("%f\t", aaWorld.y);
00526             printf("%f\t\r\n", aaWorld.z);
00527         #endif
00528    /* 
00529         #ifdef OUTPUT_TEAPOT
00530             // display quaternion values in InvenSense Teapot demo format:
00531             teapotPacket[2] = fifoBuffer[0];
00532             teapotPacket[3] = fifoBuffer[1];
00533             teapotPacket[4] = fifoBuffer[4];
00534             teapotPacket[5] = fifoBuffer[5];
00535             teapotPacket[6] = fifoBuffer[8];
00536             teapotPacket[7] = fifoBuffer[9];
00537             teapotPacket[8] = fifoBuffer[12];
00538             teapotPacket[9] = fifoBuffer[13];
00539             for (int i = 0; i < 14; ++i) {
00540                 pc.send(teapotPacket[i]);
00541             }
00542             teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
00543         #endif*/
00544             printf("\r\n");
00545         // blink LED to indicate activity
00546         //led1 = !led1;
00547     }
00548 }