brendanhaines/RasPi
1
0
Fork 0
mirror of https://github.com/brendanhaines/RasPi.git synced 2025-04-15 21:38:11 -06: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
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 $@

50
CopterController/v2EchoServer.cpp
View File

@ -10,19 +10,26 @@
#include <sys/ioctl.h>
#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();
}
}

8
CopterController/v2Parser.cpp
View File

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

3
CopterController/v2Parser.h
View File

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

57
CopterController/v2main.cpp
View File

@ -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,33 +46,33 @@ 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;
cout << "\nsetting up MPU6050..." << endl;
cout << "initializing MPU6050..." << endl;
mpu.initialize();
cout << "\ttesting MPU6050 connection..." << flush;
cout << "testing MPU6050 connection..." << flush;
if( mpu.testConnection() )
{
cout << "SUCCESS" << endl;
@ -82,26 +82,26 @@ void setup( int tcpPort )
cout << "FAILURE" << endl;
exit( EXIT_FAILURE );
}
cout << "\tinitializing DMP..." << flush;
cout << "initializing DMP..." << flush;
if( mpu.dmpInitialize() == 0 )
{
cout << "\tSUCCESS" << endl;
cout << "\tEnabling DMP..." << endl;
cout << "SUCCESS" << endl;
cout << "Enabling DMP..." << endl;
mpu.setDMPEnabled( true );
fifoPacketSize = mpu.dmpGetFIFOPacketSize();
}
else
{
cout << "\tFAILURE" << endl;
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] );
}
//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() <<endl;
//cout << "tcp receive done" << endl;
/*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 ) );
@ -201,7 +197,12 @@ int main( int argc, char* argv[] )
{
setAllMotorsOff();
}
cout << "MPU data read" << endl;
}
else
{
cout << "no MPU data avail" << endl;
}*/
}
}