v2 CopterController stuff. compiles, untested

CopterController/DSM2.cpp

@@ -0,0 +1,63 @@
+#include "DSM2.h"
+
+#include "wiringSerial.h"
+#include <cstring>
+
+
+DSM2::DSM2( char* device, int mode )
+{
+
+    fd = serialOpen( device, 115200 );
+    if( mode != 1024 && mode != 2048 ) mode = 1024;
+    if( mode == 1024 )
+    {
+        valueSize = 10;
+        valueMask = 0x3FF;
+
+    }
+    else
+    {
+        valueSize = 11;
+        valueMask = 0x7FF;
+    }
+}
+
+bool DSM2::ready()
+{
+    if( serialDataAvail( fd ) >= 16 ) return true;
+    return false;
+}
+
+void DSM2::update( bool block )
+{
+    int i = 0;
+    if( block )
+    {
+        while( !ready() );
+    }
+    else if( !ready() ) return;
+
+    for( i = 0; i < 8; i++ ) readNext();
+    return;
+}
+
+void DSM2::readNext()
+{
+    int channel, value, raw;
+    raw = ( serialGetchar( fd ) << 8 ) + serialGetchar( fd );
+
+    if( lastReadChan == chanBeforeFl )
+    {
+        frameLoss = raw << 1 >> 1;
+        lastReadChan = -1;
+        return;
+    }
+
+    channel = (raw & 0x7FFF) >> valueSize;
+    value = raw & valueMask;
+    values[ channel ] = value;
+
+    lastReadChan = channel;
+
+    return;
+}

CopterController/DSM2.h

@@ -0,0 +1,24 @@
+#ifndef DSM2_H
+#define DSM2_H
+
+#define chanBeforeFl -1
+
+class DSM2 {
+public:
+    DSM2( char* device = "/dev/ttyAMA0", int mode = 1024 );
+    
+    bool ready();
+    void update( bool block = false );
+    void readNext();
+
+    int* values;
+    int frameLoss;
+private:
+    int lastReadChan;
+
+    int fd; // file descriptor for serial bus
+    int valueSize;  // number of bits for value part of frame
+    int valueMask;
+};
+
+#endif

CopterController/DSM2tester.cpp

@@ -0,0 +1,21 @@
+#include "DSM2.h"
+
+using namespace std;
+
+int main( int argc, char* argv[] )
+{
+    DSM2 dsm();
+
+    while( true )
+    {
+        cout << "Thro" << dsm.values[0] <<
+                "Aile" << dsm.values[1] << 
+                "Elev" << dsm.values[2] << 
+                "Rudd" << dsm.values[3] << 
+                "Gear" << dsm.values[4] << 
+                "Aux1" << dsm.values[5] << 
+                "FL" << dsm.frameLoss << endl;
+
+        dsm.update( true );
+    }
+}

CopterController/MPU6050_6Axis_MotionApps20.h

@@ -33,6 +33,8 @@ THE SOFTWARE.
 #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_
 #define _MPU6050_6AXIS_MOTIONAPPS20_H_
 
+#include <cstdio>
+#include <cstring>
 #include "I2Cdev.h"
 #include "helper_3dmath.h"
 

CopterController/Makefile

@@ -1,18 +1,28 @@
-all: PID.o I2Cdev.o MPU6050.o PCA9685.o echoServerAdvanced PCA9685tester main
-#pidTest MPU6050dmp_tester PCA9685tester
+all: PID.o I2Cdev.o MPU6050.o PCA9685.o DSM2.o main v2main PCA9685tester echoServerAdvanced
+
+PCA_HDRS = PCA9685.h PCA9685_Addresses.h
+MPU_HDRS = helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.h
+PID_HDRS = PID.h
+DSM_HDRS = DSM2.h
+HDRS = $(PID_HDRS) $(MPU_HDRS) $(PCA_HDRS)
 
-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
 
 I2Cdev.o MPU6050.o PCA9685.o PID.o: $(HDRS)
-MPU6050dmp_tester.o PCA9685tester.o echoServerAdvanced.o main.o: $(HDRS)
+PCA9685tester.o echoServerAdvanced.o main.o: $(HDRS)
+
+# version 2 stuff
+v2Parser.o: v2Parser.h helper_3dmath.h
+DSM2.o: $(DSM_HDRS)
+PID.o: $(PID_HDRS)
+v2main.o: v2Parser.h $(PCA_HDRS) $(MPU_HDRS) $(PID_HDRS)
+
+v2main: v2main.o v2Parser.o PCA9685.o PID.o I2Cdev.o MPU6050.o DSM2.o
+	$(CXX) $^ -lwiringPi -o $@
 
 main: main.o PID.o PCA9685.o I2Cdev.o MPU6050.o
 	$(CXX) $^ -lwiringPi -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 $@
 

