brendanhaines/RasPi
1
0
Fork 0
mirror of https://github.com/brendanhaines/RasPi.git synced 2024-11-09 16:44:40 -07:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Brendan Haines 2015-03-17 23:35:18 -06:00
parent 52fea904ca
commit c3292e033c
5 changed files with 97 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
CopterController/Makefile
View File

@ -1,6 +1,6 @@
all: libraries oneThatWorks 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 PCA_HDRS = PCA9685.h PCA9685_Addresses.h
MPU_HDRS = helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.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) I2Cdev.o MPU6050.o PCA9685.o PID.o: $(HDRS)
PCA9685tester.o echoServerAdvanced.o main.o: $(HDRS) PCA9685tester.o echoServerAdvanced.o main.o: $(HDRS)
v2Parser.o: v2Parser.h helper_3dmath.h
PID.o: $(PID_HDRS) PID.o: $(PID_HDRS)
DSM2.o DSM2tester.o: $(DSM_HDRS) DSM2.o DSM2tester.o: $(DSM_HDRS)
DSM2tester: DSM2tester.o DSM2.o DSM2tester: DSM2tester.o DSM2.o
$(CXX) $^ -lwiringPi -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 main: main.o PID.o PCA9685.o I2Cdev.o MPU6050.o
$(CXX) $^ -lwiringPi -o $@ $(CXX) $^ -lwiringPi -o $@

50
CopterController/v2EchoServer.cpp
View File

@ -10,19 +10,26 @@
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include "fcntl.h" #include "fcntl.h"
#include "DSM2.h"
using namespace std; using namespace std;
// TCP vars // TCP vars
#define TCP_PORT 51717 #define TCP_PORT 51714
int sockfd, newsockfd; int sockfd, newsockfd;
char buffer[ 256 ]; char buffer[ 256 ];
struct sockaddr_in serv_addr, cli_addr; struct sockaddr_in serv_addr, cli_addr;
socklen_t clilen; 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 // Status vars
bool motorsEnabled; bool motorsEnabled;
int motorValues[ 4 ]; int motorValues[ 4 ];
int controlValues[ 6 ];
//Quaternion q; //Quaternion q;
bool setupTcp( int portNum ) bool setupTcp( int portNum )
@ -94,26 +101,39 @@ void readTcp()
{ {
if( strncmp( buffer + carotPos, "DISCONNECT", strlen("DISCONNECT") ) == 0 && strlen("DISCONNECT") + carotPos <= n ) shutdown(); 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, "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 ) else if( strncmp( buffer + carotPos, "E", strlen("E") ) == 0 && strlen("E") + carotPos <= n )
{ {
motorsEnabled = true; motorsEnabled = true;
cout << "E "; cout << "E " << flush;
carotPos += strlen("E "); carotPos++;
// carotPos += strlen("E");
} }
else if( strncmp( buffer + carotPos, "M", strlen("M") ) == 0 && strlen("Mxx_xxxx") + carotPos <= n ) else if( strncmp( buffer + carotPos, "M", strlen("M") ) == 0 && strlen("Mxx_xxxx") + carotPos <= n )
{ {
motorValues[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 ); 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 ); dsm.values[ atoi( buffer + carotPos + 1 ) ] = atoi( buffer + carotPos + 4 );
carotPos += strlen("Cxx_xxxx "); carotPos += strlen("Cxx_xxxx");
} }*/
else carotPos++; else carotPos++;
} }
cout << endl; 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++ ) 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 ); 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[] ) int main( int argc, char* argv[] )
@ -141,7 +167,9 @@ int main( int argc, char* argv[] )
while( true ) while( true )
{ {
readTcp(); readTcp();
cout << "EN: " << motorsEnabled << "\tM0: " << motorValues[0] << "\tM1: " << motorValues[1] << "\tM2: " << motorValues[2] << "\tM3: " << motorValues[3] << endl;
writeTcp(); writeTcp();
} }
} }

8
CopterController/v2Parser.cpp
View File

@ -18,6 +18,10 @@
using namespace std; using namespace std;
// No-Args constructor
v2Parser::v2Parser()
{}
v2Parser::v2Parser( int port ) v2Parser::v2Parser( int port )
{ {
setup( port ); setup( port );
@ -98,14 +102,14 @@ bool v2Parser::recieve()
cout << "M" << channel << endl; cout << "M" << channel << endl;
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 )
{ {
channel = atoi( buffer + carotPos + 1 ); channel = atoi( buffer + carotPos + 1 );
value = atoi( buffer + carotPos + 4 ); value = atoi( buffer + carotPos + 4 );
motorValues[ channel ] = value; motorValues[ channel ] = value;
cout << "C" << channel << endl; cout << "C" << channel << endl;
carotPos += strlen("Cxx_xxxx "); carotPos += strlen("Cxx_xxxx ");
} }*/
else carotPos++; else carotPos++;
} }
cout << endl; cout << endl;

3
CopterController/v2Parser.h
View File

