File content as of revision 0:5abac0d420fc:

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

DigitalOut led1(LED1, "led1");
DigitalOut led2(LED2, "led2");
DigitalOut led3(LED3, "led3");
DigitalOut led4(LED4, "led4");
AnalogIn ana_x(p19, "ana_x");
AnalogIn ana_y(p20, "ana_y");
DigitalIn init(p5);
DigitalIn garbage(p6);
DigitalIn closesock(p7);
ADXL345_I2C accelerometer(p9, p10);


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
);


// == stability vars
Ticker routine;

float base_x_0 = 50;
float base_y_0 = 50;
float base_x_1 = 50;
float base_y_1 = 50;
float base_x_2 = 50;
float base_y_2 = 50;
float base_x_3 = 50;
float base_y_3 = 50;

float motor_0 = 50;
float motor_1 = 50;
float motor_2 = 50;
float motor_3 = 50;

float a_motor_0 = motor_0;
float a_motor_1 = motor_1;
float a_motor_2 = motor_2;
float a_motor_3 = motor_3;

long x = 50;
long y = 50;

// == end

int go = 0;
int countertje = 0;

#define TCP_LISTENING_PORT 1337

TCPSocket ListeningSock; //Listener
TCPSocket* pConnectedSock; // for ConnectedSock
Host client; 
TCPSocketErr err;
char data[128];

void sender(const char*);
void interrupt();
void onConnectedTCPSocketEvent( TCPSocketEvent );
void onListeningTCPSocketEvent( TCPSocketEvent );

int main() {

    //Go into standby mode to configure the device.
    accelerometer.setPowerControl(0x00);
 
    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);
     
    //3.2kHz data rate.
    accelerometer.setDataRate(ADXL345_3200HZ);
 
    //Measurement mode.
    accelerometer.setPowerControl(0x08);

    EthernetErr ethErr = eth.setup();
      if(ethErr)
      {
        led3 = 1;
        return -1;
      }
    // Set the callbacks for Listening
    ListeningSock.setOnEvent(&onListeningTCPSocketEvent); 
    // bind and listen on TCP
    err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT)); // listen on own ipaddres, port 1337
    if(err)
    {
    //Deal with that error...
        led3 = 1;
    }
    err=ListeningSock.listen(); // Starts listening
    if(err)
    {
         led3 = 1;
    }
    routine.attach(&interrupt, 1);
    Timer timer;
    timer.start();
    while(true)
    {
        Net::poll(); //poll TCPstack
        while(init) {
            if(!init){
                sender("220 Connected\n");
                go=1;
                break;
                
            }
        }
        while(closesock) {
            if(!closesock){
                led1=0;
                go=0;
                pConnectedSock->close();
                break;
            }
        }
        if(go){
            if(countertje == 10){
                countertje = 0;
            }
            char buffer [128];
            sprintf (buffer, "%d\n",countertje);
            sender(buffer);
            sprintf (buffer, "x: %d\ny: %d\nmotor_0: %f\nmotor_1: %f\nmotor_2: %f\nmotor_3: %f\n", x,y,motor_0,motor_1,motor_2,motor_3);
            sender(buffer);
            char receive[128];
            while ( int len = pConnectedSock->recv(receive, 128) ) {
              led2 != led2;
              if(strcmp(receive,"Got it!")==0){
                led2 = 0;
                break;
              }
            }
            /*
            sprintf (buffer, "y: %d\n", y);
            sender(buffer);
            sprintf (buffer, "motor_0: %d\n", motor_0);
            sender(buffer);
            sprintf (buffer, "motor_1: %d\n", motor_1);
            sender(buffer);
            sprintf (buffer, "motor_2: %d\n", motor_2);
            sender(buffer);
            sprintf (buffer, "motor_3: %d\n", motor_3);
            sender(buffer); */
            countertje++;
        }
        if(timer.read() > 0.2) 
        {
            led4=!led4; //Status light toggle
            timer.reset();
        }
  }
}

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


void interrupt() 
{
    x = accelerometer.getAx();
    y = accelerometer.getAy();

    a_motor_0 = a_motor_0-((((base_x_0-x)/2)+((base_x_0-y)/2))/1000);
    if(a_motor_0>99){
        motor_0 = 100;}
    else if(a_motor_0 < 1){
        motor_0 = 0;}
    else{
        motor_0 = a_motor_0;}
    a_motor_1 = a_motor_1-((((base_x_1-x)/2)-((base_x_1-y)/2))/1000);
    if(a_motor_1>99){
        motor_1 = 100;}
    else if(a_motor_1 < 1){
        motor_1 = 0;}
    else{
        motor_1 = a_motor_1;}
    a_motor_2 = a_motor_2+((((base_x_2-x)/2)-((base_x_2-y)/2))/1000);
    if(a_motor_2>99){
        motor_2 = 100;}
    else if(a_motor_2 < 1){
        motor_2 = 0;}
    else{
        motor_2 = a_motor_2;}
    a_motor_3 = a_motor_3+((((base_x_3-x)/2)+((base_x_3-y)/2))/1000);
    if(a_motor_3>99){
        motor_3 = 100;}
    else if(a_motor_3 < 1){
        motor_3 = 0;}
    else{
        motor_3 = a_motor_3;}
}

void onConnectedTCPSocketEvent(TCPSocketEvent e)
{
   switch(e)
    {
    case TCPSOCKET_CONNECTED:
        led1 = 1;
        break;
    case TCPSOCKET_WRITEABLE:
      //Can now write some data...
        led1 = 1;
        break;
    case TCPSOCKET_READABLE:
      //Can now read dome data...
        led1 = 1;
       // 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);
           //buff[len]=0; // make terminater
           led2 != led2;
           strcpy(data,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();
        led1=0;
        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) {
            led3 = 1;
            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:
        led3 = 1;
        break;
    case TCPSOCKET_DISCONNECTED:
    //Close socket...
        printf("Listening: TCP Socket Disconnected\r\n");        
        ListeningSock.close();
        break;
    default:
        printf("DEFAULT\r\n"); 
     };
}