Diff: Controller.cpp

Revision:

12:f55b31b5bc4a

Parent:

10:7eaaa891ab81

--- 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);
     }
 
 }