diff --git a/CopterController/PCA9685/PCA9685tester.cpp b/CopterController/PCA9685/PCA9685tester.cpp
index 8335387..9d08c08 100644
--- a/CopterController/PCA9685/PCA9685tester.cpp
+++ b/CopterController/PCA9685/PCA9685tester.cpp
@@ -1,30 +1,74 @@
 
 #include <iostream>
+#include "stdlib.h"
 #include "wiringPi.h"
 #include "PCA9685.h"
 
+#define MOTOR_MIN 200
+#define MOTOR_MAX 600
+#define SERVO_FREQ 60
+
 int main( int argc, const char* argv[] )
 {
+    std::cout << "will only test motor 0" << std::endl;
+    int currentValue = MOTOR_MIN;
+
     wiringPiSetup();
     PCA9685 servo = PCA9685( 0x40 );
-    servo.setFrequency( 60 );
-    std::cout << "freq set" << std::endl;
+    servo.setFrequency( SERVO_FREQ );
+    std::cout << "freq set: " << SERVO_FREQ << std::endl;
 
-    servo.setPwm( 0, 0, 600 );
-    std::cout << "pwm set 600" << std::endl;
-    while( std::cin.get() != '\n' );
+    if( argc > 1 )
+    {
+        if( atoi(argv[1]) == 1 )
+        {
+            std::cout << "will set max pwm" << std::endl;
+            servo.setPwm( 0, 0, MOTOR_MAX );
+            std::cout << "pwm set " << MOTOR_MAX << std::endl;
+            std::cout << "press ENTER to continue" << std::endl;
+            while( std::cin.get() != '\n' );
+        }
+    }
 
-    servo.setPwm( 0, 0, 150 );
-    std::cout << "pwm set 150" << std::endl;
+    servo.setPwm( 0, 0, MOTOR_MIN );
+    std::cout << "pwm set " << MOTOR_MIN << std::endl;
+    std::cout << "press ENTER to continue" << std::endl;
     while( std::cin.get() != '\n' );
 
     while( true )
     {
-        servo.setPwm( 0, 0, 600 );
-        std::cout << "pwm: 600" << std::endl;
+        int keyPressed = std::cin.get();
+        if( keyPressed == ' ' )
+        {
+            std::cout << "KILL MOTOR" << std::endl;
+            currentValue = MOTOR_MIN;
+            servo.setPwm( 0, 0, currentValue );
+            std::cout << "pwm: " << currentValue << std::endl;
+        }
+        else if( keyPressed == 'w' )  // up arrow
+        {
+            std::cout << "up pressed" << std::endl;
+            currentValue += 5;
+            if( currentValue > MOTOR_MAX ) currentValue = MOTOR_MAX;
+            servo.setPwm( 0, 0, currentValue );
+            std::cout << "pwm: " << currentValue << std::endl;
+        }
+        else if( keyPressed == 's' ) // down arrow
+        {
+            std::cout << "down pressed" << std::endl;
+            currentValue -= 5;
+            if( currentValue < MOTOR_MIN ) currentValue = MOTOR_MIN;
+            servo.setPwm( 0, 0, currentValue );
+            std::cout << "pwm: " << currentValue << std::endl;
+        }
+
+        /*
+        servo.setPwm( 0, 0, MOTOR_MAX );
+        std::cout << "pwm: " << MOTOR_MAX << std::endl;
         delay( 1000 );
-        servo.setPwm( 0, 0, 150 );
-        std::cout << "pwm: 150" << std::endl;
+        servo.setPwm( 0, 0, MOTOR_MIN );
+        std::cout << "pwm: " << MOTOR_MIN << std::endl;
         delay( 1000 );
+        */
     }
 }
diff --git a/CopterController/PCA9685/echoServerAdvanced.cpp b/CopterController/PCA9685/echoServerAdvanced.cpp
new file mode 100644
index 0000000..824d537
--- /dev/null
+++ b/CopterController/PCA9685/echoServerAdvanced.cpp
@@ -0,0 +1,126 @@
+#include <stdio.h>
+#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 "wiringPi.h"
+#include "PCA9685.h"
+
+#define MOTOR_MIN 200
+#define MOTOR_MAX 600
+#define SERVO_FREQ 60
+
+int sockfd, newsockfd;
+socklen_t clilen;
+char buffer[256];
+struct sockaddr_in serv_addr, cli_addr;
+int nread, nwrite;
+
+void setupTCP( int portno )
+{
+    printf( "Setting up socket\n" );
+    sockfd = socket( AF_INET, SOCK_STREAM, 0 );
+    if( sockfd < 0 ) 
+        printf( "ERROR opening socket" );
+
+    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);
+
+    printf( "Binding socket to portno\n" );
+    if( bind( sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr) ) < 0 ) 
+            printf("ERROR on binding");
+
+    printf( "Listening\n" );
+    listen( sockfd, 5 );
+    clilen = sizeof(cli_addr);
+
+    printf( "Waiting for connection\n" );
+    newsockfd = accept( sockfd, 
+                (struct sockaddr *) &cli_addr, 
+                &clilen );
+    if( newsockfd < 0 ) 
+        printf( "ERROR on accept" );
+
+    printf( "Connected\n" );
+    bzero( buffer, 256 );
+    fcntl( newsockfd, F_SETFL, O_NONBLOCK );
+}
+
+int main( int argc, char* argv[] )
+{
+
+    setupTCP( 51717 );
+
+    wiringPiSetup();
+    PCA9685 servo = PCA9685( 0x40 );
+    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 );
+
+    while( true )
+    {
+        nread = read( newsockfd, buffer, 255 );
+        if( nread < 0 )
+        {
+            //printf( "ERROR on read" );
+        }
+        else if( nread != 0 )
+        {
+            nwrite = write( newsockfd, buffer, strlen(buffer) );
+            if( nwrite < 0 )
+                printf( "ERROR on write" );
+            //PARSE
+            printf( "Just read: %s", buffer );
+            if( strncmp( buffer, "DISCONNECT", strlen("DISCONNECT") ) == 0 )
+            {
+                printf( "Closing Socket\n" );
+                close( newsockfd );
+                close( sockfd );
+                return 0;
+            }
+            else if( strncmp( buffer, "HEARTBEAT", strlen("HEARTBEAT") ) == 0 )   // hearbeat transmission
+            {
+                // format:
+                // HEARTBEAT FCENABLE_x 
+                // FCENABLE_x flight controller enable/disable x=0: disabled x=1: enabled
+            }
+            else if( strncmp( buffer, "SET_MOTOR_", strlen("SET_MOTOR_") ) == 0 )
+            {
+                int motor = atoi( buffer + 10 );
+                int speed = atoi( buffer + 12 );
+                if( speed < MOTOR_MIN )
+                {
+                    speed = MOTOR_MIN;
+                }
+                else if( speed > MOTOR_MAX )
+                {
+                    speed = MOTOR_MAX;
+                }
+                if( motor >= 0 && motor <= 3 )
+                {
+                    printf( "setting motor %d to %d\n", motor, speed );
+                    servo.setPwm( motor, 0, speed );
+                    delay(5);
+                    printf( "motor %d set to %d\n", motor, speed );
+                }
+            }
+        }
+        delay(5);
+    }
+}
+
+
+
+
+
+