Diff: main.cpp

Revision:

18:1b0196cf779f

Parent:

17:740028fe5c99

--- a/main.cpp	Mon Dec 04 16:17:53 2017 +0000
+++ b/main.cpp	Thu Dec 07 20:59:47 2017 +0000
@@ -9,7 +9,7 @@
 MultiModalRobot robot(lw, rw);
 
 Serial blue(p13, p14);
-Serial py(USBTX, USBRX);
+Serial py(USBTX, USBRX);  
 char y;
 
 bool visible = false;
@@ -18,18 +18,18 @@
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
 
 void thread_read() { // Read from Serial Port Thread
-    myled1 = 0;
     char buffer[4];
     while (true) {
         if (!py.readable()) { // nothing read - serial empty
+            myled4 = 1;
             visible = false;
         } else { // found something
+            myled4 = 0;
             y = py.getc();
-            if (y != 'c') {
-                continue;
-            } else {
+            if (y == 'c') {
                 visible = true;
                 myled3 = !myled3;
                 //for (int count = 0; count < 4; count++) {
@@ -46,7 +46,7 @@
 //                    }
 //                }
             }
-            wait(.01);
+            wait(.05);
         }
     }
 }
@@ -58,7 +58,7 @@
     float leftSpeed = 0;
     float rightSpeed = 0;
     float DEFAULT_SPEED = 0.75;
-    float ROTATE_SPEED = 0.50;
+    float ROTATE_SPEED = 0.75;
     while(1) {
         wait(0.1);
         if(blue.readable() && blue.getc() == '!') {
@@ -74,13 +74,18 @@
                         t1.start(thread_read);
                         while (1) {
                             if (visible) { // CAMERA CAN SEE A CIRCLE
+                                myled1 = 1;
+                                myled2 = 0;
                                 leftSpeed = DEFAULT_SPEED;
                                 rightSpeed = DEFAULT_SPEED;
                                 robot.driveWheels(leftSpeed, rightSpeed);
                             } else { // CIRCLE NOT VISIBLE
+                                myled1 = 0;
+                                myled2 = 1;
                                 leftSpeed = ROTATE_SPEED;
                                 rightSpeed = -ROTATE_SPEED;
                                 robot.driveWheels(leftSpeed, rightSpeed);
+                                wait(0.1);
                             }
                         }
                         // stop thread