Revision 17:740028fe5c99, committed 2017-12-04

Comitter:

kshah521

Date:

Mon Dec 04 16:17:53 2017 +0000

Parent:

16:47d3b1f2e90d

Commit message:

State machine code.

Changed in this revision

--- a/main.cpp	Mon Nov 20 04:01:33 2017 +0000
+++ b/main.cpp	Mon Dec 04 16:17:53 2017 +0000
@@ -1,17 +1,56 @@
-//This will rotate the wheels so that after 5 seconds the robot counterclockwise and then clockwise 
+//This will rotate the wheels so that after 5 seconds the robot counterclockwise and then clockwise
 #include "mbed.h"
 #include "rtos.h"
 #include "motordriver.h"
 #include "MultiModalRobot.h"
- 
 
- 
 Motor lw(p26, p29, p30, 1); // pwm, fwd, rev   LEFT WHEEL
 Motor rw(p25, p28, p27, 1); // pwm, fwd, rev   RIGHT WHEEL
 MultiModalRobot robot(lw, rw);
- 
-Serial blue(p13, p14); 
- 
+
+Serial blue(p13, p14);
+Serial py(USBTX, USBRX);
+char y;
+
+bool visible = false;
+Thread t1;
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+
+void thread_read() { // Read from Serial Port Thread
+    myled1 = 0;
+    char buffer[4];
+    while (true) {
+        if (!py.readable()) { // nothing read - serial empty
+            visible = false;
+        } else { // found something
+            y = py.getc();
+            if (y != 'c') {
+                continue;
+            } else {
+                visible = true;
+                myled3 = !myled3;
+                //for (int count = 0; count < 4; count++) {
+//                    y = py.getc();
+//                    buffer[count%4] = y;
+//                    count++;
+//                    if (count % 3 == 0) {
+//                        myled1 = !myled1;
+//                        int i;
+//                        memcpy(&i, buffer, 4);
+//                        if (i == 84) {
+//                            myled2 = 1;
+//                        }
+//                    }
+//                }
+            }
+            wait(.01);
+        }
+    }
+}
+
 int main() {
     char bnum = 0;
     char bhit = 0;
@@ -19,33 +58,48 @@
     float leftSpeed = 0;
     float rightSpeed = 0;
     float DEFAULT_SPEED = 0.75;
-    while(1){
+    float ROTATE_SPEED = 0.50;
+    while(1) {
         wait(0.1);
-        if(blue.readable() && blue.getc() == '!'){
-            if(blue.readable() && blue.getc() == 'B'){
+        if(blue.readable() && blue.getc() == '!') {
+            if(blue.readable() && blue.getc() == 'B') {
                 bnum = blue.getc();
                 bhit = blue.getc();
                 blue.getc();
-                switch(bnum){
+                switch(bnum) {
                     case '1':
                         needToStopRobot = 1;
                         break;
+                    case '2': // become follower
+                        t1.start(thread_read);
+                        while (1) {
+                            if (visible) { // CAMERA CAN SEE A CIRCLE
+                                leftSpeed = DEFAULT_SPEED;
+                                rightSpeed = DEFAULT_SPEED;
+                                robot.driveWheels(leftSpeed, rightSpeed);
+                            } else { // CIRCLE NOT VISIBLE
+                                leftSpeed = ROTATE_SPEED;
+                                rightSpeed = -ROTATE_SPEED;
+                                robot.driveWheels(leftSpeed, rightSpeed);
+                            }
+                        }
+                        // stop thread
                     case '5': //up
                         if(bhit=='1'){
                             leftSpeed = rightSpeed = DEFAULT_SPEED;
                         } else {
                             needToStopRobot = 1;
                         }
-                        break;  
+                        break;
                     case '6': //down
-                        if(bhit=='1'){
+                        if(bhit=='1') {
                             leftSpeed = rightSpeed = -DEFAULT_SPEED;
                         } else {
                             needToStopRobot = 1;
                         }
                         break;
                     case '7': //left
-                        if(bhit=='1'){
+                        if(bhit=='1') {
                             leftSpeed = -DEFAULT_SPEED;
                             rightSpeed = DEFAULT_SPEED;
                         } else {
@@ -53,7 +107,7 @@
                         }
                         break;
                     case '8': //right
-                        if(bhit=='1'){
+                        if(bhit=='1') {
                             leftSpeed = DEFAULT_SPEED;
                             rightSpeed = -DEFAULT_SPEED;
                         } else {
@@ -64,15 +118,16 @@
                         robot.stop(0.5);
                         break;
                 }
-            }           
+            }
         }
         if(needToStopRobot){
             robot.stop(0.5);
             needToStopRobot = 0;
             leftSpeed = 0;
             rightSpeed = 0;
+            robot.driveWheels(leftSpeed, rightSpeed);
         } else {
-            robot.driveWheels(leftSpeed, rightSpeed);   
+            robot.driveWheels(leftSpeed, rightSpeed);
         }
     }
-}
\ No newline at end of file
+}