File content as of revision 7:d9d30a2d8d9f:

#include "NetCentricApp.h"
#include "MbedCommand.h"
#include "HBridge.h"
#include "MotorController.h"
#include "PcControls.h"

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

int* intToFourBits (int number) {
    int bits [4] = {};
    
    for (int i = 0; i < 4; i++) {
        bits[i] = number & (1 << i) ? 1 : 0;
    }
    
    return bits;
}

/**
 * Set the 4 LEDS on the mBed to the first 4 bits of the integer
 */
void setLeds (int number) {
    DigitalOut leds [] = {myled1, myled2, myled3, myled4};
    
    int* bits = intToFourBits(number);
    
    for (int i = 0; i < 4; i++) {
        leds[i] = bits[i];
    }
}

// Process commands here.
MbedResponse *NetCentricApp::getResponse(MbedRequest *request) {
    if (request->commandId == COMMAND_SUM) {
        return sumCommand(request);
    } else if (request->commandId == COMMAND_AVG) {
        return avgCommand(request);
    } else if (request->commandId == COMMAND_LED) {
        return ledCommand(request);
    } else if (request->commandId == COMMAND_MOTOR) {
        return motorCommand(request);
    } else if (request->commandId == COMMAND_STATUS) {
        return statusCommand(request);
    }
    
    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;
}

// Sample commands.
MbedResponse *NetCentricApp::sumCommand(MbedRequest *request) {
    float sum = 0.0f;
    for (int i = 0; i < request->n; i++) {
        sum += request->args[i];
    }
    
    MbedResponse *r = new MbedResponse();
    r->requestId = request->id;
    r->commandId = request->commandId;
    r->error = NO_ERROR;
    r->n = 1;
    r->values = new float[1];
    r->values[0] = sum;
    return r;
}

MbedResponse *NetCentricApp::avgCommand(MbedRequest *request) {
    float sum = 0.0f;
    for (int i = 0; i < request->n; i++) {
        sum += request->args[i];
    }
    
    MbedResponse *r = new MbedResponse();
    r->requestId = request->id;
    r->commandId = request->commandId;
    r->error = NO_ERROR;
    r->n = 1;
    r->values = new float[1];
    
    if (request->n > 0) {
        r->values[0] = sum / request->n;
    } else {
        r->values[0] = sum;
    }
    return r;
}

// Control the LED's.
MbedResponse *NetCentricApp::ledCommand(MbedRequest *request) {
    DigitalOut led1(LED1);
    DigitalOut led2(LED2);
    DigitalOut led3(LED3);
    DigitalOut led4(LED4);
    
    if (request->n > 0) led1 = request->args[0];
    if (request->n > 1) led2 = request->args[1];
    if (request->n > 2) led3 = request->args[2];
    if (request->n > 3) led4 = request->args[3];
    
    MbedResponse *r = new MbedResponse();
    r->requestId = request->id;
    r->commandId = request->commandId;
    r->error = NO_ERROR;
    r->n = 4;
    r->values = new float[4];
    r->values[0] = led1;
    r->values[1] = led2;
    r->values[2] = led3;
    r->values[3] = led4;
    
    return r;
}

// Move the motor
MbedResponse *NetCentricApp::motorCommand(MbedRequest *request) {
    setLeds((int) request->args[0]);
    char c = (char) request->args[0];
    this->motorControlsPc.ProcessPcInput(c);

    // generate response
    MbedResponse *r = new MbedResponse();
    r->requestId = request->id;
    r->commandId = request->commandId;
    r->error = NO_ERROR;
    
    if (c == MotorControlsPc::MOTOR_POSITION) {
        r->n = 2;
        r->values = new float[r->n];
        r->values[1] = this->motorControlsPc.getMotorController().getPosition();
    } else {
        r->n = 1;
        r->values = new float[r->n];
    }
    r->values[0] = request->args[0];
    
    return r;
}

MbedResponse *NetCentricApp::statusCommand(MbedRequest *request) {
    MbedResponse *r = new MbedResponse();
    r->requestId = request->id;
    r->commandId = request->commandId;
    r->error = NO_ERROR;
    r->n = 1;
    
    r->values = new float[r->n];
    r->values[0] = (float) this->motorControlsPc.getMotorController().getCurrentStatus();
    
    return r;
}

// 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;
}