From c3292e033ce1e2daa1efe8357b231f7cc2d86e30 Mon Sep 17 00:00:00 2001 From: Brendan Date: Tue, 17 Mar 2015 23:35:18 -0600 Subject: [PATCH] I don't think any of these files will end up being used, just committing them to preserve them on the off-chance that they are eventually needed --- CopterController/Makefile | 11 +--- CopterController/v2EchoServer.cpp | 50 ++++++++++++---- CopterController/v2Parser.cpp | 8 ++- CopterController/v2Parser.h | 3 +- CopterController/v2main.cpp | 97 ++++++++++++++++--------------- 5 files changed, 97 insertions(+), 72 deletions(-) diff --git a/CopterController/Makefile b/CopterController/Makefile index bb9d212..9ee0c97 100644 --- a/CopterController/Makefile +++ b/CopterController/Makefile @@ -1,6 +1,6 @@ all: libraries oneThatWorks -libraries: PID.o I2Cdev.o MPU6050.o PCA9685.o DSM2.o PID.o v2Parser.o +libraries: PID.o I2Cdev.o MPU6050.o PCA9685.o DSM2.o PID.o PCA_HDRS = PCA9685.h PCA9685_Addresses.h MPU_HDRS = helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.h @@ -12,21 +12,12 @@ CXXFLAGS = -DDMP_FIFO_RATE=9 -Wall -lwiringPi I2Cdev.o MPU6050.o PCA9685.o PID.o: $(HDRS) PCA9685tester.o echoServerAdvanced.o main.o: $(HDRS) - -v2Parser.o: v2Parser.h helper_3dmath.h PID.o: $(PID_HDRS) DSM2.o DSM2tester.o: $(DSM_HDRS) DSM2tester: DSM2tester.o DSM2.o $(CXX) $^ -lwiringPi -o $@ -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 $@ - -v2EchoServer: v2EchoServer.cpp - $(CXX) $^ -o $@ - main: main.o PID.o PCA9685.o I2Cdev.o MPU6050.o $(CXX) $^ -lwiringPi -o $@ diff --git a/CopterController/v2EchoServer.cpp b/CopterController/v2EchoServer.cpp index 1dae33f..dec30d9 100644 --- a/CopterController/v2EchoServer.cpp +++ b/CopterController/v2EchoServer.cpp @@ -10,19 +10,26 @@ #include #include "fcntl.h" +#include "DSM2.h" + using namespace std; // TCP vars -#define TCP_PORT 51717 +#define TCP_PORT 51714 int sockfd, newsockfd; char buffer[ 256 ]; struct sockaddr_in serv_addr, cli_addr; socklen_t clilen; +// DSM2 receiver vars +const int RX_MAX[] = { 900, 900, 900, 900, 900, 900 }; +const int RX_CENTER[] = { 511, 511, 511, 511, 511, 511 }; +const int RX_MIN[] = { 122, 122, 122, 122, 122, 122 }; +DSM2 dsm; + // Status vars bool motorsEnabled; int motorValues[ 4 ]; -int controlValues[ 6 ]; //Quaternion q; bool setupTcp( int portNum ) @@ -94,26 +101,39 @@ void readTcp() { 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, "H", strlen("H") ) == 0 && strlen("H") + carotPos <= n ) + { + cout << "H " << flush; + carotPos++; + } else if( strncmp( buffer + carotPos, "E", strlen("E") ) == 0 && strlen("E") + carotPos <= n ) { motorsEnabled = true; - cout << "E "; - carotPos += strlen("E "); + cout << "E " << flush; + carotPos++; +// 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 "); + carotPos += strlen("Mxx_xxxx"); } - else if( strncmp( buffer + carotPos, "C", strlen("C") ) == 0 && strlen("Cxx_xxxx") + carotPos <= n ) + /*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 "); - } + dsm.values[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 ); + carotPos += strlen("Cxx_xxxx"); + }*/ else carotPos++; } cout << endl; + bzero( buffer, 256 ); + if( !motorsEnabled ) + { + motorValues[ 0 ] = 0; + motorValues[ 1 ] = 0; + motorValues[ 2 ] = 0; + motorValues[ 3 ] = 0; + } } } @@ -128,10 +148,16 @@ void writeTcp() for( i = 0; i < 4; i++ ) { - sprintf( temp, "M%2d_%4d ", i, motorValues[ i ] ); + sprintf( temp, "M%02d_%04d ", i, motorValues[ i ] ); strcat( outBuffer, temp ); } } + + for( i = 0; i < 6; i++ ) + { + sprintf( temp, "C%02d_%04d ", i, dsm.values[ i ] ); + strcat( outBuffer, temp ); + } } int main( int argc, char* argv[] ) @@ -141,7 +167,9 @@ int main( int argc, char* argv[] ) while( true ) { readTcp(); + cout << "EN: " << motorsEnabled << "\tM0: " << motorValues[0] << "\tM1: " << motorValues[1] << "\tM2: " << motorValues[2] << "\tM3: " << motorValues[3] << endl; writeTcp(); + } } diff --git a/CopterController/v2Parser.cpp b/CopterController/v2Parser.cpp index b7ce8f4..a39ef49 100644 --- a/CopterController/v2Parser.cpp +++ b/CopterController/v2Parser.cpp @@ -18,6 +18,10 @@ using namespace std; +// No-Args constructor +v2Parser::v2Parser() +{} + v2Parser::v2Parser( int port ) { setup( port ); @@ -98,14 +102,14 @@ bool v2Parser::recieve() cout << "M" << channel << endl; carotPos += strlen("Mxx_xxxx "); } - else if( strncmp( buffer + carotPos, "C", strlen("C") ) == 0 && strlen("Cxx_xxxx") + carotPos <= n ) + /*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; diff --git a/CopterController/v2Parser.h b/CopterController/v2Parser.h index 0dc37dc..db0c4d3 100644 --- a/CopterController/v2Parser.h +++ b/CopterController/v2Parser.h @@ -12,7 +12,8 @@ class v2Parser { public: - v2Parser( int port = 51717 ); + v2Parser(); + v2Parser( int port); bool setup( int portNum ); bool recieve(); void send(); diff --git a/CopterController/v2main.cpp b/CopterController/v2main.cpp index 2b8fa0c..c96d80a 100644 --- a/CopterController/v2main.cpp +++ b/CopterController/v2main.cpp @@ -30,8 +30,8 @@ double yawP = 0.0001; double yawI = 0; double yawD = 0; -v2Parser tcp; PCA9685 pca; +v2Parser tcp; DSM2 dsm; MPU6050 mpu; @@ -46,62 +46,62 @@ void setAllMotorsOff() { int i; for( i = 0; i < 16; i++ ) - pca.setPwm( i, 0, 0 ); + pca.setPwm( i, 0, MOTOR_OFF ); } void setup( int tcpPort ) { - cout << "setting up PCA9685 (motor controller)..." << endl; + cout << "\nsetting 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 << "PCA9685 set up\n" << endl; cout << "setting up TCP link..." << endl; tcp = v2Parser( tcpPort ); cout << "TCP set up" << endl; - cout << "setting up RC receiver..." << endl; + /*cout << "setting up RC receiver..." << endl; dsm = DSM2(); - dsm.values = tcp.controlValues; - cout << "RC receiver set up" << endl; + //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 << "\nsetting up MPU6050..." << endl; + cout << "initializing MPU6050..." << endl; + mpu.initialize(); + cout << "testing MPU6050 connection..." << flush; + if( mpu.testConnection() ) + { + cout << "SUCCESS" << endl; + } + else + { + cout << "FAILURE" << endl; + exit( EXIT_FAILURE ); + } + cout << "initializing DMP..." << flush; + if( mpu.dmpInitialize() == 0 ) + { + cout << "SUCCESS" << endl; + cout << "Enabling DMP..." << endl; + mpu.setDMPEnabled( true ); + fifoPacketSize = mpu.dmpGetFIFOPacketSize(); + } + else + { + cout << "FAILURE" << endl; + exit( EXIT_FAILURE ); + } cout << "MPU6050 set up" << endl; - cout << "setting up PID..." << endl; + cout << "\nsetting up PID..." << endl; pitchPID = PID( pitchP, pitchI, pitchD ); rollPID = PID( rollP, rollI, rollD ); yawPID = PID( yawP, yawI, yawD ); - cout << "PID set up" << endl; + cout << "PID set up\n" << endl; atexit( setAllMotorsOff ); } @@ -154,22 +154,18 @@ int main( int argc, char* argv[] ) float rollMod = 1; float yawMod = 1; - if( argc < 1 ) - { - setup( DEFAULT_TCP_PORT ); - } - else - { - setup( *argv[0] ); - } + setup( DEFAULT_TCP_PORT ); - //dsm.sync(); + cout << "starting loop" << endl; while( true ) { - //dsm.update(); - tcp.send(); - tcp.recieve(); - if( updateMpu() ) + dsm.update(); + cout << "dsm rx: " << dsm.values[0] << "\t" << dsm.values[1] << "\t" << dsm.values[2] << "\t" << dsm.values[3] << "\t" << dsm.values[4] << "\t" << dsm.values[5] << "\t" << endl; + //tcp.send(); + //cout << "tcp send done" << endl; + //cout<< tcp.recieve() <