v2 CopterController stuff. compiles, untested

This commit is contained in:
Brendan Haines 2015-03-03 23:43:50 -07:00
parent ad72a19c36
commit 4405f009b4
13 changed files with 688 additions and 22 deletions

63
CopterController/DSM2.cpp Normal file
View File

@ -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;
}

24
CopterController/DSM2.h Normal file
View File

@ -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

View File

@ -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 );
}
}

View File

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

View File

@ -1,18 +1,28 @@
all: PID.o I2Cdev.o MPU6050.o PCA9685.o echoServerAdvanced PCA9685tester main all: PID.o I2Cdev.o MPU6050.o PCA9685.o DSM2.o main v2main PCA9685tester echoServerAdvanced
#pidTest MPU6050dmp_tester PCA9685tester
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 CXXFLAGS = -DDMP_FIFO_RATE=9 -Wall -lwiringPi
I2Cdev.o MPU6050.o PCA9685.o PID.o: $(HDRS) 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 main: main.o PID.o PCA9685.o I2Cdev.o MPU6050.o
$(CXX) $^ -lwiringPi -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 PCA9685tester: PCA9685.o PCA9685tester.o
$(CXX) $^ -lwiringPi -o $@ $(CXX) $^ -lwiringPi -o $@

View File

@ -38,7 +38,7 @@ const int PCA9685_LED_ADDRS[] = {
0x36, 0x37, 0x38, 0x39, // 12 0x36, 0x37, 0x38, 0x39, // 12
0x3A, 0x3B, 0x3C, 0x3D, // 13 0x3A, 0x3B, 0x3C, 0x3D, // 13
0x3E, 0x3F, 0x40, 0x41, // 14 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_L 0xFA
#define PCA9685_ALL_LED_ON_H 0xFB #define PCA9685_ALL_LED_ON_H 0xFB

View File

@ -15,7 +15,9 @@ class Server {
try { try {
System.out.print( "Setting up server\n" ); 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(); skt = srvr.accept();
System.out.print( "Server has connected!\n" ); System.out.print( "Server has connected!\n" );

View File

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

View File

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

View File

@ -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();
}
}

View File

@ -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 );
}
}
}

View File

@ -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

214
CopterController/v2main.cpp Normal file
View File

@ -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();
}
}
}
}