File content as of revision 0:de06f19dce2b:

#include "mbed.h"
#include "GSwifiInterface.h"
#include "Servo.h"

Servo servo1(p22);
Servo servo2(p23);

Serial pc(USBTX, USBRX);

GSwifiInterface gs(p13, p14, NC, NC, p9, "SSID", "PHRASE", GSwifi::SEC_WPA_PSK);
// TX, RX, CTS, RTS, RESET

unsigned char buff[30];
int RecvCount = 0;
int ParamLen = 0;
int ParamCount = 0;
void Recv(unsigned char data);
void ServoCmdExec();

int main () {
    pc.baud(115200);
    RecvCount = 0;
    ParamLen = 0;
    ParamCount = 0;
    servo1 = 0.5f;
    servo2 = 0.5f;
    pc.printf("OK");

    gs.init("192,168,1,17", "255,255,255,0", "192,168,1,254"); // use Static IP
    if (gs.connect() == false) {
        return -1; // join the network
    }
    printf("IP Address is %s\n\r", gs.getIPAddress());
    
    TCPSocketServer server;
    server.bind(10002);
    server.listen();
    
    while (true) {
        printf("\nWait for new connection...\n");
        TCPSocketConnection client;
        server.accept(client);
        client.set_blocking(false, 1500); // Timeout after (1.5)s
        
        printf("Connection from: %s\n", client.get_address());
        char buf[300];
        while (true) {
            int len = client.receive(buf, sizeof(buf));
            for (int i = 0; i < len; i++) {
                Recv((unsigned char)buf[i]);
            }
        }
        
        client.close();
    }

    gs.disconnect();
}

void Recv(unsigned char data) {
    switch (RecvCount) {
        case 0:
            ParamLen = 0;
            ParamCount = 0;
            if (data == 0xff) { // Headder
                buff[RecvCount++] = data;
            }
            break;
        case 1:
            if (data == 0) {    // ID
                buff[RecvCount++] = data;
            }
            break;
        case 2:                 // LENGTH
            buff[RecvCount++] = data;
            ParamLen = data;
            break;
        default:
            if (ParamLen > 0) {
                buff[RecvCount++] = data;
                ParamCount++;
                if (ParamCount >= ParamLen) {
                    ServoCmdExec();
                    RecvCount = 0;
                }
            } else {
                RecvCount = 0;
            }
            break;
    }
}

void ServoCmdExec() {
    float tmp1 = (float)buff[4]/255.0f;
    float tmp2 = (float)buff[5]/255.0f;
    servo1 = tmp1;
    servo2 = tmp2;

    pc.printf("servo1 = %02Xh %.1f",buff[4],tmp1);
    pc.printf("servo2 = %02Xh %.1f",buff[5],tmp2);
}