CopterController/PCA9685_Addresses.h

@@ -38,7 +38,7 @@ const int PCA9685_LED_ADDRS[] = {
     0x36, 0x37, 0x38, 0x39, // 12
     0x3A, 0x3B, 0x3C, 0x3D, // 13
     0x3E, 0x3F, 0x40, 0x41, // 14
-    0x42, 0x43, 0x44, 0x45 }// 15
+    0x42, 0x43, 0x44, 0x45 };// 15
 
 #define PCA9685_ALL_LED_ON_L    0xFA
 #define PCA9685_ALL_LED_ON_H    0xFB

CopterController/TCPexample/Server.java

@@ -15,7 +15,9 @@ class Server {
 
       try {
          System.out.print( "Setting up server\n" );
-         srvr = new ServerSocket( 51717 );
+         srvr = new ServerSocket( Integer.parseInt( args[0] ) );
+         System.out.println( srvr.getInetAddress() );
+         System.out.println( srvr.getLocalPort() );
          skt = srvr.accept();
          System.out.print( "Server has connected!\n" );
 

CopterController/echoServerAdvanced.cpp

@@ -2,7 +2,7 @@
 
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
+#include <cstring>
 #include <unistd.h>
 #include <sys/types.h> 
 #include <sys/socket.h>
@@ -76,7 +76,7 @@ int main( int argc, char* argv[] )
         {
             //printf( "ERROR on read" );
         }
-        else if( nread != 0 )
+        else if( nread > 0 )
         {
             nwrite = write( newsockfd, buffer, strlen(buffer) );
             if( nwrite < 0 )
@@ -116,6 +116,8 @@ int main( int argc, char* argv[] )
                     printf( "motor %d set to %d\n", motor, speed );
                 }
             }
+            bzero( buffer, 256 );
+            nread  = 0;
         }
         delay(5);
     }

CopterController/main.cpp

@@ -19,6 +19,7 @@
 #include "MPU6050_6Axis_MotionApps20.h"
 
 // TCP stuff
+#define TCP_PORT 51717
 int sockfd, newsockfd;
 char buffer[256];
 
@@ -31,11 +32,13 @@ MPU6050 mpu;
 #define MOTOR_OFF 200
 #define MOTOR_MIN 220
 #define MOTOR_MAX 600
+const int MOTOR_ON_RANGE = MOTOR_MAX - MOTOR_MIN;
 #define SERVO_FREQ 60
 PCA9685 servo;
 
 // PID stuff
 #define P_GAIN 2
+#define YAW_P_GAIN 0.0001
 #define SIN_45 0.70710678118
 PID pitchPID, rollPID, yawPID;
 int throttle = (int)( ( MOTOR_MAX - MOTOR_MIN ) / 2 + MOTOR_MIN );
