Revision 2:17b18ea93551, committed 2015-12-03

Comitter:

amandaghassaei

Date:

Thu Dec 03 02:23:34 2015 +0000

Parent:

1:ec0a08108442

Child:

3:be5cf450436d

Commit message:

delegate stuff working;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Comm/Comm.cpp	Thu Dec 03 02:23:34 2015 +0000
@@ -0,0 +1,87 @@
+#include "Comm.h"
+
+    
+Comm::Comm(CommDelegate *gains, CommDelegate *target, CommDelegate *myMPU6050_1):_json(_pc)//_pc(USBTX, USBRX)
+{
+//    _pc.baud(115200);
+    _gains = gains;
+    _target = target;
+    _myMPU6050_1 = myMPU6050_1;
+}
+
+void Comm::openGripper1(){
+    printGripper1State(true);
+}
+
+void Comm::closeGripper1(){
+    printGripper1State(false);
+}
+
+void Comm::printGripper1State(bool state){
+    _json.open();
+    if (state) _json.print("gripper1", "true");
+    else _json.print("gripper1", "false");
+    _json.close();
+}
+
+void Comm::openGripper2(){
+    printGripper2State(true);
+}
+
+void Comm::closeGripper2(){
+    printGripper2State(false);
+}
+
+void Comm::printGripper2State(bool state){
+    _json.open();
+    if (state) _json.print("gripper2", "true");
+    else _json.print("gripper2", "false");
+    _json.close();
+}
+
+void Comm::setGains(float k1, float d1, float k2, float d2){
+    _gains->setGains(k1, d1, k2, d2);
+}
+
+void Comm::printGains(){
+    _json.open();
+    _json.print("k1", _gains->getK1());
+    _json.sepItem();
+    _json.print("d1", _gains->getD1());
+    _json.sepItem();
+    _json.print("k2", _gains->getK2());
+    _json.sepItem();
+    _json.print("d2", _gains->getD1());
+    _json.close();
+}
+
+void Comm::setTarget(int targetPosition){
+    _target->setPosition(targetPosition);
+}
+
+void Comm::printTarget(){
+    _json.open();
+    _json.print("target", _target->getPosition());
+    _json.close();
+}
+
+void Comm::printPosition(){
+    _json.open();
+    _json.print("th1", _myMPU6050_1->getTheta());
+    _json.sepItem();
+    _json.print("dth1", _myMPU6050_1->getDtheta());
+    _json.close();
+}
+
+//void Comm::check(){
+//    if (newline_detected){
+//        printTarget();
+//    }
+//}
+//
+//void rxCallback(MODSERIAL_IRQ_INFO *q) {
+//    MODSERIAL *serial = q->serial;
+//    if ( serial->rxGetLastChar() == '\n') {
+//        newline_detected = true;
+//    }
+//}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Comm/Comm.h	Thu Dec 03 02:23:34 2015 +0000
@@ -0,0 +1,47 @@
+#ifndef Comm_h
+#define Comm_h
+
+//#include "mbed.h"
+#include "json.h"
+#include "Target.h"
+#include "CommDelegate.h"   
+//#include "MODSERIAL.h"
+
+
+class Comm{
+    
+    public:
+    
+        Comm(CommDelegate *gains, CommDelegate *target, CommDelegate *myMPU6050_1);
+        
+        void openGripper1();
+        void closeGripper1();
+        void printGripper1State(bool state);
+        void openGripper2();
+        void closeGripper2();
+        void printGripper2State(bool state);
+        
+        void setGains(float k1, float d1, float k2, float d2);
+        void printGains();
+        
+        void setTarget(int targetPosition);
+        void printTarget();
+        
+        void printPosition();
+        
+//        void check();
+        
+            
+    private:
+    
+        Serial *_pc;
+        
+        JSON _json;
+        CommDelegate *_gains;
+        CommDelegate *_target;
+        CommDelegate *_myMPU6050_1;
+//        volatile bool newline_detected = false;
+ 
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Comm/CommDelegate.h	Thu Dec 03 02:23:34 2015 +0000
@@ -0,0 +1,33 @@
+#ifndef CommDelegate_h
+#define CommDelegate_h
+
+class CommDelegate//parent class
+{
+    public:
+    
+      CommDelegate(){}//constructor method
+      
+        //imu
+        float virtual getTheta(){return 0.0;}
+        float virtual getDtheta(){return 0.0;}
+      
+        //gains
+        void virtual setGains(float k1, float d1, float k2, float d2){};
+        void virtual setK1(float k1){};
+        void virtual setD1(float d1){};
+        void virtual setK2(float k2){};
+        void virtual setD2(float d2){};
+        float virtual getK1(){return 0;};
+        float virtual getD1(){return 0;};
+        float virtual getK2(){return 0;};
+        float virtual getD2(){return 0;};
+        
+        //target
+        void virtual setPosition(int position){};
+        int virtual getPosition(){return 0;};
+      
+    protected:
+         
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Comm/json.h	Thu Dec 03 02:23:34 2015 +0000
@@ -0,0 +1,57 @@
+#ifndef JSON_h
+#define JSON_h
+
+#include "mbed.h"
+
+class JSON {
+    
+    public:
+    
+        JSON(Serial* pc){
+            _pc = pc;
+        }
+        
+        void open(){
+            _pc->printf("{");
+        }
+        
+        void close(){
+            _pc->printf("}\n");
+        }
+        
+        void sepItem(){
+            _pc->printf(",");
+        }
+    
+        void print(char *key, float value){
+            _pc->printf("\"");
+            _pc->printf(key);
+            _pc->printf("\": \"");
+            _pc->printf("%f", value);
+            _pc->printf("\"");
+        }
+        
+        void print(char *key, int value){
+            _pc->printf("\"");
+            _pc->printf(key);
+            _pc->printf("\": \"");
+            _pc->printf("%i", value);
+            _pc->printf("\"");
+        }
+        
+        void print(char *key, char *value){
+            _pc->printf("\"");
+            _pc->printf(key);
+            _pc->printf("\": \"");
+            _pc->printf(value);
+            _pc->printf("\"");
+        }
+    
+    private:
+        
+        Serial *_pc;
+    
+    
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controls/Gains.h	Thu Dec 03 02:23:34 2015 +0000
@@ -0,0 +1,56 @@
+#ifndef Gains_h
+#define Gains_h
+
+#include "CommDelegate.h"
+
+class Gains: public CommDelegate {
+    
+    public:
+    
+        Gains(){
+            setGains(12, 4.5, 200, 3);
+        }
+        
+        void setGains(float k1, float d1, float k2, float d2){
+            setK1(k1);
+            setD1(d1);
+            setK2(k2);
+            setD2(d2);
+        };
+        
+        void setK1(float k1){
+            _k1 = k1;
+        };
+        void setD1(float d1){
+            _d1 = d1;
+        };
+        void setK2(float k2){
+            _k2 = k2;
+        };
+        void setD2(float d2){
+            _d2 = d2;
+        };
+        
+        float getK1(){
+            return _k1;
+        };
+        float getD1(){
+            return _d1;
+        };
+        float getK2(){
+            return _k2;
+        };
+        float getD2(){
+            return _d2;
+        };
+    
+    private:
+    
+        float _k1;
+        float _d1;
+        float _k2;
+        float _d2;
+    
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controls/Target.h	Thu Dec 03 02:23:34 2015 +0000
@@ -0,0 +1,29 @@
+#ifndef Target_h
+#define Target_h
+
+#include "CommDelegate.h"
+
+class Target: public CommDelegate{
+
+    public:
+    
+        Target(){
+            setPosition(6);
+        }
+    
+        void setPosition(int position){
+            _position = position;
+        }
+    
+        int getPosition(){
+            return _position;
+        }
+    
+    
+    private:
+    
+        int _position;
+    
+};
+
+#endif
\ No newline at end of file

--- a/MPU6050-DMP.lib	Sat Nov 23 16:47:59 2013 +0000
+++ b/MPU6050-DMP.lib	Thu Dec 03 02:23:34 2015 +0000
@@ -1,1 +1,1 @@
-https://mbed.org/users/syundo0730/code/MPU6050-DMP/#7d1bf3ce0053
+https://mbed.org/users/syundo0730/code/MPU6050-DMP/#748bae310e1e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RPCInterface.lib	Thu Dec 03 02:23:34 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/MichaelW/code/RPCInterface/#a9e2c45097c8

--- a/main.cpp	Sat Nov 23 16:47:59 2013 +0000
+++ b/main.cpp	Thu Dec 03 02:23:34 2015 +0000
@@ -1,343 +1,43 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
-// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// Changelog:
-//     2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
-//     2012-06-20 - improved FIFO overflow handling and simplified read process
-//     2012-06-19 - completely rearranged DMP initialization code and simplification
-//     2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
-//     2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
-//     2012-06-05 - add gravity-compensated initial reference frame acceleration output
-//                - add 3D math helper file to DMP6 example sketch
-//                - add Euler output and Yaw/Pitch/Roll output formats
-//     2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
-//     2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
-//     2012-05-30 - basic DMP initialization working
 
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
+#include "MyMPU6050.h"    
+#include "Gains.h"
+#include "Target.h"
 
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
+#include "Comm.h"
+#include "SerialRPCInterface.h"
 
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
 
-// 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"
-
-#include "MPU6050_6Axis_MotionApps20.h"
-//#include "MPU6050.h" // not necessary if using MotionApps include file
+Gains gains;
+Target target;
 
-// 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;
+MyMPU6050 myMPU6050_1(p9, p10);//I2C_SDA, I2C_SCL
 
-/* =========================================================================
-   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.
- * ========================================================================= */
-
-/* =========================================================================
-   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
-   when using Serial.write(buf, len). The Teapot output uses this method.
-   The solution requires a modification to the Arduino USBAPI.h file, which
-   is fortunately simple, but annoying. This will be fixed in the next IDE
-   release. For more info, see these links:
+SerialRPCInterface RPC(USBTX, USBRX);
 
-   http://arduino.cc/forum/index.php/topic,109987.0.html
-   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
-// 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
+DigitalOut myled(LED1);
+DigitalOut myled2(LED2);
 
-// uncomment "OUTPUT_TEAPOT" if you want output that matches the
-// format used for the InvenSense teapot demo
-//#define OUTPUT_TEAPOT
-
-// 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
+volatile bool ledstate = 1;
+void ReadRange(char * input, char * output);
+void ReadRange(char * input, char * output){
+    //Format the output of the srf08 into the output string
+    ledstate = 0;
+    myled = 0;
+//    comm.printTarget();
+//    output =  "amanda";
+}
+RPCFunction RangeFinder2(&ReadRange, "RangeFinder");
 
-// 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' };
-
-DigitalOut led1(LED1);
-InterruptIn checkpin(p29);
-Serial pc(USBTX, USBRX);
-
-// ================================================================
-// ===               INTERRUPT DETECTION ROUTINE                ===
-// ================================================================
-
-volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
-void dmpDataReady() {
-    mpuInterrupt = true;
-}
-
-void setup();
-void loop();
+Comm comm(&gains, &target, &myMPU6050_1);
 
 int main() {
-    setup();
+    myled = 1;
     while(1) {
-        loop();
+        myMPU6050_1.loop();
+        myled2 = ledstate;
+//        comm.printTarget();
     }
 }
 
-// ================================================================
-// ===                      INITIAL SETUP                       ===
-// ================================================================
-
-void setup() {
-    // 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
-    pc.printf("Initializing I2C devices...\r\n");
-    mpu.initialize();
-
-    // verify connection
-    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");
-    
-    // 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
-    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
-        pc.printf("Enabling DMP...\r\n");
-        mpu.setDMPEnabled(true);
-
-        // enable Arduino 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
-        pc.printf("DMP ready! Waiting for first interrupt...\r\n");
-        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)
-        
-        pc.printf("DDMP Initialization failed (code ");
-        pc.printf("%d", devStatus);
-        pc.printf(")\r\n");
-    }
-
-}
-
 
 
-// ================================================================
-// ===                    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);
-            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);
-            printf("euler\t");
-            printf("%f\t", euler[0] * 180/M_PI);
-            printf("%f\t", euler[1] * 180/M_PI);
-            printf("%f\t\r\n", 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);
-            printf("ypr\t");
-            printf("%f\t", ypr[0] * 180/M_PI);
-            printf("%f\t", ypr[1] * 180/M_PI);
-            printf("%f\t\r\n", 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);
-            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
-            // 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);
-            printf("aworld\t");
-            printf("%f\t", aaWorld.x);
-            printf("%f\t", aaWorld.y);
-            printf("%f\t\r\n", 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];
-            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
-        led1 = !led1;
-    }
-}
\ No newline at end of file