File content as of revision 4:bae1c47dc112:

#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);
Servo Arm(p23);
Servo Elbow(p24);
float MR=.5;
float ML=.5;

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

char messageBufferIncoming[256];

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

void right(){
Ml=1;
Mr=1;
wait(.25);
}

void left(){
Ml=0;
Mr=0;
wait(.25);
}

void forward(float time){
Ml=1;
Mr=0;
wait(time);
}

void backwards(float time){
Ml=0;
Mr=1;
wait(time);
}


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 Y=Joy.axis[1];
    float X=Joy.axis[0];
    Arm=Joy.axis[2];
    Elbow=(Joy.axis[3]+1)/2.0;
    Y=-Y;
   
    
    if (X<.1 && Y<.1 && X>-.1 && Y>-.1) {
        Ml=.5;
        Mr=.5;
    } else {
    
        if (Y>0 && X>0) {
            ML=sqrt((X*X)+(Y*Y));
            MR=Y-X;
        }
        if (Y>0 && X<0) {
            ML=Y+X;
            MR=sqrt((X*X)+(Y*Y));
        }
        if (Y<0 && X<0) {
            ML=Y-X;
            MR=-sqrt((X*X)+(Y*Y));
        }
        if (Y<0 && X>0) {
            ML=-sqrt((X*X)+(Y*Y));
            MR=Y+X;
        }
    Ml=(ML+1)/2.0;
    Mr=(1-MR)/2.0;
    }
    pc.printf("X=%f Y=%f ML=%f MR=%f\r\n", X,Y,Ml.read(),Mr.read());
    /*
    
    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();

right();
forward(1);
right();
forward(1);
right();
forward(1);
right();
forward(1);
left();
left();
backwards(.2);

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

    }
}