File content as of revision 1:5721a5772035:

#include "mbed.h"
#include "EthernetNetIf.h"
#include "TCPSocket.h"
#include "IMUfilter.h"
#include "ADXL345_I2C.h"
#include "L3G4200D.h"

#define TCP_LISTENING_PORT 1337
//Gravity at Earth's surface in m/s/s
#define g0 9.812865328
//Number of samples to average.
#define SAMPLES 4
//Number of samples to be averaged for a null bias calculation
//during calibration.
#define CALIBRATION_SAMPLES 128
//Convert from radians to degrees.
#define toDegrees(x) (x * 57.2957795)
//Convert from degrees to radians.
#define toRadians(x) (x * 0.01745329252)
//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
#define GYROSCOPE_GAIN (1 / 14.375)
//Full scale resolution on the ADXL345 is 4mg/LSB.
#define ACCELEROMETER_GAIN (0.004 * g0)
//Sampling gyroscope at 200Hz.
#define GYRO_RATE   0.005
//Sampling accelerometer at 200Hz.
#define ACC_RATE    0.005
//Updating filter at 20Hz.
#define FILTER_RATE 0.025
//Defines the delay for the corrections made by the algorithm
#define CORRECTION_DELAY 5

//At rest the gyroscope is centred around 0 and goes between about
//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
//5/15 = 0.3 degrees/sec.
IMUfilter imuFilter(FILTER_RATE, 0.3);
ADXL345_I2C accelerometer(p9, p10);
L3G4200D gyroscope(p9, p10);

PwmOut m1(p21);
PwmOut m2(p22);
PwmOut m3(p23);
PwmOut m4(p24);

Ticker accelerometerTicker;
Ticker gyroscopeTicker;
Ticker filterTicker;
Ticker motorTicker;

//Variables for the tcp controls
int connected   = 0,
    calibrated  = 0,
    led1        = 0;
    
//stabilizing variables, these control the thrust and wanted x/y level
float m1_set    = 0.001,
      m2_set    = 0.001,
      m3_set    = 0.001,
      m4_set    = 0.001;
      
double    a_motor_1   = 0,
          a_motor_2   = 0,
          a_motor_3   = 0,
          a_motor_4   = 0;

int x_level_1   = 0,
    x_level_2   = 0,
    x_level_3   = 0,
    x_level_4   = 0,
    y_level_1   = 0,
    y_level_2   = 0,
    y_level_3   = 0,
    y_level_4   = 0;
    
//O1ffsets for the gyroscope.
//The readings we take when the gyroscope is stationary won't be 0, so we'll
//average a set of readings we do get when the gyroscope is stationary and
//take those away from subsequent readings to ensure the gyroscope is offset
//or "biased" to 0.
double w_xBias;
double w_yBias;
double w_zBias;

//Offsets for the accelerometer.
//Same as with the gyroscope.
double a_xBias;
double a_yBias;
double a_zBias;

//Accumulators used for oversampling and then averaging.
volatile double a_xAccumulator = 0;
volatile double a_yAccumulator = 0;
volatile double a_zAccumulator = 0;
volatile double w_xAccumulator = 0;
volatile double w_yAccumulator = 0;
volatile double w_zAccumulator = 0;

//Accelerometer and gyroscope readings for x, y, z axes.
volatile double a_x;
volatile double a_y;
volatile double a_z;
volatile double w_x;
volatile double w_y;
volatile double w_z;

//Buffer for accelerometer readings.
int readings[3];
//Number of accelerometer samples we're on.
int accelerometerSamples = 0;
//Number of gyroscope samples we're on.
int gyroscopeSamples = 0;

/**
 * Prototypes
 */
//Set up the ADXL345 appropriately.
void initializeAccelerometer(void);
//Calculate the null bias.
void calibrateAccelerometer(void);
//Take a set of samples and average them.
void sampleAccelerometer(void);
//Calculate the null bias.
void calibrateGyroscope(void);
//Take a set of samples and average them.
void sampleGyroscope(void);
//Update the filter and calculate the Euler angles.
void filter(void);
void motor(void);
void tcp_send(const char*);
void onConnectedTCPSocketEvent(TCPSocketEvent e);
void onListeningTCPSocketEvent(TCPSocketEvent e);

