From 4a4d2f63aaf7618b0972c6952e93071aba8985dc Mon Sep 17 00:00:00 2001 From: Brendan Date: Wed, 18 Mar 2015 22:33:59 -0600 Subject: [PATCH] Mostly working version. No yaw control yet. Changes output format while setting up oneThatWorks --- CopterController/DSM2.cpp | 10 +-- CopterController/PCA9685.cpp | 16 ++-- CopterController/oneThatWorks.cpp | 102 +++++++++++++++++++----- GroundStation/v2/DisplayController.java | 15 ++-- GroundStation/v2/GroundStation.java | 2 +- 5 files changed, 103 insertions(+), 42 deletions(-) diff --git a/CopterController/DSM2.cpp b/CopterController/DSM2.cpp index 8313d8f..b9e674b 100644 --- a/CopterController/DSM2.cpp +++ b/CopterController/DSM2.cpp @@ -8,19 +8,19 @@ DSM2::DSM2() { - std::cout << "opening serial port..." << std::flush; + std::cout << "[ .... ] opening serial port..." << std::flush; fd = serialOpen( "/dev/ttyAMA0", 115200 ); if( fd < 0 ) { - std::cout << "FAILURE" << std::endl; + std::cout << "\r[ FAIL ]" << std::endl; } else { - std::cout << "SUCCESS " << fd << std::endl; + std::cout << "\r[ DONE ]" << fd << std::endl; } - std::cout << "flushing serial..." << std::flush; + std::cout << "[ .... ] flushing serial..." << std::flush; serialFlush(fd); - std::cout << "DONE" << std::endl; + std::cout << "\r[ DONE ]" << std::endl; } void DSM2::update() diff --git a/CopterController/PCA9685.cpp b/CopterController/PCA9685.cpp index 147013e..b26bf3b 100644 --- a/CopterController/PCA9685.cpp +++ b/CopterController/PCA9685.cpp @@ -14,15 +14,19 @@ PCA9685::PCA9685() PCA9685::PCA9685( int devAddr ) { + std::cout << "[ .... ] Getting I2C file handle..." << std::flush; i2cFileHandle = wiringPiI2CSetup( devAddr ); // parameter is dependent on board revision if( i2cFileHandle < 0 ) { - std::cerr << "failed to open i2c" << std::endl; + std::cout << "\r[ FAIL ]" << std::endl; } + std::cout << " Setting all pwm to 0..." << std::flush; setAllPwm( 0, 0 ); - std::cout << "all pwm set to 0" << std::endl; + std::cout << "\r[ DONE ] " << std::endl; + + std::cout << "[ .... ] Setting up PCA9685..." << std::flush; wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE2, PCA9685_OUTDRV ); wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, PCA9685_ALLCALL ); delay( 5 ); @@ -31,6 +35,7 @@ PCA9685::PCA9685( int devAddr ) mode1 = mode1 & ~PCA9685_SLEEP; // set sleep bit (to wake) wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, mode1 ); delay( 5 ); + std::cout << "\r[ DONE ]" << std::endl; } void PCA9685::setFrequency( int hertz ) @@ -43,10 +48,10 @@ void PCA9685::setFrequency( int hertz ) preScale /= hertz; preScale -= 1; // fencepost - std::cout << "setting pwm frequency to: " << hertz << " hertz" << std::endl; - std::cout << "estimate preScale: " << preScale << std::endl; + std::cout << "[ .... ] setting pwm frequency to: " << hertz << "..." << std::flush; + std::cout << " estimate preScale: " << preScale << std::flush; preScale += 0.5; - std::cout << "final preScale: " << (int) preScale << std::endl; + std::cout << " final preScale: " << (int) preScale << std::flush; oldMode = wiringPiI2CReadReg8( i2cFileHandle, PCA9685_MODE1 ); newMode = ( oldMode & 0x7F ) | PCA9685_SLEEP; // set sleep bit (to sleep) @@ -56,6 +61,7 @@ void PCA9685::setFrequency( int hertz ) delay( 5 ); wiringPiI2CWriteReg8( i2cFileHandle, PCA9685_MODE1, oldMode | PCA9685_RESTART ); // set restart bit delay( 5 ); // wait for restart + std::cout << "\r[ DONE ]" << std::endl; } void PCA9685::setPwm( int channel, int on, int off ) diff --git a/CopterController/oneThatWorks.cpp b/CopterController/oneThatWorks.cpp index 4c0f15b..bd5f11b 100644 --- a/CopterController/oneThatWorks.cpp +++ b/CopterController/oneThatWorks.cpp @@ -3,7 +3,6 @@ #include #include #include -//#include #include #include #include @@ -14,7 +13,6 @@ #include "PCA9685.h" #include "DSM2.h" #include "PID.h" -//#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #include "wiringPi.h" @@ -53,11 +51,14 @@ PCA9685 pca; #define MOTOR_MAX 600 int motorValues[4]; +int controlValues[6]; // scaled 0 to 1000. 500 is centered bool motorsEnabled; bool motorTesting; int throttle = 300; // PID stuff +#define PITCH_GAIN 0.785398163397448 +#define ROLL_GAIN 0.785398163397448 PID pitch, roll, yaw; double pitchP = 2; double pitchI = 0; @@ -75,12 +76,13 @@ bool setupTCP( int portNum ) struct sockaddr_in serv_addr, cli_addr; socklen_t clilen; - cout << "setting up TCP..." << endl; + cout << "[ .... ] setting up TCP..." << flush; sockfd = socket( AF_INET, SOCK_STREAM, 0 ); if( sockfd < 0 ) { - cerr << "ERROR opening socket" << endl; + cout << "\r[ FAIL ]" << endl; + cerr << "** ERROR opening socket " << portNum << endl; return false; } @@ -89,21 +91,24 @@ bool setupTCP( int portNum ) serv_addr.sin_addr.s_addr = INADDR_ANY; serv_addr.sin_port = htons(portNum); - cout << "binding socket to port " << portNum << endl; + //cout << "binding socket to port " << portNum << endl; + cout << " port: " << portNum << flush; if( bind( sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr) ) ) { + cout << "\r[ FAIL ]" << endl; cerr << "ERROR binding socket to port " << portNum << endl; return false; } - cout << "listening to socket" << endl; + //cout << "listening to socket" << endl; listen( sockfd, 5 ); clilen = sizeof( cli_addr ); - cout << "waiting for connection" << endl; + cout << " WAITING FOR CONNECTION..." << flush; newsockfd = accept( sockfd, (struct sockaddr *) &cli_addr, &clilen ); if( newsockfd < 0 ) { + cout << "\r[ FAIL ]" << endl; cerr << "ERROR on accept" << endl; return false; } @@ -111,7 +116,7 @@ bool setupTCP( int portNum ) bzero( buffer, 256 ); fcntl( newsockfd, F_SETFL, O_NONBLOCK ); - cout << "connection established" << endl; + cout << "\r[ DONE ]" << endl; return true; } @@ -130,7 +135,7 @@ void setupPCA() void setupMPU() { mpu = MPU6050(); - cout << "\nsetting up MPU6050..." << endl; + cout << "setting up MPU6050..." << endl; cout << "initializing MPU6050..." << endl; mpu.initialize(); cout << "testing MPU6050 connection..." << flush; @@ -216,6 +221,40 @@ void readTCP() return; } +// returns the number of characters written +int writeTCP() +{ + int i, n = 0; + char writeBuffer[256]; + //char temp[32]; + strcat( writeBuffer, "H " ); + + if( motorsEnabled ) + { + for( i = 0; i < 4; i++ ) + { + //sprintf( temp, "E M%02d_%04d ", i, motorValues[i] ); + //strcat( writeBuffer, temp ); + //bzero( temp, 32 ); + fprintf( fdopen(newsockfd, "a"), "E M%02d_%04d ", i, motorValues[i] ); + printf( "E M%02d_%04d ", i, motorValues[i] ); + } + } + + for( i = 0; i < 6; i++ ) + { + //sprintf( temp, "C%02d_%04d ", i, controlValues[i] ); + //strcat( writeBuffer, temp ); + //bzero( temp, 32 ); + fprintf( fdopen(newsockfd, "a"), "C%02d_%04d ", i, controlValues[i] ); + printf( "C%02d_%04d ", i, controlValues[i] ); + } + + //n = write( newsockfd, writeBuffer, strlen(writeBuffer) ); + //printf( writeBuffer ); + return n; +} + bool updateMPU() { fifoCount = mpu.getFIFOCount(); @@ -244,18 +283,35 @@ bool updateMPU() return false; } +void scaleRxValues() +{ + int i; + for( i = 0; i< 6; i++ ) + controlValues[i] = ( dsm.values[i] - RX_MIN[i] ) * 1000 / ( RX_MAX[i] - RX_MIN[i] ); +} + +int toMotorRange( int control ) +{ + return control * (MOTOR_MAX - MOTOR_MIN) / 1000 + MOTOR_MIN; +} + void updatePID() { float pitchMod, rollMod, yawMod; - pitchMod = pitch.update( pitchAngle, 0 ); - rollMod = roll.update( rollAngle, 0 ); + pitchMod = pitch.update( pitchAngle, (float)( controlValues[2] - 500 ) * -PITCH_GAIN / 1000 ); + rollMod = roll.update( rollAngle, (float)( controlValues[1] - 500 ) * ROLL_GAIN / 1000 ); yawMod = yaw.update( yawRate, 0 ); - motorValues[0] = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * ( 1 + yawMod ) * throttle ); + /*motorValues[0] = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * ( 1 + yawMod ) * throttle ); motorValues[1] = (int)( ( 1 + pitchMod ) * ( 1 - rollMod ) * ( 1 - yawMod ) * throttle ); motorValues[2] = (int)( ( 1 - pitchMod ) * ( 1 + rollMod ) * ( 1 - yawMod ) * throttle ); - motorValues[3] = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * ( 1 + yawMod ) * throttle ); + motorValues[3] = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * ( 1 + yawMod ) * throttle );*/ + + motorValues[0] = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * ( 1 + yawMod ) * toMotorRange( controlValues[0] ) ); + motorValues[1] = (int)( ( 1 + pitchMod ) * ( 1 - rollMod ) * ( 1 - yawMod ) * toMotorRange( controlValues[0] ) ); + motorValues[2] = (int)( ( 1 - pitchMod ) * ( 1 + rollMod ) * ( 1 - yawMod ) * toMotorRange( controlValues[0] ) ); + motorValues[3] = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * ( 1 + yawMod ) * toMotorRange( controlValues[0] ) ); } // main @@ -264,27 +320,29 @@ int main(int argc, char const *argv[]) int i; bool wasPID = false; + // RC receiver already set up setupPCA(); setupTCP( TCP_PORT ); setupMPU(); // label lines cout << "\n**REAL TIME DATA**\n" << endl; - cout << "\t"; // FIFO cout << "RC RECEIVER\t\t\t\t\t"; cout << "MOTORS\t\t\t\t\t\t"; - cout << "ORIENTATION\t"; + //cout << "ORIENTATION\t"; cout << endl; - cout << "FIFO\t"; cout << "THRO\tAILE\tELEV\tRUDD\tGEAR\tAUX1\t"; cout << "ENABLE\tTEST\tM0 \tM1 \tM2 \tM3\t"; - cout << "PITCH\t\tROLL\t\tYAW\t"; + //cout << "PITCH\t\tROLL\t\tYAW\t"; cout << endl; while( true ) { dsm.update(); + scaleRxValues(); + + //writeTCP(); readTCP(); fifoCount = mpu.getFIFOCount(); @@ -309,7 +367,7 @@ int main(int argc, char const *argv[]) } // constrain motor values if enabled: MOTOR_MIN < value < MOTOR_MAX - if( motorsEnabled && !motorTesting ) + if( motorsEnabled && !motorTesting && controlValues[5] > 500 ) { for( i = 0; i < 4; i++ ) { @@ -329,12 +387,12 @@ int main(int argc, char const *argv[]) pca.setPwm( i, 0, motorValues[i] ); cout << "\r"; - cout << fifoCount << "\t"; - cout << dsm.values[0] << " \t" << dsm.values[1] << " \t" << dsm.values[2] << " \t" << dsm.values[3] << " \t" << dsm.values[4] << " \t" << dsm.values[5] << "\t"; + //cout << dsm.values[0] << " \t" << dsm.values[1] << " \t" << dsm.values[2] << " \t" << dsm.values[3] << " \t" << dsm.values[4] << " \t" << dsm.values[5] << "\t"; + printf( "%04d\t%04d\t%04d\t%04d\t%04d\t%04d\t", controlValues[0], controlValues[1], controlValues[2], controlValues[3], controlValues[4], controlValues[5] ); cout << motorsEnabled << " \t" << motorTesting << " \t" << motorValues[0] << " \t" << motorValues[1] << " \t" << motorValues[2] << " \t" << motorValues[3] << " \t"; - printf( "%+1.5f\t%+1.5f\t%+06d\t", pitchAngle, rollAngle, yawRate ); + //printf( "%+1.5f\t%+1.5f\t%+06d\t", pitchAngle, rollAngle, yawRate ); cout << flush; - delay(100); + delay(120); } return 0; diff --git a/GroundStation/v2/DisplayController.java b/GroundStation/v2/DisplayController.java index a1c2416..d28abc0 100644 --- a/GroundStation/v2/DisplayController.java +++ b/GroundStation/v2/DisplayController.java @@ -7,9 +7,6 @@ class DisplayController extends JPanel implements ActionListener { private MessageContent source = new MessageContent(); private Font normalFont = new Font( "Normal", Font.BOLD, 15 ); - private int chanMax = 900; - private int chanMin = 100; - public DisplayController() { setBackground( Color.DARK_GRAY ); setPreferredSize( new Dimension( 0, 100 ) ); @@ -23,9 +20,9 @@ class DisplayController extends JPanel implements ActionListener { /** * */ - private int scaleChan( int chan, int range ) { + /*private int scaleChan( int chan, int range ) { return (int)(range * ( ( (double)(chan - chanMin) / (chanMax - chanMin) ) - 0.5 )); - } + }*/ public void paintComponent( Graphics g ) { super.paintComponent( g ); @@ -47,14 +44,14 @@ class DisplayController extends JPanel implements ActionListener { g.setColor( Color.RED ); // left stick g.fillOval( - scaleChan( source.controlValues[ 3 ], boxSide ) - circleRad + boxSide * 3/4, - scaleChan( source.controlValues[ 0 ], boxSide ) - circleRad + boxSide * 3/4, + source.controlValues[ 3 ] * boxSide / 1000 - circleRad + boxSide * 1/4, + source.controlValues[ 0 ] * boxSide / 1000 - circleRad + boxSide * 11/4, circleDiam, circleDiam ); // right stick g.fillOval( - scaleChan( source.controlValues[ 1 ], boxSide ) - circleRad + boxSide * 9/4, - scaleChan( source.controlValues[ 2 ], boxSide ) - circleRad + boxSide * 3/4, + source.controlValues[ 1 ] * boxSide / 1000 - circleRad + boxSide * 7/4, + source.controlValues[ 2 ] * boxSide / 1000 - circleRad + boxSide * 11/4, circleDiam, circleDiam ); diff --git a/GroundStation/v2/GroundStation.java b/GroundStation/v2/GroundStation.java index a8e1497..a836d74 100644 --- a/GroundStation/v2/GroundStation.java +++ b/GroundStation/v2/GroundStation.java @@ -49,7 +49,7 @@ class GroundStation implements ActionListener { mainWindow.getContentPane().add( tabbedPane ); controllerPanel = new DisplayController( receiveContent ); - tabbedPane.add( controllerPanel, "Controller" ); + //tabbedPane.add( controllerPanel, "Controller" ); receiveContent.addActionListener( controllerPanel ); motorTestPanel = new MotorTestPanel( sendContent );