Diff: NetCentricApp.cpp

Revision:

1:d131f93d1828

Parent:

0:0f9ceecf1db8

--- 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.