brendanhaines/RasPi
1
0
Fork 0
mirror of https://github.com/brendanhaines/RasPi.git synced 2024-09-19 11:45:50 -06:00
Code Issues Packages Projects Releases Wiki Activity

adds echoServerAdvanced.cpp

Browse Source
This commit is contained in:
Brendan Haines 2015-02-06 15:16:40 -07:00
parent eda95d8dd5
commit a666d1cb93
2 changed files with 181 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
CopterController/PCA9685/PCA9685tester.cpp
View File

@ -1,30 +1,74 @@
#include <iostream>
#include "stdlib.h"
#include "wiringPi.h"
#include "PCA9685.h"
#define MOTOR_MIN 200
#define MOTOR_MAX 600
#define SERVO_FREQ 60
int main( int argc, const char* argv[] )
{
std::cout << "will only test motor 0" << std::endl;
int currentValue = MOTOR_MIN;
wiringPiSetup();
PCA9685 servo = PCA9685( 0x40 );
servo.setFrequency( 60 );
std::cout << "freq set" << std::endl;
servo.setFrequency( SERVO_FREQ );
std::cout << "freq set: " << SERVO_FREQ << std::endl;
servo.setPwm( 0, 0, 600 );
std::cout << "pwm set 600" << std::endl;
while( std::cin.get() != '\n' );
if( argc > 1 )
{
if( atoi(argv[1]) == 1 )
{
std::cout << "will set max pwm" << std::endl;
servo.setPwm( 0, 0, MOTOR_MAX );
std::cout << "pwm set " << MOTOR_MAX << std::endl;
std::cout << "press ENTER to continue" << std::endl;
while( std::cin.get() != '\n' );
}
}
servo.setPwm( 0, 0, 150 );
std::cout << "pwm set 150" << std::endl;
servo.setPwm( 0, 0, MOTOR_MIN );
std::cout << "pwm set " << MOTOR_MIN << std::endl;
std::cout << "press ENTER to continue" << std::endl;
while( std::cin.get() != '\n' );
while( true )
{
servo.setPwm( 0, 0, 600 );
std::cout << "pwm: 600" << std::endl;
int keyPressed = std::cin.get();
if( keyPressed == ' ' )
{
std::cout << "KILL MOTOR" << std::endl;
currentValue = MOTOR_MIN;
servo.setPwm( 0, 0, currentValue );
std::cout << "pwm: " << currentValue << std::endl;
}
else if( keyPressed == 'w' ) // up arrow
{
std::cout << "up pressed" << std::endl;
currentValue += 5;
if( currentValue > MOTOR_MAX ) currentValue = MOTOR_MAX;
servo.setPwm( 0, 0, currentValue );
std::cout << "pwm: " << currentValue << std::endl;
}
else if( keyPressed == 's' ) // down arrow
{
std::cout << "down pressed" << std::endl;
currentValue -= 5;
if( currentValue < MOTOR_MIN ) currentValue = MOTOR_MIN;
servo.setPwm( 0, 0, currentValue );
std::cout << "pwm: " << currentValue << std::endl;
}
/*
servo.setPwm( 0, 0, MOTOR_MAX );
std::cout << "pwm: " << MOTOR_MAX << std::endl;
delay( 1000 );
servo.setPwm( 0, 0, 150 );
std::cout << "pwm: 150" << std::endl;
servo.setPwm( 0, 0, MOTOR_MIN );
std::cout << "pwm: " << MOTOR_MIN << std::endl;
delay( 1000 );
*/
}
}

126
CopterController/PCA9685/echoServerAdvanced.cpp Normal file
View File

@ -0,0 +1,126 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#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 "wiringPi.h"
#include "PCA9685.h"
#define MOTOR_MIN 200
#define MOTOR_MAX 600
#define SERVO_FREQ 60
int sockfd, newsockfd;
socklen_t clilen;
char buffer[256];
struct sockaddr_in serv_addr, cli_addr;
int nread, nwrite;
void setupTCP( int portno )
{
printf( "Setting up socket\n" );
sockfd = socket( AF_INET, SOCK_STREAM, 0 );
if( sockfd < 0 )
printf( "ERROR opening socket" );
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(portno);
printf( "Binding socket to portno\n" );
if( bind( sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr) ) < 0 )
printf("ERROR on binding");
printf( "Listening\n" );
listen( sockfd, 5 );
clilen = sizeof(cli_addr);
printf( "Waiting for connection\n" );
newsockfd = accept( sockfd,
(struct sockaddr *) &cli_addr,
&clilen );
if( newsockfd < 0 )
printf( "ERROR on accept" );
printf( "Connected\n" );
bzero( buffer, 256 );
fcntl( newsockfd, F_SETFL, O_NONBLOCK );
}
int main( int argc, char* argv[] )
{
setupTCP( 51717 );
wiringPiSetup();
PCA9685 servo = PCA9685( 0x40 );
servo.setFrequency( SERVO_FREQ );
servo.setPwm( 0, 0, MOTOR_MIN );
servo.setPwm( 1, 0, MOTOR_MIN );
servo.setPwm( 2, 0, MOTOR_MIN );
servo.setPwm( 3, 0, MOTOR_MIN );
while( true )
{
nread = read( newsockfd, buffer, 255 );
if( nread < 0 )
{
//printf( "ERROR on read" );
}
else if( nread != 0 )
{
nwrite = write( newsockfd, buffer, strlen(buffer) );
if( nwrite < 0 )
printf( "ERROR on write" );
//PARSE
printf( "Just read: %s", buffer );
if( strncmp( buffer, "DISCONNECT", strlen("DISCONNECT") ) == 0 )
{
printf( "Closing Socket\n" );
close( newsockfd );
close( sockfd );
return 0;
}
else if( strncmp( buffer, "HEARTBEAT", strlen("HEARTBEAT") ) == 0 ) // hearbeat transmission
{
// format:
// HEARTBEAT FCENABLE_x
// FCENABLE_x flight controller enable/disable x=0: disabled x=1: enabled
}
else if( strncmp( buffer, "SET_MOTOR_", strlen("SET_MOTOR_") ) == 0 )
{
int motor = atoi( buffer + 10 );
int speed = atoi( buffer + 12 );
if( speed < MOTOR_MIN )
{
speed = MOTOR_MIN;
}
else if( speed > MOTOR_MAX )
{
speed = MOTOR_MAX;
}
if( motor >= 0 && motor <= 3 )
{
printf( "setting motor %d to %d\n", motor, speed );
servo.setPwm( motor, 0, speed );
delay(5);
printf( "motor %d set to %d\n", motor, speed );
}
}
}
delay(5);
}
}