From 4405f009b47877b1ba9d2d870b4125ac180bec5b Mon Sep 17 00:00:00 2001 From: Brendan Date: Tue, 3 Mar 2015 23:43:50 -0700 Subject: [PATCH] v2 CopterController stuff. compiles, untested --- CopterController/DSM2.cpp | 63 ++++++ CopterController/DSM2.h | 24 ++ CopterController/DSM2tester.cpp | 21 ++ CopterController/MPU6050_6Axis_MotionApps20.h | 2 + CopterController/Makefile | 24 +- CopterController/PCA9685_Addresses.h | 2 +- CopterController/TCPexample/Server.java | 4 +- CopterController/echoServerAdvanced.cpp | 6 +- CopterController/main.cpp | 37 ++- CopterController/v2EchoServer.cpp | 150 ++++++++++++ CopterController/v2Parser.cpp | 132 +++++++++++ CopterController/v2Parser.h | 31 +++ CopterController/v2main.cpp | 214 ++++++++++++++++++ 13 files changed, 688 insertions(+), 22 deletions(-) create mode 100644 CopterController/DSM2.cpp create mode 100644 CopterController/DSM2.h create mode 100644 CopterController/DSM2tester.cpp create mode 100644 CopterController/v2EchoServer.cpp create mode 100644 CopterController/v2Parser.cpp create mode 100644 CopterController/v2Parser.h create mode 100644 CopterController/v2main.cpp diff --git a/CopterController/DSM2.cpp b/CopterController/DSM2.cpp new file mode 100644 index 0000000..e00824d --- /dev/null +++ b/CopterController/DSM2.cpp @@ -0,0 +1,63 @@ +#include "DSM2.h" + +#include "wiringSerial.h" +#include + + +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; +} diff --git a/CopterController/DSM2.h b/CopterController/DSM2.h new file mode 100644 index 0000000..6a7120c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/CopterController/DSM2tester.cpp b/CopterController/DSM2tester.cpp new file mode 100644 index 0000000..c2a7420 --- /dev/null +++ b/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 ); + } +} \ No newline at end of file diff --git a/CopterController/MPU6050_6Axis_MotionApps20.h b/CopterController/MPU6050_6Axis_MotionApps20.h index e14e5f2..2d2c211 100644 --- a/CopterController/MPU6050_6Axis_MotionApps20.h +++ b/CopterController/MPU6050_6Axis_MotionApps20.h @@ -33,6 +33,8 @@ THE SOFTWARE. #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ #define _MPU6050_6AXIS_MOTIONAPPS20_H_ +#include +#include #include "I2Cdev.h" #include "helper_3dmath.h" diff --git a/CopterController/Makefile b/CopterController/Makefile index 52fbe77..ab2fc31 100644 --- a/CopterController/Makefile +++ b/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 $@ diff --git a/CopterController/PCA9685_Addresses.h b/CopterController/PCA9685_Addresses.h index 17e09fc..95dd824 100644 --- a/CopterController/PCA9685_Addresses.h +++ b/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 diff --git a/CopterController/TCPexample/Server.java b/CopterController/TCPexample/Server.java index 5c5dcc9..7542bb7 100644 --- a/CopterController/TCPexample/Server.java +++ b/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" ); diff --git a/CopterController/echoServerAdvanced.cpp b/CopterController/echoServerAdvanced.cpp index 5b88480..1e92d68 100644 --- a/CopterController/echoServerAdvanced.cpp +++ b/CopterController/echoServerAdvanced.cpp @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include #include @@ -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); } diff --git a/CopterController/main.cpp b/CopterController/main.cpp index 996514e..70da9cc 100644 --- a/CopterController/main.cpp +++ b/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; diff --git a/CopterController/v2EchoServer.cpp b/CopterController/v2EchoServer.cpp new file mode 100644 index 0000000..8cb1323 --- /dev/null +++ b/CopterController/v2EchoServer.cpp @@ -0,0 +1,150 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#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(); + } +} + + + + + + diff --git a/CopterController/v2Parser.cpp b/CopterController/v2Parser.cpp new file mode 100644 index 0000000..b7ce8f4 --- /dev/null +++ b/CopterController/v2Parser.cpp @@ -0,0 +1,132 @@ +/* +* v2Parser.cpp +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 ); + } + } +} \ No newline at end of file diff --git a/CopterController/v2Parser.h b/CopterController/v2Parser.h new file mode 100644 index 0000000..0dc37dc --- /dev/null +++ b/CopterController/v2Parser.h @@ -0,0 +1,31 @@ +/* +* v2Parser.h +*/ + +#include +#include +#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 \ No newline at end of file diff --git a/CopterController/v2main.cpp b/CopterController/v2main.cpp new file mode 100644 index 0000000..2b8fa0c --- /dev/null +++ b/CopterController/v2main.cpp @@ -0,0 +1,214 @@ +/* +* v2main.cpp +*/ + +#include +#include +#include +#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(); + } + } + } +} + + + + + + + +