andorid via mbed servo bediening
Dependencies: AndroidAccessory Motordriver mbed
Fork of uva_nc by
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?
User | Revision | Line number | New 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 | } |