File content as of revision 1:d131f93d1828:

#include "NetCentricApp.h"
#include "MbedCommand.h"
#include "motordriver.h"

Motor m(p21, p27, p28, 1); // pwm, fwd, rev, can brake
Serial pc(USBTX, USBRX);
AnalogIn ain(p16);
DigitalOut led1(LED1);

// function of the servo system
float f(int x){
  return -9.0319860139858321e-003  
        + 3.0391558663558485e-002 * x 
        + -7.2787622377621999e-003 * (x*x) 
        + 1.4421476301476278e-003 * (x*x*x);
}

// Check if the motor is in error state
void error_check(){
    if (m.state() == -3){
        m.speed(0);
        m.stop(1);
        printf("ERROR");
        while(1){
            led1=1;
            wait(0.2);
            led1=0;
            wait(0.2);
        }
    }   
}

void goto_pos(int x){
    led1 = 1;
    float pos = ain.read();
    float pos_new = f(x);
    if (pos_new > 1.0){
        pos_new = 1;
    }
    else if (pos_new < 0.0){
        pos_new = 0;   
    }
    printf("Go to position %i ", x);
    if (pos < pos_new){
        m.speed(1);
    }
    else if (pos > pos_new){
        m.speed(-1); 
    }
    while( (pos < (pos_new - (pos_new*0.01))) || (pos > (pos_new + (pos_new*0.01))) ){
        error_check();
        pos = ain.read();
    }
    m.stop(1);
    m.speed(0);
    led1 = 0;   
}

// Process commands here.
MbedResponse *NetCentricApp::getResponse(MbedRequest *request) {
    MbedResponse *response = new MbedResponse();
    response->requestId = request->id;
    response->commandId = request->commandId;
    response->values = NULL;
    response->error = NO_ERROR;
    response->n = 0;
    switch(request->commandId) {
        case COMMAND_ZERO:
            goto_pos(0);
            break;
        case COMMAND_ONE:
            goto_pos(1);
            response->error = NO_ERROR;
            break;
        case COMMAND_TWO:
            goto_pos(2);
            break;
        case COMMAND_THREE:
            goto_pos(3);
            break;
        case COMMAND_FOUR:
            goto_pos(4);
            break;
        case COMMAND_FIVE:
            goto_pos(5);
            break;
        case COMMAND_SIX:
            goto_pos(6);
            break;
        case COMMAND_SEVEN:
            goto_pos(7);
            break;
        case COMMAND_EIGHT:
            goto_pos(8);
            break;
        case COMMAND_NINE:
            goto_pos(9);
            break;
        case COMMAND_TEN:
            goto_pos(10);
            break;
        case COMMAND_LEFT:
            led1 = 1;
            m.speed(-1); 
            wait(1);
            m.stop(1);
            led1 = 0; 
            break;
        case COMMAND_RIGHT: 
            led1 = 1;
            m.speed(1); 
            wait(1);  
            m.stop(1);
            led1 = 0;
            break;
        case COMMAND_POSITION:
            response->n = 1;
            response->values = new float[1];
            response->values[0] = ain.read();
            break;
        default:
            MbedResponse *commandNotFound = new MbedResponse();
            commandNotFound->requestId = request->id;
            commandNotFound->commandId = request->commandId;
            commandNotFound->error = ERR_COMMAND_NOT_FOUND;
            commandNotFound->n = 0;
            commandNotFound->values = NULL;
            return commandNotFound;
    }
        return response;
}

// Setup once a device is connected.
void NetCentricApp::setupDevice() {
    printf("Connected to Android!\r\n");
}

// Called on disconnect.
void NetCentricApp::resetDevice() {
    printf("Disconnected\r\n");
}


// Construction of requests.
int NetCentricApp::callbackRead(u8 *buffer, int len) {
    if (len > 0) {
        // Parse request, format:
        //  int     - request ID
        //  int     - command ID
        //  ubyte   - # args
        //  float[] -- args
        
        // Note len is fixed as the packet is always equally big. Don't try to use
        // packets of variable size, the smallest size of a encountered packet is 
        // used.
        
        MbedRequest *request = new MbedRequest();
        
        request->id = getInt(buffer, 0, len);
        request->commandId = getInt(buffer, 4, len);
        request->n = getInt(buffer, 8, len);
        request->args = NULL;
        
        printf("request: %i, command: %i, n-args: %i\r\n", request->id, request->commandId, request->n);
        
        int n = request->n;
        if (n > 0) {
            request->args = new float[n];
            for (int i = 0; i < n; i++) {
                int offset = 12 + (i * 4);
                float f = getFloat(buffer, offset, len);
                request->args[i] = f;
            }
        }
        
        // Construct and send response.
        MbedResponse *response = getResponse(request);
        int responseSize = 4 + 4 + 4 + 4 + (response->n*4);
        u8 responseBuffer[responseSize];
        
        memcpy(responseBuffer + 0, reinterpret_cast<u8 const *>(&response->requestId), 4);
        memcpy(responseBuffer + 4, reinterpret_cast<u8 const *>(&response->commandId), 4);
        memcpy(responseBuffer + 8, reinterpret_cast<u8 const *>(&response->error), 4);
        memcpy(responseBuffer + 12, reinterpret_cast<u8 const *>(&response->n), 4);
        if (response->n > 0) {
            for (int i = 0; i < response->n; i++)  {
                float f = response->values[i];
                memcpy(responseBuffer + 16 + i*4, reinterpret_cast<u8 const *>(&f), 4);
            }
            
        }
        
        write(responseBuffer, responseSize);
        
        // Clean up.
        if (request->n > 0) {
            delete[] request->args;
        }
        delete request;
        
        if (response->n > 0) {
            delete[] response->values;
        }
        delete response;
    }
    
    return 0;
}

// Called to confirm a write operation.
int NetCentricApp::callbackWrite() {
    return 0;
}


/* Unsigned byte to primitives. Little endian assumed, Java sends Big endian by default. */
float NetCentricApp::getFloat(u8 *buffer, int offset, int bufferLen) {
    if (offset + 3 > bufferLen) {
        printf("float index out of bounds!\r\n");
        return 0.0;
    }
        
    float f;
    memcpy(&f, buffer + offset, sizeof(f));
    return f;
}

int NetCentricApp::getInt(u8 *buffer, int offset, int bufferLen) {
    if (offset + 3 > bufferLen) {
        printf("int index out of bounds!\r\n");
        return 0;
    }
        
    int i;
    memcpy(&i, buffer + offset, sizeof(i));
    return i;
}

u8 NetCentricApp::getUByte(u8 *buffer, int offset, int bufferLen) {
    if (offset > bufferLen) {
        printf("byte index out of bounds!\r\n");
        return 0;
    }
    
    u8 b;
    memcpy(&b, buffer + offset, sizeof(b));
    return b;
}