Netcentric Computing / Mbed 2 deprecated uva_nc

UvA NetCentric Computing listener

Dependencies:   AndroidAccessory Controls HBridge MotorController mbed

Fork of uva_nc by Jurgen Baas

NetCentricApp.cpp@7:d9d30a2d8d9f, 2016-01-10 (annotated)

Committer:
Sinterbaas
Date:
Sun Jan 10 23:48:29 2016 +0000
Revision:
7:d9d30a2d8d9f
Parent:
6:b5072fb695f2 
Remove the ghetto

Who changed what in which revision?

UserRevisionLine numberNew contents of line
diepvriezer 0:0f9ceecf1db8 1 #include "NetCentricApp.h"
diepvriezer 0:0f9ceecf1db8 2 #include "MbedCommand.h"
jurgy 1:3cde55629b1d 3 #include "HBridge.h"
jurgy 1:3cde55629b1d 4 #include "MotorController.h"
jurgy 1:3cde55629b1d 5 #include "PcControls.h"
diepvriezer 0:0f9ceecf1db8 6
jurgy 3:0229ad4f2db1 7 DigitalOut myled1(LED1);
jurgy 3:0229ad4f2db1 8 DigitalOut myled2(LED2);
jurgy 3:0229ad4f2db1 9 DigitalOut myled3(LED3);
jurgy 3:0229ad4f2db1 10 DigitalOut myled4(LED4);
jurgy 3:0229ad4f2db1 11
jurgy 3:0229ad4f2db1 12 int* intToFourBits (int number) {
jurgy 3:0229ad4f2db1 13 int bits [4] = {};
jurgy 3:0229ad4f2db1 14
jurgy 3:0229ad4f2db1 15 for (int i = 0; i < 4; i++) {
jurgy 3:0229ad4f2db1 16 bits[i] = number & (1 << i) ? 1 : 0;
jurgy 3:0229ad4f2db1 17 }
jurgy 3:0229ad4f2db1 18
jurgy 3:0229ad4f2db1 19 return bits;
jurgy 3:0229ad4f2db1 20 }
jurgy 3:0229ad4f2db1 21
Sinterbaas 7:d9d30a2d8d9f 22 /**
Sinterbaas 7:d9d30a2d8d9f 23 * Set the 4 LEDS on the mBed to the first 4 bits of the integer
Sinterbaas 7:d9d30a2d8d9f 24 */
jurgy 3:0229ad4f2db1 25 void setLeds (int number) {
jurgy 3:0229ad4f2db1 26 DigitalOut leds [] = {myled1, myled2, myled3, myled4};
jurgy 3:0229ad4f2db1 27
jurgy 3:0229ad4f2db1 28 int* bits = intToFourBits(number);
jurgy 3:0229ad4f2db1 29
jurgy 3:0229ad4f2db1 30 for (int i = 0; i < 4; i++) {
jurgy 3:0229ad4f2db1 31 leds[i] = bits[i];
jurgy 3:0229ad4f2db1 32 }
jurgy 3:0229ad4f2db1 33 }
jurgy 3:0229ad4f2db1 34
diepvriezer 0:0f9ceecf1db8 35 // Process commands here.
diepvriezer 0:0f9ceecf1db8 36 MbedResponse *NetCentricApp::getResponse(MbedRequest *request) {
diepvriezer 0:0f9ceecf1db8 37 if (request->commandId == COMMAND_SUM) {
diepvriezer 0:0f9ceecf1db8 38 return sumCommand(request);
diepvriezer 0:0f9ceecf1db8 39 } else if (request->commandId == COMMAND_AVG) {
diepvriezer 0:0f9ceecf1db8 40 return avgCommand(request);
diepvriezer 0:0f9ceecf1db8 41 } else if (request->commandId == COMMAND_LED) {
diepvriezer 0:0f9ceecf1db8 42 return ledCommand(request);
jurgy 1:3cde55629b1d 43 } else if (request->commandId == COMMAND_MOTOR) {
jurgy 1:3cde55629b1d 44 return motorCommand(request);
Sinterbaas 6:b5072fb695f2 45 } else if (request->commandId == COMMAND_STATUS) {
Sinterbaas 6:b5072fb695f2 46 return statusCommand(request);
diepvriezer 0:0f9ceecf1db8 47 }
diepvriezer 0:0f9ceecf1db8 48
diepvriezer 0:0f9ceecf1db8 49 MbedResponse *commandNotFound = new MbedResponse();
diepvriezer 0:0f9ceecf1db8 50 commandNotFound->requestId = request->id;
diepvriezer 0:0f9ceecf1db8 51 commandNotFound->commandId = request->commandId;
diepvriezer 0:0f9ceecf1db8 52 commandNotFound->error = ERR_COMMAND_NOT_FOUND;
diepvriezer 0:0f9ceecf1db8 53 commandNotFound->n = 0;
diepvriezer 0:0f9ceecf1db8 54 commandNotFound->values = NULL;
diepvriezer 0:0f9ceecf1db8 55
diepvriezer 0:0f9ceecf1db8 56 return commandNotFound;
diepvriezer 0:0f9ceecf1db8 57 }
diepvriezer 0:0f9ceecf1db8 58
diepvriezer 0:0f9ceecf1db8 59 // Sample commands.
diepvriezer 0:0f9ceecf1db8 60 MbedResponse *NetCentricApp::sumCommand(MbedRequest *request) {
diepvriezer 0:0f9ceecf1db8 61 float sum = 0.0f;
diepvriezer 0:0f9ceecf1db8 62 for (int i = 0; i < request->n; i++) {
diepvriezer 0:0f9ceecf1db8 63 sum += request->args[i];
diepvriezer 0:0f9ceecf1db8 64 }
diepvriezer 0:0f9ceecf1db8 65
diepvriezer 0:0f9ceecf1db8 66 MbedResponse *r = new MbedResponse();
diepvriezer 0:0f9ceecf1db8 67 r->requestId = request->id;
diepvriezer 0:0f9ceecf1db8 68 r->commandId = request->commandId;
diepvriezer 0:0f9ceecf1db8 69 r->error = NO_ERROR;
diepvriezer 0:0f9ceecf1db8 70 r->n = 1;
diepvriezer 0:0f9ceecf1db8 71 r->values = new float[1];
diepvriezer 0:0f9ceecf1db8 72 r->values[0] = sum;
diepvriezer 0:0f9ceecf1db8 73 return r;
diepvriezer 0:0f9ceecf1db8 74 }
diepvriezer 0:0f9ceecf1db8 75
diepvriezer 0:0f9ceecf1db8 76 MbedResponse *NetCentricApp::avgCommand(MbedRequest *request) {
diepvriezer 0:0f9ceecf1db8 77 float sum = 0.0f;
diepvriezer 0:0f9ceecf1db8 78 for (int i = 0; i < request->n; i++) {
diepvriezer 0:0f9ceecf1db8 79 sum += request->args[i];
diepvriezer 0:0f9ceecf1db8 80 }
diepvriezer 0:0f9ceecf1db8 81
diepvriezer 0:0f9ceecf1db8 82 MbedResponse *r = new MbedResponse();
diepvriezer 0:0f9ceecf1db8 83 r->requestId = request->id;
diepvriezer 0:0f9ceecf1db8 84 r->commandId = request->commandId;
diepvriezer 0:0f9ceecf1db8 85 r->error = NO_ERROR;
diepvriezer 0:0f9ceecf1db8 86 r->n = 1;
diepvriezer 0:0f9ceecf1db8 87 r->values = new float[1];
diepvriezer 0:0f9ceecf1db8 88
diepvriezer 0:0f9ceecf1db8 89 if (request->n > 0) {
diepvriezer 0:0f9ceecf1db8 90 r->values[0] = sum / request->n;
diepvriezer 0:0f9ceecf1db8 91 } else {
diepvriezer 0:0f9ceecf1db8 92 r->values[0] = sum;
diepvriezer 0:0f9ceecf1db8 93 }
diepvriezer 0:0f9ceecf1db8 94 return r;
diepvriezer 0:0f9ceecf1db8 95 }
diepvriezer 0:0f9ceecf1db8 96
diepvriezer 0:0f9ceecf1db8 97 // Control the LED's.
diepvriezer 0:0f9ceecf1db8 98 MbedResponse *NetCentricApp::ledCommand(MbedRequest *request) {
diepvriezer 0:0f9ceecf1db8 99 DigitalOut led1(LED1);
diepvriezer 0:0f9ceecf1db8 100 DigitalOut led2(LED2);
diepvriezer 0:0f9ceecf1db8 101 DigitalOut led3(LED3);
diepvriezer 0:0f9ceecf1db8 102 DigitalOut led4(LED4);
diepvriezer 0:0f9ceecf1db8 103
diepvriezer 0:0f9ceecf1db8 104 if (request->n > 0) led1 = request->args[0];
diepvriezer 0:0f9ceecf1db8 105 if (request->n > 1) led2 = request->args[1];
diepvriezer 0:0f9ceecf1db8 106 if (request->n > 2) led3 = request->args[2];
diepvriezer 0:0f9ceecf1db8 107 if (request->n > 3) led4 = request->args[3];
diepvriezer 0:0f9ceecf1db8 108
diepvriezer 0:0f9ceecf1db8 109 MbedResponse *r = new MbedResponse();
diepvriezer 0:0f9ceecf1db8 110 r->requestId = request->id;
diepvriezer 0:0f9ceecf1db8 111 r->commandId = request->commandId;
diepvriezer 0:0f9ceecf1db8 112 r->error = NO_ERROR;
diepvriezer 0:0f9ceecf1db8 113 r->n = 4;
diepvriezer 0:0f9ceecf1db8 114 r->values = new float[4];
diepvriezer 0:0f9ceecf1db8 115 r->values[0] = led1;
diepvriezer 0:0f9ceecf1db8 116 r->values[1] = led2;
diepvriezer 0:0f9ceecf1db8 117 r->values[2] = led3;
diepvriezer 0:0f9ceecf1db8 118 r->values[3] = led4;
diepvriezer 0:0f9ceecf1db8 119
diepvriezer 0:0f9ceecf1db8 120 return r;
diepvriezer 0:0f9ceecf1db8 121 }
diepvriezer 0:0f9ceecf1db8 122
jurgy 1:3cde55629b1d 123 // Move the motor
jurgy 1:3cde55629b1d 124 MbedResponse *NetCentricApp::motorCommand(MbedRequest *request) {
jurgy 3:0229ad4f2db1 125 setLeds((int) request->args[0]);
jurgy 3:0229ad4f2db1 126 char c = (char) request->args[0];
jurgy 3:0229ad4f2db1 127 this->motorControlsPc.ProcessPcInput(c);
jurgy 4:eabd917e3353 128
jurgy 1:3cde55629b1d 129 // generate response
jurgy 1:3cde55629b1d 130 MbedResponse *r = new MbedResponse();
jurgy 1:3cde55629b1d 131 r->requestId = request->id;
jurgy 1:3cde55629b1d 132 r->commandId = request->commandId;
jurgy 1:3cde55629b1d 133 r->error = NO_ERROR;
jurgy 4:eabd917e3353 134
jurgy 4:eabd917e3353 135 if (c == MotorControlsPc::MOTOR_POSITION) {
jurgy 4:eabd917e3353 136 r->n = 2;
jurgy 4:eabd917e3353 137 r->values = new float[r->n];
jurgy 4:eabd917e3353 138 r->values[1] = this->motorControlsPc.getMotorController().getPosition();
jurgy 4:eabd917e3353 139 } else {
jurgy 4:eabd917e3353 140 r->n = 1;
jurgy 4:eabd917e3353 141 r->values = new float[r->n];
jurgy 4:eabd917e3353 142 }
jurgy 4:eabd917e3353 143 r->values[0] = request->args[0];
jurgy 1:3cde55629b1d 144
jurgy 1:3cde55629b1d 145 return r;
jurgy 1:3cde55629b1d 146 }
jurgy 1:3cde55629b1d 147
Sinterbaas 6:b5072fb695f2 148 MbedResponse *NetCentricApp::statusCommand(MbedRequest *request) {
Sinterbaas 6:b5072fb695f2 149 MbedResponse *r = new MbedResponse();
Sinterbaas 6:b5072fb695f2 150 r->requestId = request->id;
Sinterbaas 6:b5072fb695f2 151 r->commandId = request->commandId;
Sinterbaas 6:b5072fb695f2 152 r->error = NO_ERROR;
Sinterbaas 6:b5072fb695f2 153 r->n = 1;
Sinterbaas 6:b5072fb695f2 154
Sinterbaas 6:b5072fb695f2 155 r->values = new float[r->n];
Sinterbaas 6:b5072fb695f2 156 r->values[0] = (float) this->motorControlsPc.getMotorController().getCurrentStatus();
Sinterbaas 6:b5072fb695f2 157
Sinterbaas 6:b5072fb695f2 158 return r;
Sinterbaas 6:b5072fb695f2 159 }
Sinterbaas 6:b5072fb695f2 160
diepvriezer 0:0f9ceecf1db8 161 // Setup once a device is connected.
diepvriezer 0:0f9ceecf1db8 162 void NetCentricApp::setupDevice() {
diepvriezer 0:0f9ceecf1db8 163 printf("Connected to Android!\r\n");
diepvriezer 0:0f9ceecf1db8 164 }
diepvriezer 0:0f9ceecf1db8 165
diepvriezer 0:0f9ceecf1db8 166 // Called on disconnect.
diepvriezer 0:0f9ceecf1db8 167 void NetCentricApp::resetDevice() {
diepvriezer 0:0f9ceecf1db8 168 printf("Disconnected\r\n");
diepvriezer 0:0f9ceecf1db8 169 }
diepvriezer 0:0f9ceecf1db8 170
diepvriezer 0:0f9ceecf1db8 171
diepvriezer 0:0f9ceecf1db8 172 // Construction of requests.
diepvriezer 0:0f9ceecf1db8 173 int NetCentricApp::callbackRead(u8 *buffer, int len) {
diepvriezer 0:0f9ceecf1db8 174 if (len > 0) {
diepvriezer 0:0f9ceecf1db8 175 // Parse request, format:
diepvriezer 0:0f9ceecf1db8 176 // int - request ID
diepvriezer 0:0f9ceecf1db8 177 // int - command ID
diepvriezer 0:0f9ceecf1db8 178 // ubyte - # args
diepvriezer 0:0f9ceecf1db8 179 // float[] -- args
diepvriezer 0:0f9ceecf1db8 180
diepvriezer 0:0f9ceecf1db8 181 // Note len is fixed as the packet is always equally big. Don't try to use
diepvriezer 0:0f9ceecf1db8 182 // packets of variable size, the smallest size of a encountered packet is
diepvriezer 0:0f9ceecf1db8 183 // used.
diepvriezer 0:0f9ceecf1db8 184
diepvriezer 0:0f9ceecf1db8 185 MbedRequest *request = new MbedRequest();
diepvriezer 0:0f9ceecf1db8 186
diepvriezer 0:0f9ceecf1db8 187 request->id = getInt(buffer, 0, len);
diepvriezer 0:0f9ceecf1db8 188 request->commandId = getInt(buffer, 4, len);
diepvriezer 0:0f9ceecf1db8 189 request->n = getInt(buffer, 8, len);
diepvriezer 0:0f9ceecf1db8 190 request->args = NULL;
diepvriezer 0:0f9ceecf1db8 191
diepvriezer 0:0f9ceecf1db8 192 printf("request: %i, command: %i, n-args: %i\r\n", request->id, request->commandId, request->n);
diepvriezer 0:0f9ceecf1db8 193
diepvriezer 0:0f9ceecf1db8 194 int n = request->n;
diepvriezer 0:0f9ceecf1db8 195 if (n > 0) {
diepvriezer 0:0f9ceecf1db8 196 request->args = new float[n];
diepvriezer 0:0f9ceecf1db8 197 for (int i = 0; i < n; i++) {
diepvriezer 0:0f9ceecf1db8 198 int offset = 12 + (i * 4);
diepvriezer 0:0f9ceecf1db8 199 float f = getFloat(buffer, offset, len);
diepvriezer 0:0f9ceecf1db8 200 request->args[i] = f;
diepvriezer 0:0f9ceecf1db8 201 }
diepvriezer 0:0f9ceecf1db8 202 }
diepvriezer 0:0f9ceecf1db8 203
diepvriezer 0:0f9ceecf1db8 204 // Construct and send response.
diepvriezer 0:0f9ceecf1db8 205 MbedResponse *response = getResponse(request);
diepvriezer 0:0f9ceecf1db8 206 int responseSize = 4 + 4 + 4 + 4 + (response->n*4);
diepvriezer 0:0f9ceecf1db8 207 u8 responseBuffer[responseSize];
diepvriezer 0:0f9ceecf1db8 208
diepvriezer 0:0f9ceecf1db8 209 memcpy(responseBuffer + 0, reinterpret_cast<u8 const *>(&response->requestId), 4);
diepvriezer 0:0f9ceecf1db8 210 memcpy(responseBuffer + 4, reinterpret_cast<u8 const *>(&response->commandId), 4);
diepvriezer 0:0f9ceecf1db8 211 memcpy(responseBuffer + 8, reinterpret_cast<u8 const *>(&response->error), 4);
diepvriezer 0:0f9ceecf1db8 212 memcpy(responseBuffer + 12, reinterpret_cast<u8 const *>(&response->n), 4);
diepvriezer 0:0f9ceecf1db8 213 if (response->n > 0) {
diepvriezer 0:0f9ceecf1db8 214 for (int i = 0; i < response->n; i++) {
diepvriezer 0:0f9ceecf1db8 215 float f = response->values[i];
diepvriezer 0:0f9ceecf1db8 216 memcpy(responseBuffer + 16 + i*4, reinterpret_cast<u8 const *>(&f), 4);
diepvriezer 0:0f9ceecf1db8 217 }
diepvriezer 0:0f9ceecf1db8 218
diepvriezer 0:0f9ceecf1db8 219 }
diepvriezer 0:0f9ceecf1db8 220
diepvriezer 0:0f9ceecf1db8 221 write(responseBuffer, responseSize);
diepvriezer 0:0f9ceecf1db8 222
diepvriezer 0:0f9ceecf1db8 223 // Clean up.
diepvriezer 0:0f9ceecf1db8 224 if (request->n > 0) {
diepvriezer 0:0f9ceecf1db8 225 delete[] request->args;
diepvriezer 0:0f9ceecf1db8 226 }
diepvriezer 0:0f9ceecf1db8 227 delete request;
diepvriezer 0:0f9ceecf1db8 228
diepvriezer 0:0f9ceecf1db8 229 if (response->n > 0) {
diepvriezer 0:0f9ceecf1db8 230 delete[] response->values;
diepvriezer 0:0f9ceecf1db8 231 }
diepvriezer 0:0f9ceecf1db8 232 delete response;
diepvriezer 0:0f9ceecf1db8 233 }
diepvriezer 0:0f9ceecf1db8 234
diepvriezer 0:0f9ceecf1db8 235 return 0;
diepvriezer 0:0f9ceecf1db8 236 }
diepvriezer 0:0f9ceecf1db8 237
diepvriezer 0:0f9ceecf1db8 238 // Called to confirm a write operation.
diepvriezer 0:0f9ceecf1db8 239 int NetCentricApp::callbackWrite() {
diepvriezer 0:0f9ceecf1db8 240 return 0;
diepvriezer 0:0f9ceecf1db8 241 }
diepvriezer 0:0f9ceecf1db8 242
diepvriezer 0:0f9ceecf1db8 243
diepvriezer 0:0f9ceecf1db8 244 /* Unsigned byte to primitives. Little endian assumed, Java sends Big endian by default. */
diepvriezer 0:0f9ceecf1db8 245 float NetCentricApp::getFloat(u8 *buffer, int offset, int bufferLen) {
diepvriezer 0:0f9ceecf1db8 246 if (offset + 3 > bufferLen) {
diepvriezer 0:0f9ceecf1db8 247 printf("float index out of bounds!\r\n");
diepvriezer 0:0f9ceecf1db8 248 return 0.0;
diepvriezer 0:0f9ceecf1db8 249 }
diepvriezer 0:0f9ceecf1db8 250
diepvriezer 0:0f9ceecf1db8 251 float f;
diepvriezer 0:0f9ceecf1db8 252 memcpy(&f, buffer + offset, sizeof(f));
diepvriezer 0:0f9ceecf1db8 253 return f;
diepvriezer 0:0f9ceecf1db8 254 }
diepvriezer 0:0f9ceecf1db8 255
diepvriezer 0:0f9ceecf1db8 256 int NetCentricApp::getInt(u8 *buffer, int offset, int bufferLen) {
diepvriezer 0:0f9ceecf1db8 257 if (offset + 3 > bufferLen) {
diepvriezer 0:0f9ceecf1db8 258 printf("int index out of bounds!\r\n");
diepvriezer 0:0f9ceecf1db8 259 return 0;
diepvriezer 0:0f9ceecf1db8 260 }
diepvriezer 0:0f9ceecf1db8 261
diepvriezer 0:0f9ceecf1db8 262 int i;
diepvriezer 0:0f9ceecf1db8 263 memcpy(&i, buffer + offset, sizeof(i));
diepvriezer 0:0f9ceecf1db8 264 return i;
diepvriezer 0:0f9ceecf1db8 265 }
diepvriezer 0:0f9ceecf1db8 266
diepvriezer 0:0f9ceecf1db8 267 u8 NetCentricApp::getUByte(u8 *buffer, int offset, int bufferLen) {
diepvriezer 0:0f9ceecf1db8 268 if (offset > bufferLen) {
diepvriezer 0:0f9ceecf1db8 269 printf("byte index out of bounds!\r\n");
diepvriezer 0:0f9ceecf1db8 270 return 0;
diepvriezer 0:0f9ceecf1db8 271 }
diepvriezer 0:0f9ceecf1db8 272
diepvriezer 0:0f9ceecf1db8 273 u8 b;
diepvriezer 0:0f9ceecf1db8 274 memcpy(&b, buffer + offset, sizeof(b));
diepvriezer 0:0f9ceecf1db8 275 return b;
diepvriezer 0:0f9ceecf1db8 276 }