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@4:eabd917e3353, 2016-01-07 (annotated)

Committer:
jurgy
Date:
Thu Jan 07 16:20:07 2016 +0000
Revision:
4:eabd917e3353
Parent:
3:0229ad4f2db1
Child:
6:b5072fb695f2
all of dem fixes

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