UvA NetCentric Computing listener
Dependencies: AndroidAccessory Controls HBridge MotorController mbed
Fork of uva_nc by
NetCentricApp.cpp
- Committer:
- Sinterbaas
- Date:
- 2016-01-10
- Revision:
- 7:d9d30a2d8d9f
- Parent:
- 6:b5072fb695f2
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; }