mirror of
https://github.com/brendanhaines/RasPi.git
synced 2025-04-11 19:24:51 -06:00
removes v2EchoServer.cpp, v2Parser.cpp, v2Parser.h, v2main.cpp
This commit is contained in:
parent
c3292e033c
commit
83c4c3ee8b
@ -1,180 +0,0 @@
|
||||
#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 "DSM2.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
// TCP vars
|
||||
#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 ];
|
||||
//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 )
|
||||
{
|
||||
cout << "H " << flush;
|
||||
carotPos++;
|
||||
}
|
||||
else if( strncmp( buffer + carotPos, "E", strlen("E") ) == 0 && strlen("E") + carotPos <= n )
|
||||
{
|
||||
motorsEnabled = true;
|
||||
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");
|
||||
}
|
||||
/*else if( strncmp( buffer + carotPos, "C", strlen("C") ) == 0 && strlen("Cxx_xxxx") + carotPos <= n )
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void writeTcp()
|
||||
{
|
||||
int i;
|
||||
char temp[256];
|
||||
char outBuffer[] = "H ";
|
||||
|
||||
if( motorsEnabled ) {
|
||||
strcat( outBuffer, "E " );
|
||||
|
||||
for( i = 0; i < 4; 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[] )
|
||||
{
|
||||
//if( !setupTcp( TCP_PORT ) ) exit( EXIT_FAILURE );
|
||||
setupTcp( TCP_PORT );
|
||||
while( true )
|
||||
{
|
||||
readTcp();
|
||||
cout << "EN: " << motorsEnabled << "\tM0: " << motorValues[0] << "\tM1: " << motorValues[1] << "\tM2: " << motorValues[2] << "\tM3: " << motorValues[3] << endl;
|
||||
writeTcp();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,136 +0,0 @@
|
||||
/*
|
||||
* 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;
|
||||
|
||||
// No-Args constructor
|
||||
v2Parser::v2Parser()
|
||||
{}
|
||||
|
||||
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 );
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1,32 +0,0 @@
|
||||
/*
|
||||
* v2Parser.h
|
||||
*/
|
||||
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
#include "helper_3dmath.h"
|
||||
|
||||
#ifndef V2PARSER_H
|
||||
#define V2PARSER_H
|
||||
|
||||
class v2Parser
|
||||
{
|
||||
public:
|
||||
v2Parser();
|
||||
v2Parser( int port);
|
||||
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
|
||||
@ -1,215 +0,0 @@
|
||||
/*
|
||||
* 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;
|
||||
|
||||
PCA9685 pca;
|
||||
v2Parser tcp;
|
||||
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, MOTOR_OFF );
|
||||
}
|
||||
|
||||
void setup( int tcpPort )
|
||||
{
|
||||
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\n" << 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 << "\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 << "\nsetting up PID..." << endl;
|
||||
pitchPID = PID( pitchP, pitchI, pitchD );
|
||||
rollPID = PID( rollP, rollI, rollD );
|
||||
yawPID = PID( yawP, yawI, yawD );
|
||||
cout << "PID set up\n" << 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;
|
||||
|
||||
setup( DEFAULT_TCP_PORT );
|
||||
|
||||
cout << "starting loop" << endl;
|
||||
while( true )
|
||||
{
|
||||
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 ) );
|
||||
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();
|
||||
}
|
||||
cout << "MPU data read" << endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
cout << "no MPU data avail" << endl;
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user