Revision 12:f55b31b5bc4a, committed 2017-05-17

Comitter:

gimohd

Date:

Wed May 17 07:14:01 2017 +0000

Parent:

11:e9faca228d30

Commit message:

changed some things

Changed in this revision

--- a/Controller.cpp	Thu May 11 15:43:21 2017 +0200
+++ b/Controller.cpp	Wed May 17 07:14:01 2017 +0000
@@ -11,10 +11,10 @@
     //MAKES A NEW LIGHTCOMMUNICATION (starts receiving on serial)
     lightCommunication = new LightCommunication();
     car = new m3pi();
-
-    //receiveThread.start(callback(this,&Controller::runThread));
+    car->stop();
+    receiveThread.start(callback(this,&Controller::runThread));
     runningThread.start(callback(this, &Controller::run));
-    //receiveThread.join();
+    receiveThread.join();
     runningThread.join();
 
 
@@ -31,31 +31,41 @@
 void Controller::runThread() {
     while (true) {
         lightCommunication->receiveData();
-        printf("%s", "receiving");
     }
 }
 
 void Controller::run() {
 
     while (true) {
-
+    /*if (lightCommunication->needsToStop()){
+        car->stop();
+    }else {
+        current_left = lightCommunication->getLeft(current_left);
+        current_right = lightCommunication->getRight(current_right);
+        car->left_motor(current_left) ;
+        car->right_motor(current_right) ;
+    }*/
         float left = lightCommunication->getLeft(current_left);
         float right = lightCommunication->getRight(current_right);
-        car->cls();
-        car->locate(0, 1);
-        car->printf("Left: %0.3f Right: %0.3f", left, right);
-        printf("left = %f right = %f \r\n", current_left, current_right);
+        //car->cls();
+        //car->locate(0, 1);
+        //car->printf("Left: %0.3f Right: %0.3f", left, right);
         if (current_right == right && current_left == left) {
+             current_left = left;
+            current_right = right;
             ;
-        } else if (current_right == 0 && current_left == 0) {
+        } else if (right == 0 && left == 0) {
+             current_left = left;
+            current_right = right;
             car->stop();
         } else {
             current_left = left;
             current_right = right;
+            
+        printf("cur_left = %f cur_right = %f \r\n", current_left, current_right);
             car->left_motor(current_left);
             car->right_motor(current_right);
         }
-        wait_ms(20);
     }
 
 }

--- a/LightCommunication.cpp	Thu May 11 15:43:21 2017 +0200
+++ b/LightCommunication.cpp	Wed May 17 07:14:01 2017 +0000
@@ -8,13 +8,13 @@
 
 LightCommunication::LightCommunication()
 {
+    left= 0;
+    right = 0;
     ir_rx = new ReceiverIR(p21);
     device = new Serial(p13,p14);
     mut = new Mutex();
-    left= 0;
-    right = 0;
-    device->baud(19200);
-    device->attach(callback(this,&LightCommunication::receiveData), Serial::RxIrq);
+    device->baud(9600);
+    //device->attach(callback(this,&LightCommunication::receiveData), Serial::RxIrq);
 }
 
 LightCommunication::~LightCommunication()
@@ -34,53 +34,72 @@
 
 void LightCommunication::receiveData()
 {
+    uint8_t data [4];
     //SERIAL RECEIVE
-    if(device->readable()) {
-        uint8_t data [128];
-        device->scanf("%s",data);
-        printf("Received data: client:%d left: %d right:%d \r\n",data[0],data[1],data[2]);
+    //if(device->scanf("%c%c%c",data[0],data[1],data[2])>0) {
+        if (device->readable()){
+            data[0] = device->getc();
+            
+            
+        printf("DATA");
+        printf("Received data: client:%d \r\n",data[0]);
         if (OWN_ID == (int) data[0]) {
+            data[1] = device->getc();
+            data[2] = device->getc();
+            data[3] = device->getc();
+            printf("%X%X%X%X",data[0],data[1],data[2],data[3]);
+            if(data[3] ==  (((data[1] + data[2] + 127) % 251) & 0xFF)){
             mut->lock();
             left = LightCommunication::toFloat(data[1]);
             right = LightCommunication::toFloat(data[2]);
 
             printf("DATA left = %f right = %f \r\n",left,right);
             mut->unlock();
+            }
         }
     }
 }
 
 float LightCommunication::getRight(float cur_right)
 {
-
+    float r = right;
+    mut->unlock();
+    return r;
+/*
     float r = right - cur_right;
 
     float cur_speed = 0;
-    if (r>0) {
+    if (r>0.0f) {
         cur_speed = (float)(cur_right + (0.005*SPEED));
-    } else if (r<0) {
+    } else if (r<0.0f) {
 
         cur_speed = (float)(cur_right - (0.005*SPEED));
     } else {
-        cur_speed = right;
+        cur_speed = cur_right;
     }
     mut->unlock();
     return cur_speed;
+    */
 }
 
 float LightCommunication::getLeft(float cur_left)
 {
-
+    mut->lock();
+    float l = left;
+    
+    return l;
+/*
     mut->lock();
     float l = left - cur_left;
-    if (l>0) {
+    if (l>0.0f) {
+    printf("lefdif: %f\r\n",l);
         return (float)(cur_left + (0.005*SPEED));
-    } else if (l<0) {
+    } else if (l<0.0f) {
         return (float)(cur_left - (0.005*SPEED));
     } else {
         return cur_left;
-    }
-    //return left;
+    }*/
+    
 }
 
 float LightCommunication::toFloat(int8_t a)