Arend Koopmans / Mbed 2 deprecated androidservo

andorid via mbed servo bediening

Dependencies:   AndroidAccessory Motordriver mbed

Fork of uva_nc by Floris den Heijer

NetCentricApp.cpp@1:d131f93d1828, 2014-06-04 (annotated)

Committer:
aapje123
Date:
Wed Jun 04 12:02:36 2014 +0000
Revision:
1:d131f93d1828
Parent:
0:0f9ceecf1db8 
Android+servo

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"
aapje123 1:d131f93d1828 3 #include "motordriver.h"
aapje123 1:d131f93d1828 4
aapje123 1:d131f93d1828 5 Motor m(p21, p27, p28, 1); // pwm, fwd, rev, can brake
aapje123 1:d131f93d1828 6 Serial pc(USBTX, USBRX);
aapje123 1:d131f93d1828 7 AnalogIn ain(p16);
aapje123 1:d131f93d1828 8 DigitalOut led1(LED1);
aapje123 1:d131f93d1828 9
aapje123 1:d131f93d1828 10 // function of the servo system
aapje123 1:d131f93d1828 11 float f(int x){
aapje123 1:d131f93d1828 12 return -9.0319860139858321e-003
aapje123 1:d131f93d1828 13 + 3.0391558663558485e-002 * x
aapje123 1:d131f93d1828 14 + -7.2787622377621999e-003 * (x*x)
aapje123 1:d131f93d1828 15 + 1.4421476301476278e-003 * (x*x*x);
aapje123 1:d131f93d1828 16 }
aapje123 1:d131f93d1828 17
aapje123 1:d131f93d1828 18 // Check if the motor is in error state
aapje123 1:d131f93d1828 19 void error_check(){
aapje123 1:d131f93d1828 20 if (m.state() == -3){
aapje123 1:d131f93d1828 21 m.speed(0);
aapje123 1:d131f93d1828 22 m.stop(1);
aapje123 1:d131f93d1828 23 printf("ERROR");
aapje123 1:d131f93d1828 24 while(1){
aapje123 1:d131f93d1828 25 led1=1;
aapje123 1:d131f93d1828 26 wait(0.2);
aapje123 1:d131f93d1828 27 led1=0;
aapje123 1:d131f93d1828 28 wait(0.2);
aapje123 1:d131f93d1828 29 }
aapje123 1:d131f93d1828 30 }
aapje123 1:d131f93d1828 31 }
aapje123 1:d131f93d1828 32
aapje123 1:d131f93d1828 33 void goto_pos(int x){
aapje123 1:d131f93d1828 34 led1 = 1;
aapje123 1:d131f93d1828 35 float pos = ain.read();
aapje123 1:d131f93d1828 36 float pos_new = f(x);
aapje123 1:d131f93d1828 37 if (pos_new > 1.0){
aapje123 1:d131f93d1828 38 pos_new = 1;
aapje123 1:d131f93d1828 39 }
aapje123 1:d131f93d1828 40 else if (pos_new < 0.0){
aapje123 1:d131f93d1828 41 pos_new = 0;
aapje123 1:d131f93d1828 42 }
aapje123 1:d131f93d1828 43 printf("Go to position %i ", x);
aapje123 1:d131f93d1828 44 if (pos < pos_new){
aapje123 1:d131f93d1828 45 m.speed(1);
aapje123 1:d131f93d1828 46 }
aapje123 1:d131f93d1828 47 else if (pos > pos_new){
aapje123 1:d131f93d1828 48 m.speed(-1);
aapje123 1:d131f93d1828 49 }
aapje123 1:d131f93d1828 50 while( (pos < (pos_new - (pos_new*0.01))) || (pos > (pos_new + (pos_new*0.01))) ){
aapje123 1:d131f93d1828 51 error_check();
aapje123 1:d131f93d1828 52 pos = ain.read();
aapje123 1:d131f93d1828 53 }
aapje123 1:d131f93d1828 54 m.stop(1);
aapje123 1:d131f93d1828 55 m.speed(0);
aapje123 1:d131f93d1828 56 led1 = 0;
aapje123 1:d131f93d1828 57 }
diepvriezer 0:0f9ceecf1db8 58
diepvriezer 0:0f9ceecf1db8 59 // Process commands here.
diepvriezer 0:0f9ceecf1db8 60 MbedResponse *NetCentricApp::getResponse(MbedRequest *request) {
aapje123 1:d131f93d1828 61 MbedResponse *response = new MbedResponse();
aapje123 1:d131f93d1828 62 response->requestId = request->id;
aapje123 1:d131f93d1828 63 response->commandId = request->commandId;
aapje123 1:d131f93d1828 64 response->values = NULL;
aapje123 1:d131f93d1828 65 response->error = NO_ERROR;
aapje123 1:d131f93d1828 66 response->n = 0;
aapje123 1:d131f93d1828 67 switch(request->commandId) {
aapje123 1:d131f93d1828 68 case COMMAND_ZERO:
aapje123 1:d131f93d1828 69 goto_pos(0);
aapje123 1:d131f93d1828 70 break;
aapje123 1:d131f93d1828 71 case COMMAND_ONE:
aapje123 1:d131f93d1828 72 goto_pos(1);
aapje123 1:d131f93d1828 73 response->error = NO_ERROR;
aapje123 1:d131f93d1828 74 break;
aapje123 1:d131f93d1828 75 case COMMAND_TWO:
aapje123 1:d131f93d1828 76 goto_pos(2);
aapje123 1:d131f93d1828 77 break;
aapje123 1:d131f93d1828 78 case COMMAND_THREE:
aapje123 1:d131f93d1828 79 goto_pos(3);
aapje123 1:d131f93d1828 80 break;
aapje123 1:d131f93d1828 81 case COMMAND_FOUR:
aapje123 1:d131f93d1828 82 goto_pos(4);
aapje123 1:d131f93d1828 83 break;
aapje123 1:d131f93d1828 84 case COMMAND_FIVE:
aapje123 1:d131f93d1828 85 goto_pos(5);
aapje123 1:d131f93d1828 86 break;
aapje123 1:d131f93d1828 87 case COMMAND_SIX:
aapje123 1:d131f93d1828 88 goto_pos(6);
aapje123 1:d131f93d1828 89 break;
aapje123 1:d131f93d1828 90 case COMMAND_SEVEN:
aapje123 1:d131f93d1828 91 goto_pos(7);
aapje123 1:d131f93d1828 92 break;
aapje123 1:d131f93d1828 93 case COMMAND_EIGHT:
aapje123 1:d131f93d1828 94 goto_pos(8);
aapje123 1:d131f93d1828 95 break;
aapje123 1:d131f93d1828 96 case COMMAND_NINE:
aapje123 1:d131f93d1828 97 goto_pos(9);
aapje123 1:d131f93d1828 98 break;
aapje123 1:d131f93d1828 99 case COMMAND_TEN:
aapje123 1:d131f93d1828 100 goto_pos(10);
aapje123 1:d131f93d1828 101 break;
aapje123 1:d131f93d1828 102 case COMMAND_LEFT:
aapje123 1:d131f93d1828 103 led1 = 1;
aapje123 1:d131f93d1828 104 m.speed(-1);
aapje123 1:d131f93d1828 105 wait(1);
aapje123 1:d131f93d1828 106 m.stop(1);
aapje123 1:d131f93d1828 107 led1 = 0;
aapje123 1:d131f93d1828 108 break;
aapje123 1:d131f93d1828 109 case COMMAND_RIGHT:
aapje123 1:d131f93d1828 110 led1 = 1;
aapje123 1:d131f93d1828 111 m.speed(1);
aapje123 1:d131f93d1828 112 wait(1);
aapje123 1:d131f93d1828 113 m.stop(1);
aapje123 1:d131f93d1828 114 led1 = 0;
aapje123 1:d131f93d1828 115 break;
aapje123 1:d131f93d1828 116 case COMMAND_POSITION:
aapje123 1:d131f93d1828 117 response->n = 1;
aapje123 1:d131f93d1828 118 response->values = new float[1];
aapje123 1:d131f93d1828 119 response->values[0] = ain.read();
aapje123 1:d131f93d1828 120 break;
aapje123 1:d131f93d1828 121 default:
aapje123 1:d131f93d1828 122 MbedResponse *commandNotFound = new MbedResponse();
aapje123 1:d131f93d1828 123 commandNotFound->requestId = request->id;
aapje123 1:d131f93d1828 124 commandNotFound->commandId = request->commandId;
aapje123 1:d131f93d1828 125 commandNotFound->error = ERR_COMMAND_NOT_FOUND;
aapje123 1:d131f93d1828 126 commandNotFound->n = 0;
aapje123 1:d131f93d1828 127 commandNotFound->values = NULL;
aapje123 1:d131f93d1828 128 return commandNotFound;
diepvriezer 0:0f9ceecf1db8 129 }
aapje123 1:d131f93d1828 130 return response;
diepvriezer 0:0f9ceecf1db8 131 }
diepvriezer 0:0f9ceecf1db8 132
diepvriezer 0:0f9ceecf1db8 133 // Setup once a device is connected.
diepvriezer 0:0f9ceecf1db8 134 void NetCentricApp::setupDevice() {
diepvriezer 0:0f9ceecf1db8 135 printf("Connected to Android!\r\n");
diepvriezer 0:0f9ceecf1db8 136 }
diepvriezer 0:0f9ceecf1db8 137
diepvriezer 0:0f9ceecf1db8 138 // Called on disconnect.
diepvriezer 0:0f9ceecf1db8 139 void NetCentricApp::resetDevice() {
diepvriezer 0:0f9ceecf1db8 140 printf("Disconnected\r\n");
diepvriezer 0:0f9ceecf1db8 141 }
diepvriezer 0:0f9ceecf1db8 142
diepvriezer 0:0f9ceecf1db8 143
diepvriezer 0:0f9ceecf1db8 144 // Construction of requests.
diepvriezer 0:0f9ceecf1db8 145 int NetCentricApp::callbackRead(u8 *buffer, int len) {
diepvriezer 0:0f9ceecf1db8 146 if (len > 0) {
diepvriezer 0:0f9ceecf1db8 147 // Parse request, format:
diepvriezer 0:0f9ceecf1db8 148 // int - request ID
diepvriezer 0:0f9ceecf1db8 149 // int - command ID
diepvriezer 0:0f9ceecf1db8 150 // ubyte - # args
diepvriezer 0:0f9ceecf1db8 151 // float[] -- args
diepvriezer 0:0f9ceecf1db8 152
diepvriezer 0:0f9ceecf1db8 153 // Note len is fixed as the packet is always equally big. Don't try to use
diepvriezer 0:0f9ceecf1db8 154 // packets of variable size, the smallest size of a encountered packet is
diepvriezer 0:0f9ceecf1db8 155 // used.
diepvriezer 0:0f9ceecf1db8 156
diepvriezer 0:0f9ceecf1db8 157 MbedRequest *request = new MbedRequest();
diepvriezer 0:0f9ceecf1db8 158
diepvriezer 0:0f9ceecf1db8 159 request->id = getInt(buffer, 0, len);
diepvriezer 0:0f9ceecf1db8 160 request->commandId = getInt(buffer, 4, len);
diepvriezer 0:0f9ceecf1db8 161 request->n = getInt(buffer, 8, len);
diepvriezer 0:0f9ceecf1db8 162 request->args = NULL;
diepvriezer 0:0f9ceecf1db8 163
diepvriezer 0:0f9ceecf1db8 164 printf("request: %i, command: %i, n-args: %i\r\n", request->id, request->commandId, request->n);
diepvriezer 0:0f9ceecf1db8 165
diepvriezer 0:0f9ceecf1db8 166 int n = request->n;
diepvriezer 0:0f9ceecf1db8 167 if (n > 0) {
diepvriezer 0:0f9ceecf1db8 168 request->args = new float[n];
diepvriezer 0:0f9ceecf1db8 169 for (int i = 0; i < n; i++) {
diepvriezer 0:0f9ceecf1db8 170 int offset = 12 + (i * 4);
diepvriezer 0:0f9ceecf1db8 171 float f = getFloat(buffer, offset, len);
diepvriezer 0:0f9ceecf1db8 172 request->args[i] = f;
diepvriezer 0:0f9ceecf1db8 173 }
diepvriezer 0:0f9ceecf1db8 174 }
diepvriezer 0:0f9ceecf1db8 175
diepvriezer 0:0f9ceecf1db8 176 // Construct and send response.
diepvriezer 0:0f9ceecf1db8 177 MbedResponse *response = getResponse(request);
diepvriezer 0:0f9ceecf1db8 178 int responseSize = 4 + 4 + 4 + 4 + (response->n*4);
diepvriezer 0:0f9ceecf1db8 179 u8 responseBuffer[responseSize];
diepvriezer 0:0f9ceecf1db8 180
diepvriezer 0:0f9ceecf1db8 181 memcpy(responseBuffer + 0, reinterpret_cast<u8 const *>(&response->requestId), 4);
diepvriezer 0:0f9ceecf1db8 182 memcpy(responseBuffer + 4, reinterpret_cast<u8 const *>(&response->commandId), 4);
diepvriezer 0:0f9ceecf1db8 183 memcpy(responseBuffer + 8, reinterpret_cast<u8 const *>(&response->error), 4);
diepvriezer 0:0f9ceecf1db8 184 memcpy(responseBuffer + 12, reinterpret_cast<u8 const *>(&response->n), 4);
diepvriezer 0:0f9ceecf1db8 185 if (response->n > 0) {
diepvriezer 0:0f9ceecf1db8 186 for (int i = 0; i < response->n; i++) {
diepvriezer 0:0f9ceecf1db8 187 float f = response->values[i];
diepvriezer 0:0f9ceecf1db8 188 memcpy(responseBuffer + 16 + i*4, reinterpret_cast<u8 const *>(&f), 4);
diepvriezer 0:0f9ceecf1db8 189 }
diepvriezer 0:0f9ceecf1db8 190
diepvriezer 0:0f9ceecf1db8 191 }
diepvriezer 0:0f9ceecf1db8 192
diepvriezer 0:0f9ceecf1db8 193 write(responseBuffer, responseSize);
diepvriezer 0:0f9ceecf1db8 194
diepvriezer 0:0f9ceecf1db8 195 // Clean up.
diepvriezer 0:0f9ceecf1db8 196 if (request->n > 0) {
diepvriezer 0:0f9ceecf1db8 197 delete[] request->args;
diepvriezer 0:0f9ceecf1db8 198 }
diepvriezer 0:0f9ceecf1db8 199 delete request;
diepvriezer 0:0f9ceecf1db8 200
diepvriezer 0:0f9ceecf1db8 201 if (response->n > 0) {
diepvriezer 0:0f9ceecf1db8 202 delete[] response->values;
diepvriezer 0:0f9ceecf1db8 203 }
diepvriezer 0:0f9ceecf1db8 204 delete response;
diepvriezer 0:0f9ceecf1db8 205 }
diepvriezer 0:0f9ceecf1db8 206
diepvriezer 0:0f9ceecf1db8 207 return 0;
diepvriezer 0:0f9ceecf1db8 208 }
diepvriezer 0:0f9ceecf1db8 209
diepvriezer 0:0f9ceecf1db8 210 // Called to confirm a write operation.
diepvriezer 0:0f9ceecf1db8 211 int NetCentricApp::callbackWrite() {
diepvriezer 0:0f9ceecf1db8 212 return 0;
diepvriezer 0:0f9ceecf1db8 213 }
diepvriezer 0:0f9ceecf1db8 214
diepvriezer 0:0f9ceecf1db8 215
diepvriezer 0:0f9ceecf1db8 216 /* Unsigned byte to primitives. Little endian assumed, Java sends Big endian by default. */
diepvriezer 0:0f9ceecf1db8 217 float NetCentricApp::getFloat(u8 *buffer, int offset, int bufferLen) {
diepvriezer 0:0f9ceecf1db8 218 if (offset + 3 > bufferLen) {
diepvriezer 0:0f9ceecf1db8 219 printf("float index out of bounds!\r\n");
diepvriezer 0:0f9ceecf1db8 220 return 0.0;
diepvriezer 0:0f9ceecf1db8 221 }
diepvriezer 0:0f9ceecf1db8 222
diepvriezer 0:0f9ceecf1db8 223 float f;
diepvriezer 0:0f9ceecf1db8 224 memcpy(&f, buffer + offset, sizeof(f));
diepvriezer 0:0f9ceecf1db8 225 return f;
diepvriezer 0:0f9ceecf1db8 226 }
diepvriezer 0:0f9ceecf1db8 227
diepvriezer 0:0f9ceecf1db8 228 int NetCentricApp::getInt(u8 *buffer, int offset, int bufferLen) {
diepvriezer 0:0f9ceecf1db8 229 if (offset + 3 > bufferLen) {
diepvriezer 0:0f9ceecf1db8 230 printf("int index out of bounds!\r\n");
diepvriezer 0:0f9ceecf1db8 231 return 0;
diepvriezer 0:0f9ceecf1db8 232 }
diepvriezer 0:0f9ceecf1db8 233
diepvriezer 0:0f9ceecf1db8 234 int i;
diepvriezer 0:0f9ceecf1db8 235 memcpy(&i, buffer + offset, sizeof(i));
diepvriezer 0:0f9ceecf1db8 236 return i;
diepvriezer 0:0f9ceecf1db8 237 }
diepvriezer 0:0f9ceecf1db8 238
diepvriezer 0:0f9ceecf1db8 239 u8 NetCentricApp::getUByte(u8 *buffer, int offset, int bufferLen) {
diepvriezer 0:0f9ceecf1db8 240 if (offset > bufferLen) {
diepvriezer 0:0f9ceecf1db8 241 printf("byte index out of bounds!\r\n");
diepvriezer 0:0f9ceecf1db8 242 return 0;
diepvriezer 0:0f9ceecf1db8 243 }
diepvriezer 0:0f9ceecf1db8 244
diepvriezer 0:0f9ceecf1db8 245 u8 b;
diepvriezer 0:0f9ceecf1db8 246 memcpy(&b, buffer + offset, sizeof(b));
diepvriezer 0:0f9ceecf1db8 247 return b;
diepvriezer 0:0f9ceecf1db8 248 }