Diff: LightCommunication.cpp

Revision:

10:7eaaa891ab81

Parent:

9:8c5229dfab82

Child:

11:e9faca228d30

diff -r 8c5229dfab82 -r 7eaaa891ab81 LightCommunication.cpp
--- a/LightCommunication.cpp	Wed May 10 10:01:29 2017 +0200
+++ b/LightCommunication.cpp	Thu May 11 15:27:12 2017 +0200
@@ -4,77 +4,95 @@
 
 #include "LightCommunication.h"
 
-LightCommunication::LightCommunication()  {
+
+
+LightCommunication::LightCommunication()
+{
     ir_rx = new ReceiverIR(p21);
-    device = new Serial(p14,p15);
+    device = new Serial(p13,p14);
     mut = new Mutex();
-    left,right = 0;
+    left= 0;
+    right = 0;
+    device->baud(19200);
+    device->attach(callback(this,&LightCommunication::receiveData), Serial::RxIrq);
 }
 
-LightCommunication::~LightCommunication() {
-
+LightCommunication::~LightCommunication()
+{
+    delete mut;
+    delete device;
+    delete ir_rx;
 }
 
-void LightCommunication::getSerial(char * test) {
-    if(device->readable()) {
-       device->scanf("%s",test);
-    }
-}
-
-void LightCommunication::getIRStyle(uint8_t * buf) {
+void LightCommunication::getIRStyle(uint8_t * buf)
+{
     RemoteIR::Format format;
     if (ir_rx->getState() == ReceiverIR::Received) {
         ir_rx->getData(&format, buf, sizeof(buf) * 8);
     }
 }
 
-void LightCommunication::receiveData() {
-    mut->lock();
-    //uint8_t *  data = LightCommunication::getIRStyle();
-    char  data[3];
-    LightCommunication::getSerial(data);
-
-    printf("%X \n",data[2]);
-    printf("%d \n",data[2]);
-    if (OWN_ID == (int) data[0]) {
-        left = LightCommunication::toFloat(data[1]);
-        right = LightCommunication::toFloat(data[2]);
-    }
+void LightCommunication::receiveData()
+{
+    //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 (OWN_ID == (int) data[0]) {
+            mut->lock();
+            left = LightCommunication::toFloat(data[1]);
+            right = LightCommunication::toFloat(data[2]);
 
-    mut->unlock();
-}
-
-float LightCommunication::getRight(float cur_right){
-    float r = right - cur_right;
-    if (r>0){
-        return (float)(cur_right + 0.005);
-    }else if (r<0){
-
-        return (float)(cur_right -0.005);
-    }else{
-        return cur_right;
+            printf("DATA left = %f right = %f \r\n",left,right);
+            mut->unlock();
+        }
     }
 }
 
-float LightCommunication::getLeft(float cur_left){
+float LightCommunication::getRight(float cur_right)
+{
+
+    float r = right;
+    mut->unlock();
+    /*
+    if (r>0) {
+        return (float)(cur_right + (0.005*SPEED));
+    } else if (r<0) {
+
+        return (float)(cur_right - (0.005*SPEED));
+    } else {
+        return cur_right;
+    }*/
+    return r;
+}
+
+float LightCommunication::getLeft(float cur_left)
+{
 
     mut->lock();
-    float l = left - cur_left;
-    if (l>0){
-        return (float)(cur_left + 0.005);
-    }else if (l<0){
-        return (float)(cur_left -0.005);
-    }else{
+    /*float l = left - cur_left;
+    if (l>0) {
+        return (float)(cur_left + (0.005*SPEED));
+    } else if (l<0) {
+        return (float)(cur_left - (0.005*SPEED));
+    } else {
         return cur_left;
-    }
+    }*/
+    return left;
 }
 
-float LightCommunication::toFloat(int8_t a) {
-    return ((float)a)/100;
+float LightCommunication::toFloat(int8_t a)
+{
+    return (((float)a)/100.0)*SPEED;
 }
 
-bool LightCommunication::needsToStop() {
-    return right == 0 && left == 0;
+bool LightCommunication::needsToStop()
+{
+    mut->lock();
+    bool stop = right == 0 && left == 0;
+    mut->unlock();
+    return stop;
 }