andorid via mbed servo bediening
Dependencies: AndroidAccessory Motordriver mbed
Fork of uva_nc by
Diff: NetCentricApp.cpp
- Revision:
- 1:d131f93d1828
- Parent:
- 0:0f9ceecf1db8
diff -r 0f9ceecf1db8 -r d131f93d1828 NetCentricApp.cpp --- a/NetCentricApp.cpp Thu May 22 08:33:12 2014 +0000 +++ b/NetCentricApp.cpp Wed Jun 04 12:02:36 2014 +0000 @@ -1,88 +1,133 @@ #include "NetCentricApp.h" #include "MbedCommand.h" +#include "motordriver.h" + +Motor m(p21, p27, p28, 1); // pwm, fwd, rev, can brake +Serial pc(USBTX, USBRX); +AnalogIn ain(p16); +DigitalOut led1(LED1); + +// function of the servo system +float f(int x){ + return -9.0319860139858321e-003 + + 3.0391558663558485e-002 * x + + -7.2787622377621999e-003 * (x*x) + + 1.4421476301476278e-003 * (x*x*x); +} + +// Check if the motor is in error state +void error_check(){ + if (m.state() == -3){ + m.speed(0); + m.stop(1); + printf("ERROR"); + while(1){ + led1=1; + wait(0.2); + led1=0; + wait(0.2); + } + } +} + +void goto_pos(int x){ + led1 = 1; + float pos = ain.read(); + float pos_new = f(x); + if (pos_new > 1.0){ + pos_new = 1; + } + else if (pos_new < 0.0){ + pos_new = 0; + } + printf("Go to position %i ", x); + if (pos < pos_new){ + m.speed(1); + } + else if (pos > pos_new){ + m.speed(-1); + } + while( (pos < (pos_new - (pos_new*0.01))) || (pos > (pos_new + (pos_new*0.01))) ){ + error_check(); + pos = ain.read(); + } + m.stop(1); + m.speed(0); + led1 = 0; +} // 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); - } - - 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 *response = new MbedResponse(); + response->requestId = request->id; + response->commandId = request->commandId; + response->values = NULL; + response->error = NO_ERROR; + response->n = 0; + switch(request->commandId) { + case COMMAND_ZERO: + goto_pos(0); + break; + case COMMAND_ONE: + goto_pos(1); + response->error = NO_ERROR; + break; + case COMMAND_TWO: + goto_pos(2); + break; + case COMMAND_THREE: + goto_pos(3); + break; + case COMMAND_FOUR: + goto_pos(4); + break; + case COMMAND_FIVE: + goto_pos(5); + break; + case COMMAND_SIX: + goto_pos(6); + break; + case COMMAND_SEVEN: + goto_pos(7); + break; + case COMMAND_EIGHT: + goto_pos(8); + break; + case COMMAND_NINE: + goto_pos(9); + break; + case COMMAND_TEN: + goto_pos(10); + break; + case COMMAND_LEFT: + led1 = 1; + m.speed(-1); + wait(1); + m.stop(1); + led1 = 0; + break; + case COMMAND_RIGHT: + led1 = 1; + m.speed(1); + wait(1); + m.stop(1); + led1 = 0; + break; + case COMMAND_POSITION: + response->n = 1; + response->values = new float[1]; + response->values[0] = ain.read(); + break; + default: + 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; } - - 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; + return response; } // Setup once a device is connected.