@@ -76,15 +79,19 @@ void setupTCP( int portno )
 void setupPCA()
 {
     servo = PCA9685( 0x40 );
+    delay( 5 );
     servo.setFrequency( SERVO_FREQ );
+    delay( 5 );
     servo.setPwm( 0, 0, MOTOR_OFF );
-    delay(1);
     servo.setPwm( 1, 0, MOTOR_OFF );
-    delay(1);
     servo.setPwm( 2, 0, MOTOR_OFF );
-    delay(1);
     servo.setPwm( 3, 0, MOTOR_OFF );
-    delay(1);
+    delay( 500 );
+    servo.setPwm( 0, 0, MOTOR_OFF );
+    servo.setPwm( 1, 0, MOTOR_OFF );
+    servo.setPwm( 2, 0, MOTOR_OFF );
+    servo.setPwm( 3, 0, MOTOR_OFF );
+    delay( 5 );
 }
 
 /**
@@ -129,13 +136,19 @@ int setupMPU()
     return 0;
 }
 
+void setupPID()
+{
+    pitchPID = PID( P_GAIN, 0, 0 );
+    rollPID = PID( P_GAIN, 0, 0 );
+    yawPID = PID( YAW_P_GAIN, 0, 0 );
+}
+
 void setup()
 {
     setupPCA();
-    setupTCP( 51717 );
+    setupTCP( TCP_PORT );
     setupMPU();
-    pitchPID = PID( P_GAIN, 0, 0 );
-    rollPID = PID( P_GAIN, 0, 0 );
+    setupPID();
 }
 
 void loop()
@@ -215,10 +228,12 @@ void loop()
         rollMod = rollPID.update( rollAngle, 0 );
         yawMod = yawPID.update( yawRate, 0 );
 
-        m0 = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * throttle );
-        m1 = (int)( ( 1 + pitchMod ) * ( 1 - rollMod ) * throttle );
-        m2 = (int)( ( 1 - pitchMod ) * ( 1 + rollMod ) * throttle );
-        m3 = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * throttle );
+        std::cout << "\t" << pitchMod << "\t" << rollMod << "\t" << yawMod;
+
+        m0 = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * ( 1 + yawMod ) * throttle );
+        m1 = (int)( ( 1 + pitchMod ) * ( 1 - rollMod ) * ( 1 - yawMod ) * throttle );
+        m2 = (int)( ( 1 - pitchMod ) * ( 1 + rollMod ) * ( 1 - yawMod ) * throttle );
+        m3 = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * ( 1 + yawMod ) * throttle );
 
         if( m0 < MOTOR_MIN ) m0 = MOTOR_MIN;
         if( m0 > MOTOR_MAX ) m0 = MOTOR_MAX;

CopterController/v2EchoServer.cpp

@@ -0,0 +1,150 @@
+#include <iostream>
+#include <cstdio>
+#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"
+
+using namespace std;
+
+// TCP vars
+#define TCP_PORT 51718
+int sockfd, newsockfd;
+char buffer[ 256 ];
+struct sockaddr_in serv_addr, cli_addr;
+socklen_t clilen;
+
+// Status vars
+bool motorsEnabled;
+int motorValues[ 4 ];
+int controlValues[ 6 ];
+//Quaternion q;
+
+bool setupTcp( int portNum )
+{
+    struct sockaddr_in serv_addr, cli_addr;
+    socklen_t clilen;
+
+    cout << "setting up TCP..." << endl;
+
+    sockfd = socket( AF_INET, SOCK_STREAM, 0 );
+    if( sockfd < 0 )
+    {
+        cerr << "ERROR opening socket" << endl;
+        return false;
+    }
+
+    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(portNum);
+
+    cout << "binding socket to port " << portNum << endl;
+    if( bind( sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr) ) )
+    {
+        cerr << "ERROR binding socket to port " << portNum << endl;
+        return false;
+    }
+
+    cout << "listening to socket" << endl;
+    listen( sockfd, 5 );
+    clilen = sizeof( cli_addr );
+    
+    cout << "waiting for connection" << endl;
+    newsockfd = accept( sockfd, (struct sockaddr *) &cli_addr, &clilen );
+    if( newsockfd < 0 )
+    {
+        cerr << "ERROR on accept" << endl;
+        return false;
+    }
+
+    bzero( buffer, 256 );
+    fcntl( newsockfd, F_SETFL, O_NONBLOCK );
+    
+    cout << "connection established" << endl;
+    return true;
+}
+
+void shutdown()
+{
+    cout << "shutting down..." << endl;
+
+    close( newsockfd );
+    close( sockfd );
+    cout << "sockets closed" << endl;
+
+    exit( EXIT_SUCCESS );
+}
+
+void readTcp()
+{
+    int n;  // number of bytes read
+    int carotPos;
+    n = read( newsockfd, buffer, 255 );
+    if( n > 0 )
+    {
+        motorsEnabled = false;
+        carotPos = 0;
+        while( carotPos < n )
+        {
+            if( strncmp( buffer + carotPos, "DISCONNECT", strlen("DISCONNECT") ) == 0 && strlen("DISCONNECT") + carotPos <= n ) shutdown();
+            else if( strncmp( buffer + carotPos, "D", strlen("D") ) == 0 && strlen("D") + carotPos <= n ) shutdown();
+            else if( strncmp( buffer + carotPos, "H", strlen("H") ) == 0 && strlen("H") + carotPos <= n ) carotPos++;
+            else if( strncmp( buffer + carotPos, "E", strlen("E") ) == 0 && strlen("E") + carotPos <= n )
+            {
+                motorsEnabled = true;
+                cout << "E ";
+                carotPos += strlen("E ");
+            }
+            else if( strncmp( buffer + carotPos, "M", strlen("M") ) == 0 && strlen("Mxx_xxxx") + carotPos <= n )
+            {
+                motorValues[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 );
+                carotPos += strlen("Mxx_xxxx ");
+            }
+            else if( strncmp( buffer + carotPos, "C", strlen("C") ) == 0 && strlen("Cxx_xxxx") + carotPos <= n )
+            {
+                controlValues[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 );
+                carotPos += strlen("Cxx_xxxx ");
+            }
+            else carotPos++;
+        }
+        cout << endl;
+    }
+}
+
+void writeTcp()
+{
+    int i;
+    char temp[256];
+    char outBuffer[] = "H ";
+
+    if( motorsEnabled ) {
+        strcat( outBuffer, "E " );
+
+        for( i = 0; i < 4; i++ )
+        {
+            sprintf( temp, "M%2d_%4d ", i, motorValues[ i ] );
+            strcat( outBuffer, temp );
+        }
+    }
+}
+
+int main( int argc, char* argv[] )
+{
+    //if( !setupTcp( TCP_PORT ) ) exit( EXIT_FAILURE );
+    setupTcp( TCP_PORT );
+    while( true )
+    {
+        readTcp();
+        writeTcp();
+    }
+}
+
+
+
+
+
+

CopterController/v2Parser.cpp

@@ -0,0 +1,132 @@
+/*
+* v2Parser.cpp
+*/
+
+#include <iostream>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#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 "v2Parser.h"
+
+using namespace std;
+
+v2Parser::v2Parser( int port )
+{
+    setup( port );
+}
+
+bool v2Parser::setup( int portNum )
+{
+    struct sockaddr_in serv_addr, cli_addr;
+    socklen_t clilen;
+
+    cout << "setting up TCP..." << endl;
+
+    sockfd = socket( AF_INET, SOCK_STREAM, 0 );
+    if( sockfd < 0 )
+    {
+        cerr << "ERROR opening socket" << endl;
+        return false;
+    }
+
+    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(portNum);
+
+    cout << "binding socket to port " << portNum << endl;
+    if( bind( sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr) ) )
+    {
+        cerr << "ERROR binding socket to port " << portNum << endl;
+        return false;
+    }
+
+    cout << "listening to socket" << endl;
+    listen( sockfd, 5 );
+    clilen = sizeof( cli_addr );
+    
+    cout << "waiting for connection" << endl;
+    newsockfd = accept( sockfd, (struct sockaddr *) &cli_addr, &clilen );
+    if( newsockfd < 0 )
+    {
+        cerr << "ERROR on accept" << endl;
+        return false;
+    }
+
+    bzero( buffer, 256 );
+    fcntl( newsockfd, F_SETFL, O_NONBLOCK );
+    
+    cout << "connection established" << endl;
+    return true;
+}
+
+bool v2Parser::recieve()
+{
+    size_t n;  // number of bytes read
+    uint8_t carotPos;
+    int channel, value;
+
+    n = read( newsockfd, buffer, 255 );
+    if( n > 0 )
+    {
+        motorsEnabled = false;
+        carotPos = 0;
+        while( carotPos < n )
+        {
+            if( strncmp( buffer + carotPos, "DISCONNECT", strlen("DISCONNECT") ) == 0 && strlen("DISCONNECT") + carotPos <= n ) exit( EXIT_SUCCESS );
+            else if( strncmp( buffer + carotPos, "D", strlen("D") ) == 0 && strlen("D") + carotPos <= n ) exit( EXIT_SUCCESS );
+            else if( strncmp( buffer + carotPos, "H", strlen("H") ) == 0 && strlen("H") + carotPos <= n ) carotPos++;
+            else if( strncmp( buffer + carotPos, "E", strlen("E") ) == 0 && strlen("E") + carotPos <= n )
+            {
+                motorsEnabled = true;
+                cout << "E ";
+                carotPos += strlen("E ");
+            }
+            else if( strncmp( buffer + carotPos, "M", strlen("M") ) == 0 && strlen("Mxx_xxxx") + carotPos <= n )
+            {
+                channel = atoi( buffer + carotPos + 1 );
+                value = atoi( buffer + carotPos + 4 );
+                motorValues[ channel ] = value;
+                cout << "M" << channel << endl;
+                carotPos += strlen("Mxx_xxxx ");
+            }
+            else if( strncmp( buffer + carotPos, "C", strlen("C") ) == 0 && strlen("Cxx_xxxx") + carotPos <= n )
+            {
+                channel = atoi( buffer + carotPos + 1 );
+                value = atoi( buffer + carotPos + 4 );
+                motorValues[ channel ] = value;
+                cout << "C" << channel << endl;
+                carotPos += strlen("Cxx_xxxx ");
+            }
+            else carotPos++;
+        }
+        cout << endl;
+        return true;
+    }
+    return false;
+}
+
+void v2Parser::send()
+{
+    int i;
+    char temp[256];
+    char outBuffer[] = "H ";
+
+    if( motorsEnabled ) {
+        strcat( outBuffer, "E " );
+
+        for( i = 0; i < 4; i++ )
+        {
+            sprintf( temp, "M%2d_%4d ", i, motorValues[ i ] );
+            strcat( outBuffer, temp );
+        }
+    }
+}