// EthernetNetIf eth;
EthernetNetIf eth(
  IpAddr(192,168,0,16), //IP Address
  IpAddr(255,255,255,0), //Network Mask
  IpAddr(192,168,0,1), //Gateway
  IpAddr(192,168,0,1)  //DNS
);

TCPSocket ListeningSock;
TCPSocket* pConnectedSock; // for ConnectedSock
Host client;
TCPSocketErr err;

void sampleAccelerometer(void) {

    //Have we taken enough samples?
    if (accelerometerSamples == SAMPLES) {

        //Average the samples, remove the bias, and calculate the acceleration
        //in m/s/s.
        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;

        a_xAccumulator = 0;
        a_yAccumulator = 0;
        a_zAccumulator = 0;
        accelerometerSamples = 0;

    } else {
        //Take another sample.
        accelerometer.getOutput(readings);

        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];

        accelerometerSamples++;

    }

}

void sampleGyroscope(void) {

    //Have we taken enough samples?
    if (gyroscopeSamples == SAMPLES) {

        //Average the samples, remove the bias, and calculate the angular
        //velocity in rad/s.
        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);

        w_xAccumulator = 0;
        w_yAccumulator = 0;
        w_zAccumulator = 0;
        gyroscopeSamples = 0;

    } else {
        //Take another sample.
        int g[3];
        gyroscope.read(g);
        w_xAccumulator += g[0];
        w_yAccumulator += g[1];
        w_zAccumulator += g[2];

        gyroscopeSamples++;

    }

}

void filter(void) {

    //Update the filter variables.
    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
    //Calculate the new Euler angles.
    imuFilter.computeEuler();

}

void motor(void) {
            int x = toDegrees(imuFilter.getRoll());
            int y = toDegrees(imuFilter.getPitch());
            a_motor_1 = a_motor_1+((float)(x/2)/CORRECTION_DELAY)+((float)(y/2)/CORRECTION_DELAY);
            if(a_motor_1>99){       m1_set = 0.002;}
            else if(a_motor_1 < 1){ m1_set = 0.001175;}
            else{                   m1_set = (a_motor_1+117.5)/100000;}
    m1.pulsewidth(m1_set);
    m2.pulsewidth(m2_set);
    m3.pulsewidth(m3_set);
    m4.pulsewidth(m4_set); 
}

int main() {
   
    EthernetErr ethErr = eth.setup();
    if(ethErr)  { return -1;}
    IpAddr ip = eth.getIp();    
    // Set the callbacks for Listening
    ListeningSock.setOnEvent(&onListeningTCPSocketEvent);     
    // bind and listen on TCP
    err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT));
    //Deal with that error...
    if(err){printf("Binding Error\n");} 
    err=ListeningSock.listen(); // Starts listening
    if(err){printf("Listening Error\r\n");}
    
    m1.pulsewidth(0.001);
    m2.pulsewidth(0.001);
    m3.pulsewidth(0.001);
    m4.pulsewidth(0.001); 
    
    //Initialize inertial sensors.
    initializeAccelerometer();
    calibrateAccelerometer();
    calibrateGyroscope();

    //Set up timers.
    //Accelerometer data rate is 200Hz, so we'll sample at this speed.
//    accelerometerTicker.attach(&sampleAccelerometer, 0.005);
    //Gyroscope data rate is 200Hz, so we'll sample at this speed.
//    gyroscopeTicker.attach(&sampleGyroscope, 0.005);
    //Update the filter variables at the correct rate.
