mirror of
https://github.com/brendanhaines/RasPi.git
synced 2025-04-04 07:44:52 -06:00
Mostly working version. No yaw control yet. Changes output format while setting up oneThatWorks
This commit is contained in:
parent
46595bbaac
commit
4a4d2f63aa
@ -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()
|
||||
|
||||
@ -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 )
|
||||
|
||||
@ -3,7 +3,6 @@
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <unistd.h>
|
||||
//#include <stdint.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
@ -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;
|
||||
|
||||
@ -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 );
|
||||
|
||||
|
||||
@ -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 );
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user