CopterController/v2Parser.h

@@ -0,0 +1,31 @@
+/*
+* v2Parser.h
+*/
+
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include "helper_3dmath.h"
+
+#ifndef V2PARSER_H
+#define V2PARSER_H
+
+class v2Parser
+{
+public:
+    v2Parser( int port = 51717 );
+    bool setup( int portNum );
+    bool recieve();
+    void send();
+
+    bool motorsEnabled;
+    int motorValues[ 4 ];
+    int controlValues[ 6 ];
+    Quaternion q;
+private:
+    int sockfd, newsockfd;
+    char buffer[256];
+    struct sockaddr_in serv_addr, cli_addr;
+    socklen_t clilen;
+};
+
+#endif

CopterController/v2main.cpp

@@ -0,0 +1,214 @@
+/*
+* v2main.cpp
+*/
+
+#include <iostream>
+#include <cstdlib>
+#include <stdint.h>
+#include "v2Parser.h"
+#include "PCA9685.h"
+#include "DSM2.h"
+#include "PID.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "wiringPi.h"
+
+using namespace std;
+
+#define DEFAULT_TCP_PORT 51717
+
+#define MOTOR_OFF 200
+#define MOTOR_MIN 220
+#define MOTOR_MAX 600
+
+double pitchP   = 2;
+double pitchI   = 0;
+double pitchD   = 0;
+double rollP    = 2;
+double rollI    = 0;
+double rollD    = 0;
+double yawP     = 0.0001;
+double yawI     = 0;
+double yawD     = 0;
+
+v2Parser tcp;
+PCA9685 pca;
+DSM2 dsm;
+MPU6050 mpu;
+
+PID pitchPID;
+PID rollPID;
+PID yawPID;
+
+VectorFloat gravity;
+int fifoPacketSize;
+
+void setAllMotorsOff()
+{
+    int i;
+    for( i = 0; i < 16; i++ )
+        pca.setPwm( i, 0, 0 );
+}
+
+void setup( int tcpPort )
+{
+    cout << "setting up PCA9685 (motor controller)..." << endl;
+        pca = PCA9685( 0x40 );
+        pca.setFrequency( 60 );
+        setAllMotorsOff();
+        delay( 250 );
+        setAllMotorsOff();
+        cout << "motors set to " << MOTOR_OFF << " (OFF)" << endl;
+    cout << "PCA9685 set up" << endl;
+
+    cout << "setting up TCP link..." << endl;
+        tcp = v2Parser( tcpPort );
+    cout << "TCP set up" << endl;
+
+    cout << "setting up RC receiver..." << endl;
+        dsm = DSM2();
+        dsm.values = tcp.controlValues;
+    cout << "RC receiver set up" << endl;
+
+    cout << "setting up MPU6050..." << endl;
+        cout << "\tinitializing MPU6050..." << endl;
+        mpu.initialize();
+        cout << "\ttesting MPU6050 connection..." << flush;
+        if( mpu.testConnection() )
+        {
+            cout << "SUCCESS" << endl;
+        }
+        else
+        {
+            cout << "FAILURE" << endl;
+            exit( EXIT_FAILURE );
+        }
+        cout << "\tinitializing DMP..." << flush;
+        if( mpu.dmpInitialize() == 0 )
+        {
+            cout << "\tSUCCESS" << endl;
+            cout << "\tEnabling DMP..." << endl;
+            mpu.setDMPEnabled( true );
+            fifoPacketSize = mpu.dmpGetFIFOPacketSize();
+        }
+        else
+        {
+            cout << "\tFAILURE" << endl;
+            exit( EXIT_FAILURE );
+        }
+    cout << "MPU6050 set up" << endl;
+
+    cout << "setting up PID..." << endl;
+        pitchPID = PID( pitchP, pitchI, pitchD );
+        rollPID = PID( rollP, rollI, rollD );
+        yawPID = PID( yawP, yawI, yawD );
+    cout << "PID set up" << endl;
+
+    atexit( setAllMotorsOff );
+}
+
+bool updateMpu()
+{
+    int fifoCount;
+    uint8_t fifoBuffer[64];
+    fifoCount = mpu.getFIFOCount();
+    if( fifoCount == 1024 )
+    {
+        cout << "FIFO overflow" << endl;
+    }
+    else if( fifoCount >= 42 )
+    {
+        while( fifoCount >= 84 )
+        {
+            mpu.getFIFOBytes( fifoBuffer, fifoPacketSize );
+            fifoCount = mpu.getFIFOCount();
+        }
+
+        mpu.getFIFOBytes( fifoBuffer, fifoPacketSize );
+        mpu.dmpGetQuaternion( &(tcp.q), fifoBuffer );
+        mpu.dmpGetGravity( &gravity, &(tcp.q) );
+
+        return true;
+    }
+    return false;
+}
+
+void constrainValue( int* in, int low, int high )
+{
+    if( *in < low )
+    {
+        *in = low;
+    }
+    else if( *in > high )
+    {
+        *in = high;
+    }
+}
+
+int main( int argc, char* argv[] )
+{
+    float pitchAngle;
+    float rollAngle;
+    int16_t yawRate;
+
+    float pitchMod = 1;
+    float rollMod = 1;
+    float yawMod = 1;
+
+    if( argc < 1 )
+    {
+        setup( DEFAULT_TCP_PORT );
+    }
+    else
+    {
+        setup( *argv[0] );
+    }
+
+    //dsm.sync();
+    while( true )
+    {
+        //dsm.update();
+        tcp.send();
+        tcp.recieve();
+        if( updateMpu() )
+        {
+            pitchAngle = atan( gravity.x / sqrt( gravity.y * gravity.y + gravity.z * gravity.z) );
+            rollAngle = atan( gravity.y / sqrt( gravity.x * gravity.x + gravity.z * gravity.z ) );
+            yawRate = mpu.getRotationZ();
+            cout << "pitch: " << pitchAngle << "\troll: " << rollAngle << "\tyaw rate: " << yawRate << endl;
+
+            pitchMod = pitchPID.update( pitchAngle, 0 );
+            rollMod = rollPID.update( rollAngle, 0 );
+            yawMod = yawPID.update( yawRate, 0 );
+
+            if( tcp.motorsEnabled )
+            {
+                tcp.motorValues[ 0 ] = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * ( 1 + yawMod ) * tcp.controlValues[0] );
+                tcp.motorValues[ 1 ] = (int)( ( 1 + pitchMod ) * ( 1 - rollMod ) * ( 1 - yawMod ) * tcp.controlValues[0] );
+                tcp.motorValues[ 2 ] = (int)( ( 1 - pitchMod ) * ( 1 + rollMod ) * ( 1 - yawMod ) * tcp.controlValues[0] );
+                tcp.motorValues[ 3 ] = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * ( 1 + yawMod ) * tcp.controlValues[0] );
+
+                constrainValue( &(tcp.motorValues[ 0 ]), MOTOR_MIN, MOTOR_MAX );
+                constrainValue( &(tcp.motorValues[ 1 ]), MOTOR_MIN, MOTOR_MAX );
+                constrainValue( &(tcp.motorValues[ 2 ]), MOTOR_MIN, MOTOR_MAX );
+                constrainValue( &(tcp.motorValues[ 3 ]), MOTOR_MIN, MOTOR_MAX );
+
+                pca.setPwm( 0, 0, tcp.motorValues[ 0 ] );
+                pca.setPwm( 1, 0, tcp.motorValues[ 1 ] );
+                pca.setPwm( 2, 0, tcp.motorValues[ 2 ] );
+                pca.setPwm( 3, 0, tcp.motorValues[ 3 ] );
+            }
+            else
+            {
+                setAllMotorsOff();
+            }
+        }
+    }
+}
+
+
+
+
+
+
+
+