//    filterTicker.attach(&filter, FILTER_RATE);
        
    Timer tmr;
    tmr.start();
      
    while(true)
    {
        wait(0.2);
        
        Net::poll();
        if(tmr.read() > 0.2){
//            led4=!led4; //Show that we are alive
            tmr.reset();
        } 
        if(connected & led1 == 0){
            tcp_send("Connected\r\n");
            led1    = 1;
            calibrated = 1;
            
//            m1_set = 0.001175;
//            m2_set = 0.001175;
//            m3_set = 0.001175;
//            m4_set = 0.001175; 
            
            //Set up timers.
            //Accelerometer data rate is 200Hz, so we'll sample at this speed.
            accelerometerTicker.attach(&sampleAccelerometer, 0.005);
            //Gyroscope data rate is 200Hz, so we'll sample at this speed.
            gyroscopeTicker.attach(&sampleGyroscope, 0.005);
            //Update the filter variables at the correct rate.
            filterTicker.attach(&filter, FILTER_RATE);
            
            motorTicker.attach(&motor, 0.02);

            tcp_send("Done initializing\r\n");

        }

        if(calibrated){
            

            

            
/*            a_motor_2 = a_motor_2-((((x_level_2-x)/2)-((y_level_2-y)/2))/CORRECTION_DELAY);
            if(a_motor_2>99){       m2_set = (100+117.5)/100000;}
            else if(a_motor_2 < 1){ m2_set = (0+117.5)/100000;}
            else{                   m2_set = (a_motor_2+117.5)/100000;}
            
            a_motor_3 = a_motor_3+((((x_level_3-x)/2)-((y_level_3-y)/2))/CORRECTION_DELAY);
            if(a_motor_3>99){       m3_set = (100+117.5)/100000;}
            else if(a_motor_3 < 1){ m3_set = (0+117.5)/100000;}
            else{                   m3_set = (a_motor_3+117.5)/100000;}
            
            a_motor_4 = a_motor_4+((((x_level_4-x)/2)+((y_level_4-y)/2))/CORRECTION_DELAY);
            if(a_motor_4>99){       m4_set = (100+117.5)/100000;}
            else if(a_motor_4 < 1){ m4_set = (0+117.5)/100000;}
            else{                   m4_set = (a_motor_4+117.5)/100000;}*/
        
        
            char buffer [128];
            sprintf (buffer, "x:%f - y:%f - z:%f \r\n",toDegrees(imuFilter.getRoll()),
                                                        toDegrees(imuFilter.getPitch()),
                                                        toDegrees(imuFilter.getYaw()));
            tcp_send(buffer);   
            sprintf (buffer, "am1:%f\r\n",a_motor_1);
            tcp_send(buffer);   
        }
    }  
}

void initializeAccelerometer(void) {

    //Go into standby mode to configure the device.
    accelerometer.setPowerControl(0x00);
    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);
    //200Hz data rate.
    accelerometer.setDataRate(ADXL345_200HZ);
    //Measurement mode.
    accelerometer.setPowerControl(0x08);
    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
    wait_ms(22);

}

void calibrateAccelerometer(void) {

    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    
    //Take a number of readings and average them
    //to calculate the zero g offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {

        accelerometer.getOutput(readings);

        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];

        wait(ACC_RATE);

    }

    a_xAccumulator /= CALIBRATION_SAMPLES;
    a_yAccumulator /= CALIBRATION_SAMPLES;
    a_zAccumulator /= CALIBRATION_SAMPLES;

    //At 4mg/LSB, 250 LSBs is 1g.
    a_xBias = a_xAccumulator;
    a_yBias = a_yAccumulator;
    a_zBias = (a_zAccumulator - 250);

    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;

}

void calibrateGyroscope(void) {

    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;

    //Take a number of readings and average them
    //to calculate the gyroscope bias offset.
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
        int g[3];
        gyroscope.read(g);
        w_xAccumulator += g[0];
        w_yAccumulator += g[1];
        w_zAccumulator += g[2];
        wait(GYRO_RATE);

    }

    //Average the samples.
    w_xAccumulator /= CALIBRATION_SAMPLES;
    w_yAccumulator /= CALIBRATION_SAMPLES;
    w_zAccumulator /= CALIBRATION_SAMPLES;

    w_xBias = w_xAccumulator;
    w_yBias = w_yAccumulator;
    w_zBias = w_zAccumulator;

    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;

}



void tcp_send( const char* data ){
    int len = strlen(data);
    pConnectedSock->send(data, len);
}

