File content as of revision 0:8101522bb770:

#include "mbed.h"
#include "EthernetNetIf.h"
#include "UDPSocket.h"
#include "queue.h"
#include "Servo.h"

Ticker NetPoll;
Timer ErrorTime;
Queue UDP_queue(256,20);
Serial pc(USBTX, USBRX);
EthernetNetIf * eth;
UDPSocket UDP;
Host Robot;
Servo ML(p21);
Servo MR(p22);

DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)};

char messageBufferIncoming[256];

struct Joystick {
    float axis[4];
    float button[12];
};


Joystick Joy;
void messageProcess(void)
{

    ErrorTime.reset();
    // pc.printf("%s\r\n",messageBufferIncoming);
    sscanf(messageBufferIncoming,"%f%f%f%f",\
           &Joy.axis[0],\
           &Joy.axis[1],\
           &Joy.axis[2],\
           &Joy.axis[3]);

    pc.printf("%f %f %f %f\r\n", Joy.axis[0],Joy.axis[1],Joy.axis[2],Joy.axis[3]);
    /*
    sscanf(messageBufferIncoming,"%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f",\
    &Joy.axis[0],\
    &Joy.axis[1],\
    &Joy.axis[2],\
    &Joy.axis[3],\
    &Joy.button[0],\
    &Joy.button[1],\
    &Joy.button[2],\
    &Joy.button[3],\
    &Joy.button[4],\
    &Joy.button[5],\
    &Joy.button[6],\
    &Joy.button[7],\
    &Joy.button[8],\
    &Joy.button[9],\
    &Joy.button[10],\
    &Joy.button[11]);
    
    */
    
   /*
    float A=Joy.axis[1];
    float B=Joy.axis[0];
    if (A<.1 && B<.1 && B>-.1 && A>-.1) {
        MR=.5;
        ML=.5;
    } else {

        if (A>.1 || A<-.1) {
            ML=1-(A+1)/2;
            MR=(A+1)/2;
        }
        if (B>.25 || B<-.25) {
            ML=1-(B+1)/2;
            MR=1-(B+1)/2;
        }
    }
    
    */
    ML=1;
    MR=0;
    for (int x=0; x<8; x++) {
        leds[x]=(bool)Joy.button[x];
    }
}

void onUDPSocketEvent(UDPSocketEvent e)
{
    switch (e) {
        case UDPSOCKET_READABLE: //The only event for now
            char buf[256] = {0};
            Host host;
            while ( int len = UDP.recvfrom( buf, 255, &host ) ) {
                if ( len <= 0 )
                    break;
                UDP_queue.Put(buf);
            }
            break;
    }
}


int main()
{

    eth = new EthernetNetIf(
        IpAddr(IpAddr(192,168,1,102)),  // My IP Address
        IpAddr(IpAddr(255,255,255,0)), //Network Mask
        IpAddr(IpAddr(192,168,1,2)), //Gateway
        IpAddr(IpAddr())  //DNS
    );
    Robot.setIp(IpAddr(192,168,1,102));
    Robot.setPort(12345);

    eth->setup();
    UDP.setOnEvent(&onUDPSocketEvent);
    UDP.bind(Robot);
    NetPoll.attach_us(&Net::poll,500);

    ErrorTime.start();

    while (1) {
        if ( UDP_queue.Get(messageBufferIncoming))messageProcess();
        if (ErrorTime.read()>.2) {
            MR=.50;
            ML=0.5;
            for (int x=0; x<8; x++)leds[x]=0;
            ErrorTime.reset();
        }

    }
}