diff --git a/CopterController/IMU/I2Cdev.cpp b/CopterController/I2Cdev.cpp
similarity index 100%
rename from CopterController/IMU/I2Cdev.cpp
rename to CopterController/I2Cdev.cpp
diff --git a/CopterController/IMU/I2Cdev.h b/CopterController/I2Cdev.h
similarity index 100%
rename from CopterController/IMU/I2Cdev.h
rename to CopterController/I2Cdev.h
diff --git a/CopterController/IMU/MPU6050dmp.cpp b/CopterController/IMU/MPU6050dmp.cpp
deleted file mode 100644
index fdeb2a7..0000000
--- a/CopterController/IMU/MPU6050dmp.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
-* MPU6050dmp.cpp
-*/
-
-#include "MPU6050dmp.h"
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <string.h>
-#include <math.h>
-#include "I2Cdev.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-
-MPU6050dmp::MPU6050dmp( int devAddr )
-{
-    uint8_t devStatus;
-
-    printf("Initializing I2C devices...\n");
-    mpu.initialize();
-
-    printf("Testing device connections...\n");
-    printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
-
-    printf("Initializing DMP...\n");
-    devStatus = mpu.dmpInitialize();
-
-    if (devStatus == 0)
-    {
-        printf("Enabling DMP...\n");
-        mpu.setDMPEnabled(true);
-
-        mpuIntStatus = mpu.getIntStatus();
-
-        printf("DMP ready!\n");
-        dmpReady = true;
-
-        packetSize = mpu.dmpGetFIFOPacketSize();
-    }
-    else
-    {
-        printf("DMP Initialization failed (code %d)\n", devStatus);
-    }
-}
-
-int update()
-{
-    if( !dmpReady ) // setup failed
-    {
-        printf("dmp Initialization failed...cannot read from dmp\n");
-        return -1;
-    }
-
-    fifoCount = mpu.getFIFOCount();
-    if( fifoCount == 1024 )
-    {
-        mpu.resetFIFO();
-        printf( "FIFO overflow\n" );
-    }
-    else if( fifoCount >= 42 )
-    {
-        mpu.getFIFOBytes( fifoBuffer, packetSize );
-        mpu.dmpGetQuaternion( &q, fifoBuffer );
-        mpu.dmpGetAccel(&aa, fifoBuffer);
-        mpu.dmpGetGravity( &gravity, &q );
-        mpu.dmpGetLinearAccelInWorld( &aaWorld, &aaReal, &q );
-    }
-}
-
-
-
-
diff --git a/CopterController/IMU/MPU6050dmp.h b/CopterController/IMU/MPU6050dmp.h
deleted file mode 100644
index 0223802..0000000
--- a/CopterController/IMU/MPU6050dmp.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef MPU6050DMP_H
-#define MPU6050DMP_H
-
-#include "MPU6050_6Axis_MotionApps20.h"
-#include "helper_3dmath.h"
-
-class MPU6050dmp
-{
-public:
-    MPU6050dmp( int devAddr = 0x68 );
-
-    int update();
-
-    Quaternion q;
-    VectorInt16 aa;
-    VectorInt16 aaReal;
-    VectorInt16 aaWorld;
-    VectorFloat gravity;
-    int16_t xg;
-    int16_t yg;
-    int16_t zg;
-private:
-    MPU6050 mpu;
-
-    bool dmpReady = false;
-    uint8_t mpuIntStatus;
-    uint16_t packetSize;
-    uint16_t fifoCount;
-    uint8_t fifoBuffer[ 64 ];
-};
-
-#endif
\ No newline at end of file
diff --git a/CopterController/IMU/MPU6050dmp_tester.cpp b/CopterController/IMU/MPU6050dmp_tester.cpp
deleted file mode 100644
index 38ce38e..0000000
--- a/CopterController/IMU/MPU6050dmp_tester.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <string.h>
-#include <math.h>
-#include "I2Cdev.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-#include "wiringPi.h"
-
-#define INTERRUPT_PIN 7 // silkscreen: 4
-#define LED_PIN 6 // silkscreen: 25
-#define SWITCH_PIN // silkscreen: 
-
-MPU6050 mpu; // AD0 pin low
-//MPU6050 mpu(0x69) // AD0 pin high
-
-// MPU control/status vars
-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
-float sensorYaw;
-int32_t
-
-
-// ================================================================
-// ===                   INTERRUPT FUNCTION                     ===
-// ================================================================
-
-void dmpDataReady()
-{
-    fifoCount = mpu.getFIFOCount();
-
-    if (fifoCount == 1024)
-    {
-        // reset so we can continue cleanly
-        mpu.resetFIFO();
-        printf("FIFO overflow!\n");
-    }
-    else if( fifoCount >= 42 )
-    {
-        // 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
-        int32_t[3] accel;
-        //float euler[3];         // [psi, theta, phi]    Euler angle container
-        float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
-        
-        mpu.getFIFOBytes(fifoBuffer, packetSize);
-        mpu.dmpGetQuaternion(&q, fifoBuffer);
-        mpu.dmpGetGravity(&gravity, &q);
-        mpu.dmpGetLinearAccelInWorld( , , &q );
-
-        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-        printf("ypr  %7.2f %7.2f %7.2f    ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI);
-        printf( "\n" );
-        sensorYaw = ypr[0] * 180/M_PI;
-    }
-}
-
-// ================================================================
-// ===                      INITIAL SETUP                       ===
-// ================================================================
-
-void setup()
-{
-    // setup led as output
-    pinMode( LED_PIN, OUTPUT );
-   
-    printf( "Setting up GPIO...\n" );
-    wiringPiSetup();
-    pinMode( INTERRUPT_PIN, INPUT );
-
-    printf("Initializing I2C devices...\n");
-    mpu.initialize();
-
-    printf("Testing device connections...\n");
-    printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
-
-    printf("Initializing DMP...\n");
-    uint8_t devStatus = mpu.dmpInitialize();
-
-    if( devStatus == 0 )
-    {
-        printf("Enabling DMP...\n");
-        mpu.setDMPEnabled(true);
-
-        printf( "Enabling external interrupt on pin %d\n", INTERRUPT_PIN);
-        wiringPiISR( INTERRUPT_PIN, INT_EDGE_RISING, dmpDataReady );
-
-        printf("DMP ready!\n");
-
-        packetSize = mpu.dmpGetFIFOPacketSize();
-    }
-    else
-    {
-        printf("DMP Initialization failed (code %d)\n", devStatus);
-    }
-}
-
-// ================================================================
-// ===                    MAIN PROGRAM LOOP                     ===
-// ================================================================
-
-int main( int argc, char* argv[] )
-{
-    setup();
-    usleep(100000);
-    while(1)
-    {}
-    return 0;
-}
diff --git a/CopterController/IMU/Makefile b/CopterController/IMU/Makefile
deleted file mode 100644
index 5a53fe0..0000000
--- a/CopterController/IMU/Makefile
+++ /dev/null
@@ -1,12 +0,0 @@
-all: MPU6050dmp_tester
-
-HDRS = helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.h
-CXXFLAGS = -DDMP_FIFO_RATE=9 -Wall
-
-I2Cdev.o MPU6050.o MPU6050dmp_tester.o: $(HDRS)
-
-MPU6050dmp_tester: I2Cdev.o MPU6050.o MPU6050dmp_tester.o
-	$(CXX) $^ -lm `pkg-config gtkmm-3.0 --cflags --libs` -lwiringPi -o $@
-
-clean:
-	rm I2Cdev.o MPU6050.o MPU6050dmp.o MPU6050dmp_tester.o MPU6050dmp_tester
\ No newline at end of file
diff --git a/CopterController/IMU/MPU6050.cpp b/CopterController/MPU6050.cpp
similarity index 100%
rename from CopterController/IMU/MPU6050.cpp
rename to CopterController/MPU6050.cpp
diff --git a/CopterController/IMU/MPU6050.h b/CopterController/MPU6050.h
similarity index 100%
rename from CopterController/IMU/MPU6050.h
rename to CopterController/MPU6050.h
diff --git a/CopterController/IMU/MPU6050_6Axis_MotionApps20.h b/CopterController/MPU6050_6Axis_MotionApps20.h
similarity index 100%
rename from CopterController/IMU/MPU6050_6Axis_MotionApps20.h
rename to CopterController/MPU6050_6Axis_MotionApps20.h
diff --git a/CopterController/Makefile b/CopterController/Makefile
new file mode 100644
index 0000000..b618317
--- /dev/null
+++ b/CopterController/Makefile
@@ -0,0 +1,18 @@
+all: pid.o I2Cdev.o MPU6050.o PCA9685.o PCA9685tester pidTest MPU6050dmp_tester
+
+HDRS = pid.h helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.h PCA9685.h PCA9685_Addresses.h
+CXXFLAGS = -DDMP_FIFO_RATE=9 -Wall -lwiringPi
+
+pid.o pidTest.o I2Cdev.o MPU6050.o MPU6050dmp_tester.o PCA9685.o PCA9685tester.o: $(HDRS)
+
+pidTest: pidTest.o pid.o
+	$(CXX) $^ -o $@
+
+MPU6050dmp_tester: I2Cdev.o MPU6050.o MPU6050dmp_tester.o
+	$(CXX) $^ -lm `pkg-config gtkmm-3.0 --cflags --libs` -lwiringPi -o $@
+
+PCA9685tester: PCA9685.o PCA9685tester.o
+	$(CXX) $^ -lwiringPi -o $@
+
+clean:
+	rm I2Cdev.o MPU6050.o MPU6050dmp.o MPU6050dmp_tester.o MPU6050dmp_tester pid.o pidTest.o pidTest
\ No newline at end of file
diff --git a/CopterController/PCA9685/PCA9685.cpp b/CopterController/PCA9685.cpp
similarity index 100%
rename from CopterController/PCA9685/PCA9685.cpp
rename to CopterController/PCA9685.cpp
diff --git a/CopterController/PCA9685/PCA9685.h b/CopterController/PCA9685.h
similarity index 100%
rename from CopterController/PCA9685/PCA9685.h
rename to CopterController/PCA9685.h
diff --git a/CopterController/PCA9685/Makefile b/CopterController/PCA9685/Makefile
deleted file mode 100644
index 0a55a61..0000000
--- a/CopterController/PCA9685/Makefile
+++ /dev/null
@@ -1,9 +0,0 @@
-all: PCA9685.o PCA9685tester
-
-HDRS = PCA9685.h PCA9685_Addresses.h
-CXXFLAGS = -Wall -lwiringPi
-
-PCA9685.o PCA9685tester.o: $(HDRS)
-
-PCA9685tester: PCA9685.o PCA9685tester.o
-	$(CXX) $^ -lwiringPi -o $@
\ No newline at end of file
diff --git a/CopterController/PCA9685/PCA9685_Addresses.h b/CopterController/PCA9685_Addresses.h
similarity index 100%
rename from CopterController/PCA9685/PCA9685_Addresses.h
rename to CopterController/PCA9685_Addresses.h
diff --git a/CopterController/PCA9685/PCA9685tester.cpp b/CopterController/PCA9685tester.cpp
similarity index 100%
rename from CopterController/PCA9685/PCA9685tester.cpp
rename to CopterController/PCA9685tester.cpp
diff --git a/CopterController/PCA9685/echoServerAdvanced.cpp b/CopterController/echoServerAdvanced.cpp
similarity index 100%
rename from CopterController/PCA9685/echoServerAdvanced.cpp
rename to CopterController/echoServerAdvanced.cpp
diff --git a/CopterController/IMU/helper_3dmath.h b/CopterController/helper_3dmath.h
similarity index 100%
rename from CopterController/IMU/helper_3dmath.h
rename to CopterController/helper_3dmath.h
diff --git a/CopterController/pid.cpp b/CopterController/pid.cpp
new file mode 100644
index 0000000..4860ce4
--- /dev/null
+++ b/CopterController/pid.cpp
@@ -0,0 +1,45 @@
+/*
+* pid.cpp
+*/
+
+#include "pid.h"
+#include <ctime>
+
+pid::pid( double p, double i, double d )
+{
+    pGain = p;
+    iGain = i;
+    dGain = d;
+
+    lastPosition = 0;
+    integral = 0;
+
+    lastUpdate = clock();
+}
+
+double pid::update( double current, double desired )
+{
+    double pAdjust, iAdjust, dAdjust;
+    double difference;
+    double posChange;
+    clock_t newTime;
+
+    difference = desired - current;
+
+    // p
+    pAdjust = pGain * difference;
+
+    // i
+    newTime = clock();
+    integral += difference * (newTime - lastUpdate);
+    iAdjust = -iGain * integral;
+    lastUpdate = newTime;
+
+    // d
+    posChange = current - lastPosition;
+    dAdjust = -dGain * posChange;
+    
+
+    return pAdjust + iAdjust + dAdjust;
+}
+
diff --git a/CopterController/pid.h b/CopterController/pid.h
new file mode 100644
index 0000000..694b591
--- /dev/null
+++ b/CopterController/pid.h
@@ -0,0 +1,23 @@
+/*
+* pid.h
+*/
+
+#ifndef PID_H
+#define PID_H
+
+#include <ctime>
+
+class pid
+{
+public:
+    pid( double p, double i, double d );
+    
+    double update( double current, double desired );
+private:
+    double pGain, iGain, dGain;
+
+    clock_t lastUpdate;
+    double integral, lastPosition;
+};
+
+#endif
\ No newline at end of file
diff --git a/CopterController/pidTest.cpp b/CopterController/pidTest.cpp
new file mode 100644
index 0000000..dfebac8
--- /dev/null
+++ b/CopterController/pidTest.cpp
@@ -0,0 +1,174 @@
+#include <iostream>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/types.h> 
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h> 
+#include <sys/ioctl.h>
+#include "fcntl.h"
+
+#include "pid.h"
+#include "PCA9685.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+
+// TCP stuff
+int sockfd, newsockfd;
+char buffer[256];
+
+// IMU stuff
+//uint16_t packetSize;
+int packetSize;
+MPU6050 mpu;
+
+// servo control stuff
+#define MOTOR_MIN 200
+#define MOTOR_MAX 600
+#define SERVO_FREQ 60
+PCA9685 servo;
+
+void setupTCP( portno )
+{
+    struct sockaddr_in serv_addr, cli_addr;
+    socklen_t clilen;
+
+    std::cout << "setting up TCP on port " << portno << std::endl;
+    sockfd = socket( AF_INET, SOCKET_STREAM, 0 );
+    if( sockfd < 0 )
+        cerror << "ERROR opening socket" << std::endl;
+
+    bzero( (char*) &serv_addr, sizeof(serv_addr) );
+    serv_addr.sin_family = AF_INET;
+    serv_addr.sin_addr.s_addr = INADDR_ANY;
+    serv_addr.sin_port = htons(portno);
+
+    std::cout << "binding socket to portno" << std::endl;
+    if( bind( sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr) ) )
+        cerror << "ERROR binding socket to portno" << std::endl;
+
+    std::cout << "listening to socket" << std::endl;
+    listen( sockfd, 5 );
+    clilen = sizeof( cli_addr );
+
+    std::cout << "waiting for connection..." << std::endl;
+    newsockfd = accept( sockfd, (struct sockaddr *) &cli_addr, &clilen );
+    if( newsockfd < 0 )
+        cerror << "ERROR on accept" << std::endl;
+
+    std::cout << "connection established" << std::endl;
+    bzero( buffer, 256 );
+    fcntl( newsockfd, F_SETFL, 0_NONBLOCK );
+}
+
+void setupPCA()
+{
+    servo = PCA9685();
+    servo.setFrequency( SERVO_FREQ );
+    servo.setPwm( 0, 0, MOTOR_MIN );
+    servo.setPwm( 1, 0, MOTOR_MIN );
+    servo.setPwm( 2, 0, MOTOR_MIN );
+    servo.setPwm( 3, 0, MOTOR_MIN );
+}
+
+/**
+ * returns -1 on failure, 0 on success
+ */
+int setupMPU()
+{
+    uint8_t devStatus;
+
+    mpu = MPU6050();
+
+    std::cout << "initializing MPU6050" << std::endl;
+    mpu.initialize();
+
+    std::cout << "testing MPU6050 connection..." << std::endl;
+    if( mpu.testConnection() )
+    {
+        std::cout << "MPU6050 connection successful" << std::endl;
+    }
+    else
+    {
+        cerror << "MPU6050 connection failed" << std::endl;
+        return -1;
+    }
+
+    std::cout << "initializing DMP" << std::endl;
+    devStatus = mpu.dmpInitialize();
+    if( devStatus == 0 )
+    {
+        std::cout << "DMP initialized" << std::endl;
+
+        std::cout << "Enabling DMP" << std::endl;
+        mpu.setDMPEnabled( true );
+
+        packetSize = mpu.dmpGetFIFOPacketSize();
+    }
+    else
+    {
+        cerror << "DMP initialization failed" << std::endl;
+        return -1;
+    }
+    return 0;
+}
+
+void setup()
+{
+    setupPCA();
+    setupTCP( 51717 );
+    setupMPU();
+}
+
+void loop()
+{
+    int fifoCount;
+    int nread, nwrite;
+    uint8_t fifoBuffer[64];
+    Quaternion q;
+    VectorFloat gravity;
+    float ypr[3];
+
+    nread = read( newsockfd, buffer, 255 );
+    if( nread > 0 )
+    {
+        if( strncmp( buffer, "DISCONNECT", strlen("DISCONNECT") ) == 0 )
+        {
+            std::cout << "SHUTTING DOWN TCP SERVER" << std::endl;
+
+            servo.setPwm( 0, 0, MOTOR_MIN );
+            servo.setPwm( 1, 0, MOTOR_MIN );
+            servo.setPwm( 2, 0, MOTOR_MIN );
+            servo.setPwm( 3, 0, MOTOR_MIN );
+
+            close( newsockfd );
+            close( sockfd );
+            exit;
+        }
+    }
+
+    fifoCount = mpu.getFIFOCount();
+    if( fifoCount == 1024)
+    {
+        cerror << "FIFO overflow" << std::endl;
+    }
+    if( fifoCount >= 42 )
+    {
+        mpu.getFIFOBytes( fifoBuffer, packetSize );
+        mpu.dmpGetQuaternion( &q, fifoBuffer );
+        mpu.dmpGetGravity( &gravity, &q );
+        mpu.dmpGetYawPitchRoll( ypr, &q, &gravity );
+
+        std::cout << "pitch: " << ypr[1] << std::endl;
+    }
+
+}
+
+int main( int argc, char* argv[] )
+{
+    setup();
+    while true
+    {
+        loop();
+    }
+}
\ No newline at end of file
diff --git a/CopterController/IMU/setup-i2c.sh b/CopterController/setup-i2c.sh
similarity index 100%
rename from CopterController/IMU/setup-i2c.sh
rename to CopterController/setup-i2c.sh