void onConnectedTCPSocketEvent(TCPSocketEvent e)
{
   switch(e)
    {
    case TCPSOCKET_CONNECTED:
        printf("TCP Socket Connected\r\n");
        break;
    case TCPSOCKET_WRITEABLE:
      //Can now write some data...
        printf("TCP Socket Writable\r\n");
        break;
    case TCPSOCKET_READABLE:
      //Can now read dome data...
        printf("TCP Socket Readable\r\n");
       // Read in any available data into the buffer
       char buff[128];
       while ( int len = pConnectedSock->recv(buff, 128) ) {
       // And send straight back out again
//           pConnectedSock->send(buff, len);
           int test = buff[0];
           char buffer[128];
           sprintf(buffer, "|%i|", test);
           tcp_send(buffer); 
           if(test == 115)  {connected = 1;}
           if(test == 112)  {
                a_motor_1 += 5;
                a_motor_1 += 5;
                a_motor_1 += 5;
                a_motor_1 += 5;
           }
           if(test == 49)   {
                m1_set += 0.00002;
                sprintf (buffer, " %f\r\n",m1_set);
                tcp_send(buffer);
           }
           if(test == 50)   {
                m2_set += 0.00002;
                sprintf (buffer, " %f\r\n",m2_set);
                tcp_send(buffer);
           }
           if(test == 51)   {
                m3_set += 0.00002;
                sprintf (buffer, " %f\r\n",m3_set);
                tcp_send(buffer);
           }
           if(test == 52)   {
                m4_set += 0.00002;
                sprintf (buffer, " %f\r\n",m4_set);
                tcp_send(buffer);;
           }
           if(test == 113)  {
                m1_set -= 0.00002;
           }
           if(test == 119)  {
                m2_set -= 0.00002;
           }
           if(test == 101)  {
                m3_set -= 0.00002;
           }
           if(test == 114)  {
                m4_set -= 0.00002;
           }
           if(test == 107)  {
                m1_set = 0.0011;
                m2_set = 0.0011;
                m3_set = 0.0011;
                m4_set = 0.0011;
           }
                 

           buff[len]=0; // make terminater
           printf("Received&Wrote:%s\r\n",buff);
       }
       break;
    case TCPSOCKET_CONTIMEOUT:
        printf("TCP Socket Timeout\r\n");
        break;
    case TCPSOCKET_CONRST:
        printf("TCP Socket CONRST\r\n");
        break;
    case TCPSOCKET_CONABRT:
        printf("TCP Socket CONABRT\r\n");
        break;
    case TCPSOCKET_ERROR:
        printf("TCP Socket Error\r\n");
        break;
    case TCPSOCKET_DISCONNECTED:
    //Close socket...
        printf("TCP Socket Disconnected\r\n");        
        pConnectedSock->close();
        break;
    default:
        printf("DEFAULT\r\n"); 
      }
}


void onListeningTCPSocketEvent(TCPSocketEvent e)
{
    switch(e)
    {
    case TCPSOCKET_ACCEPT:
        printf("Listening: TCP Socket Accepted\r\n");
        // Accepts connection from client and gets connected socket.   
        err=ListeningSock.accept(&client, &pConnectedSock);
        if (err) {
            printf("onListeningTcpSocketEvent : Could not accept connection.\r\n");
            return; //Error in accept, discard connection
        }
        // Setup the new socket events
        pConnectedSock->setOnEvent(&onConnectedTCPSocketEvent);
        // We can find out from where the connection is coming by looking at the
        // Host parameter of the accept() method
        IpAddr clientIp = client.getIp();
        printf("Listening: Incoming TCP connection from %d.%d.%d.%d\r\n", 
           clientIp[0], clientIp[1], clientIp[2], clientIp[3]);
       break;
    // the following cases will not happen
    case TCPSOCKET_CONNECTED:
        printf("Listening: TCP Socket Connected\r\n");
        break;
    case TCPSOCKET_WRITEABLE:
        printf("Listening: TCP Socket Writable\r\n");
        break;
    case TCPSOCKET_READABLE:
        printf("Listening: TCP Socket Readable\r\n");
        break;
    case TCPSOCKET_CONTIMEOUT:
        printf("Listening: TCP Socket Timeout\r\n");
        break;
    case TCPSOCKET_CONRST:
        printf("Listening: TCP Socket CONRST\r\n");
        break;
    case TCPSOCKET_CONABRT:
        printf("Listening: TCP Socket CONABRT\r\n");
        break;
    case TCPSOCKET_ERROR:
        printf("Listening: TCP Socket Error\r\n");
        break;
    case TCPSOCKET_DISCONNECTED:
    //Close socket...
        printf("Listening: TCP Socket Disconnected\r\n");        
        ListeningSock.close();
        break;
    default:
        printf("DEFAULT\r\n"); 
     };
}