@ -12,7 +12,8 @@
class v2Parser class v2Parser
{ {
public: public:
v2Parser( int port = 51717 ); v2Parser();
v2Parser( int port);
bool setup( int portNum ); bool setup( int portNum );
bool recieve(); bool recieve();
void send(); void send();

57
CopterController/v2main.cpp
View File

@ -30,8 +30,8 @@ double yawP = 0.0001;
double yawI = 0; double yawI = 0;
double yawD = 0; double yawD = 0;
v2Parser tcp;
PCA9685 pca; PCA9685 pca;
v2Parser tcp;
DSM2 dsm; DSM2 dsm;
MPU6050 mpu; MPU6050 mpu;
@ -46,33 +46,33 @@ void setAllMotorsOff()
{ {
int i; int i;
for( i = 0; i < 16; i++ ) for( i = 0; i < 16; i++ )
pca.setPwm( i, 0, 0 ); pca.setPwm( i, 0, MOTOR_OFF );
} }
void setup( int tcpPort ) void setup( int tcpPort )
{ {
cout << "setting up PCA9685 (motor controller)..." << endl; cout << "\nsetting up PCA9685 (motor controller)..." << endl;
pca = PCA9685( 0x40 ); pca = PCA9685( 0x40 );
pca.setFrequency( 60 ); pca.setFrequency( 60 );
setAllMotorsOff(); setAllMotorsOff();
delay( 250 ); delay( 250 );
setAllMotorsOff(); setAllMotorsOff();
cout << "motors set to " << MOTOR_OFF << " (OFF)" << endl; 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; cout << "setting up TCP link..." << endl;
tcp = v2Parser( tcpPort ); tcp = v2Parser( tcpPort );
cout << "TCP set up" << endl; cout << "TCP set up" << endl;
cout << "setting up RC receiver..." << endl; /*cout << "setting up RC receiver..." << endl;
dsm = DSM2(); dsm = DSM2();
dsm.values = tcp.controlValues; //dsm.values = tcp.controlValues;
cout << "RC receiver set up" << endl; cout << "RC receiver set up" << endl;*/
cout << "setting up MPU6050..." << endl; cout << "\nsetting up MPU6050..." << endl;
cout << "\tinitializing MPU6050..." << endl; cout << "initializing MPU6050..." << endl;
mpu.initialize(); mpu.initialize();
cout << "\ttesting MPU6050 connection..." << flush; cout << "testing MPU6050 connection..." << flush;
if( mpu.testConnection() ) if( mpu.testConnection() )
{ {
cout << "SUCCESS" << endl; cout << "SUCCESS" << endl;
@ -82,26 +82,26 @@ void setup( int tcpPort )
cout << "FAILURE" << endl; cout << "FAILURE" << endl;
exit( EXIT_FAILURE ); exit( EXIT_FAILURE );
} }
cout << "\tinitializing DMP..." << flush; cout << "initializing DMP..." << flush;
if( mpu.dmpInitialize() == 0 ) if( mpu.dmpInitialize() == 0 )
{ {
cout << "\tSUCCESS" << endl; cout << "SUCCESS" << endl;
cout << "\tEnabling DMP..." << endl; cout << "Enabling DMP..." << endl;
mpu.setDMPEnabled( true ); mpu.setDMPEnabled( true );
fifoPacketSize = mpu.dmpGetFIFOPacketSize(); fifoPacketSize = mpu.dmpGetFIFOPacketSize();
} }
else else
{ {
cout << "\tFAILURE" << endl; cout << "FAILURE" << endl;
exit( EXIT_FAILURE ); exit( EXIT_FAILURE );
} }
cout << "MPU6050 set up" << endl; cout << "MPU6050 set up" << endl;
cout << "setting up PID..." << endl; cout << "\nsetting up PID..." << endl;
pitchPID = PID( pitchP, pitchI, pitchD ); pitchPID = PID( pitchP, pitchI, pitchD );
rollPID = PID( rollP, rollI, rollD ); rollPID = PID( rollP, rollI, rollD );
yawPID = PID( yawP, yawI, yawD ); yawPID = PID( yawP, yawI, yawD );
cout << "PID set up" << endl; cout << "PID set up\n" << endl;
atexit( setAllMotorsOff ); atexit( setAllMotorsOff );
} }
@ -154,22 +154,18 @@ int main( int argc, char* argv[] )
float rollMod = 1; float rollMod = 1;
float yawMod = 1; float yawMod = 1;
if( argc < 1 )
{
setup( DEFAULT_TCP_PORT ); setup( DEFAULT_TCP_PORT );
}
else
{
setup( *argv[0] );
}
//dsm.sync(); cout << "starting loop" << endl;
while( true ) while( true )
{ {
//dsm.update(); dsm.update();
tcp.send(); 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.recieve(); //tcp.send();
if( updateMpu() ) //cout << "tcp send done" << endl;
//cout<< tcp.recieve() <<endl;
//cout << "tcp receive done" << endl;
/*if( updateMpu() )
{ {
pitchAngle = atan( gravity.x / sqrt( gravity.y * gravity.y + gravity.z * gravity.z) ); 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 ) ); rollAngle = atan( gravity.y / sqrt( gravity.x * gravity.x + gravity.z * gravity.z ) );
@ -201,7 +197,12 @@ int main( int argc, char* argv[] )
{ {
setAllMotorsOff(); setAllMotorsOff();
} }
cout << "MPU data read" << endl;
} }
else
{
cout << "no MPU data avail" << endl;
}*/
} }
} }