File content as of revision 3:9844ec849c12:

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

BusOut myleds(LED1, LED2, LED3, LED4);
Ticker NetPoll;
Timer ErrorTime;
Timer systemtime;
Queue UDP_queue(256,1);
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 dtime(int x){
myleds=x;
//pc.printf("%d - time=%f\n",x,systemtime.read());
}

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)
{
dtime(1);
    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]);
dtime(2);
   // 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;
    dtime(3);
    
    if (X<0.1 && Y<0.1 && X>-0.1 && Y>-0.1) {
        Ml=0.5;
        Mr=0.5;
        dtime(4);
    } else {
    dtime(5);
        if (Y>0 && X>0) { //quadrant 1
            ML=sqrt((X*X)+(Y*Y));
            MR=X-Y;
        }
        if (Y>0 && X<0) { //quadrant 2
            ML=Y+X;
            MR=-sqrt((X*X)+(Y*Y));
        }
        if (Y<0 && X<0) { //quadrant 3
            ML=-sqrt((X*X)+(Y*Y));
            MR=X-Y;
        }
        if (Y<0 && X>0) { //quadrant 4
            ML=X+Y;
            MR=sqrt((X*X)+(Y*Y));
        }
    Ml=((ML*ML*ML)+1)/2.0;
    Mr=((MR*MR*MR)+1)/2.0;
    }
    dtime(6);
    //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];
        dtime(7);
    }*/
}

void onUDPSocketEvent(UDPSocketEvent e)
{
dtime(14);
    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);
            }
               // UDP.recvfrom( messageBufferIncoming, 255, &host );
                //messageProcess();
            break;
    }
}


int main()
{
    systemtime.start();
    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) {
        dtime(15);
            Mr=.50;
            Ml=0.5;
           
            ErrorTime.reset();
        }

    }
}