Diff: main.cpp

Revision:

18:6e8eae661cf3

Parent:

17:54da4359134f

Child:

20:823f05f5ffd5

--- a/main.cpp	Fri Mar 20 16:59:28 2015 +0000
+++ b/main.cpp	Mon Mar 23 01:01:52 2015 +0000
@@ -28,7 +28,22 @@
 //LED to test
 DigitalOut led(LED_RED);
 
+//integer value of sensors (has to be global)
+int val_r=0;
+int val_l=0;
+int old_val_r=0;
+int old_val_l=0;
 
+//Flags for decisions
+int l_turn_spot = 0;
+int r_turn_spot = 0;
+int l_180_spot = 0;
+int r_180_spot = 0;
+int forwards = 0;
+int backwards = 0;
+int spin = 0;
+//Junction Counter (hard coding deccsions on junctions)
+int junction_cnt = 0;
 
 void set_direction( int direction, float duty_l, float duty_r)
 {
@@ -81,15 +96,51 @@
     }
 }
 
-void motor_result(int val)
+void motor_result() //val_r and val_l are global
 {
-
+    //This needed to be completely changed. We need sequential actions, a case statement is too basic (unforetuneately)
     led= 0;
-    switch(val) {
-        case 0: //lost
+    switch(val_r) {
+        case 6: //Normal Mode
+            //  blue.printf("0110\n");
+            if (!forwards){ //if you're not already going forwards
+                set_direction(0x11, 0.3,0.3);
+                forwards = 1;
+                }
+            break;
+        case 7: // 0111                             Either misaligned or getting ready for a turn
+            r_turn_spot = 1;
+            //No need to change direction, if misaligned, it will soon go to a double line alignment protocol
+            //if the next state is 0000; then we were right and there is a right turn.
+            break;
+        case 0: //lost or turn
             //  blue.printf("0000\n");
-            set_direction(0x00, 0,0);
+            if ((r_turn_spot & l_turn_spot) & !spin) {        //if we're expecting a junction and we're not spinning
+                set_direction(0x10, 0.3,0.3);       //spin right because there's only one junction
+                spin = 1;
+                //no stop conditions yet
+
+            } else if (r_turn_spot & !spin) {               //if we're expecting a right turn
+                set_direction(0x10, 0.3,0.3);       //spin right
+                spin = 1;
+                //no stop conditions yet
+
+            } else if(val_l == 6) {                 //0110 0000 could mean a right 180 turn
+                //keep going forward,               no need to change dir
+                r_180_spot = 1;                     //raise 180 flag in prep for next time
+
+            } else if(r_180_spot) {
+                //code for 180???
+                blue.printf("I want to 180 Right, but i don't know how yet");
+                set_direction(0x00, 0,0);
+
+            } else {                                //probably lost
+                set_direction(0x00, 0,0);
+            }
             break;
+
+
+            //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE MISALIGNED*********************************
         case 1:
             //  blue.printf("0001\n");
             set_direction(0x11, 0.3,0.1);
@@ -102,10 +153,13 @@
             // blue.printf("0100\n");
             set_direction(0x11, 0.2,0.3);
             break;
-        case 6:
-            //  blue.printf("0110\n");
-            set_direction(0x11, 0.3,0.3);
+        case 8:
+            //blue.printf("1000\n");
+            set_direction(0x11, 0.1,0.3);
             break;
+            //******************************END OF SINGLE BIT ALIGNMENT********************************************
+
+            //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
         case 12:
             //   blue.printf("1100\n");
             set_direction(0x11, 0.2,0.3);
@@ -114,24 +168,98 @@
             // blue.printf("0011\n");
             set_direction(0x11, 0.3,0.2);
             break;
+            //********************************END OF DOUBLE BIT ALIGNMENT********************************************
+
+
+
+        default:
+            //blue.printf("%i\n", val);
+            set_direction(0x00, 0,0);
+    }
+//***************************************LEFT SHOULD BE PRETTY MUCH MIRRORED***********************************************************
+    switch(val_l) {
+        case 6: //Normal Mode
+            //  blue.printf("0110\n");
+            set_direction(0x11, 0.3,0.3);
+            break;
+        case 7: // 0111                             Either misaligned or getting ready for a turn
+            l_turn_spot = 1;
+            //No need to change direction, if misaligned, it will soon go to a double line alignment protocol
+            //if the next state is 0000; then we were right and there is a left turn.
+            break;
+        case 0: //lost or turn
+            //  blue.printf("0000\n");
+            if (r_turn_spot & l_turn_spot) {        //if we're expecting a junction
+                set_direction(0x10, 0.3,0.3);       //spin right because there's only one junction
+                //no stop conditions yet
+
+            } else if (l_turn_spot) {               //if we're expecting a left turn
+                set_direction(0x01, 0.3,0.3);       //spin left
+                //no stop conditions yet
+
+            } else if(val_r == 6) {                 //0000 0110 could mean a left 180 turn
+                //keep going forward,               no need to change dir
+                l_180_spot = 1;                     //raise 180 flag in prep for next time
+
+            } else if(l_180_spot) {
+                //code for 180???
+                blue.printf("I want to 180 left, but i don't know how yet");
+                set_direction(0x00, 0,0);
+
+            } else {                                //probably lost
+                set_direction(0x00, 0,0);
+            }
+            break;
+
+            //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE MISALIGNED*********************************
+        case 1:
+            //  blue.printf("0001\n");
+            set_direction(0x11, 0.1,0.3);
+            break;
+        case 2:
+            // blue.printf("0010\n");
+            set_direction(0x11, 0.2,0.3);
+            break;
+        case 4:
+            // blue.printf("0100\n");
+            set_direction(0x11, 0.3,0.2);
+            break;
         case 8:
             //blue.printf("1000\n");
-            set_direction(0x11, 0.1,0.3);
+            set_direction(0x11, 0.3,0.1);
+            break;
+            //******************************END OF SINGLE BIT ALIGNMENT********************************************
+
+            //*************IF TWO BITS ARE ON, AND NOT IN THE MIDDLE, WE'RE PROBABLY VERY MISALIGNED*******************
+        case 12:
+            //   blue.printf("1100\n");
+            set_direction(0x11, 0.3,0.2);
             break;
+        case 3:
+            // blue.printf("0011\n");
+            set_direction(0x11, 0.2,0.3);
+            break;
+            //********************************END OF DOUBLE BIT ALIGNMENT********************************************
+
+
+
         default:
             //blue.printf("%i\n", val);
             set_direction(0x00, 0,0);
     }
 
-
 }
-int sensor_read()
+void sensor_read()
 {
-    int val=0;
+
     int x = 0;
-    int sens[4] = {0};
-    for (x = 0; x < 4; x++) {
+    int sens_r[4] = {0};
+    int sens_l[4] = {0};
+
+    for (x = 0; x < 8; x++) {
         switch(x) {
+                //0 is the robot's rightest sensor and 7 is the robot's leftmost sensor
+                //Addresses are inputs to multiplexer, can ignore them mainly
             case 0:
                 sensor = 0x3;
                 break;
@@ -158,19 +286,24 @@
                 break;
         }
 
-        sens[x] = input;
+        if(x<4)sens_r[x] = input;
+        else sens_l[x%4] = input;//array of sensor output values for left and right
         //blue = !blue;
 
 
 
     }
-    for(x = 3; x >= 0; x--) {
-        val = val << 1;
-        val = val + sens[x];
+    for(x = 7; x >= 0; x--) {
+        if(x>3) {
+            val_l = val_l << 1;
+            val_l = val_l + sens_l[x%4];
+        } else {
+            val_r = val_r << 1;
+            val_r = val_r + sens_r[x];
+        }
     }
     //blue.printf("%i\n",val);
 
-    return val;
 }
 int main()
 {
@@ -183,22 +316,28 @@
     motor_rb=0;
     motor_lf=0;
     motor_lb=0;
-    int val =0;
-    int old_val=0;
-    while(1) {
-        old_val = val;
-        val = sensor_read();
-        if(!(old_val == val)) {
-            string binary = "";
+    //Now Global*****
+    //int val =0;
+    //int old_val=0;*********
+
+    while(1) { //PRINTS A STRING 0110    0110 FOR LEFT AND RIGHT RESPECTIVELY
+        old_val_r = val_r;
+        old_val_l = val_l;
+        sensor_read();
+        if((!(old_val_l == val_l)) | (!(old_val_r == val_r))) {
+            string binary_r = "";
+            string binary_l = "";
             int mask = 1;
             for(int i = 0; i < 4; i++) {
-                if((mask&val) >= 1)         binary = "1"+binary;
-                else                        binary = "0"+binary;
+                if(!(mask&val_r))             binary_r = "1"+binary_r;  //(mask&val) >= 1
+                else                        binary_r = "0"+binary_r;
+                if(!(mask&val_l))             binary_l = "1"+binary_l;  //(mask&val) >= 1
+                else                        binary_l = "0"+binary_l;
                 mask = mask << 1;
             }
-            blue.printf("%s\n", binary);
+            blue.printf("%s\t%s\n", binary_l, binary_r);
             wait(0.2);
-            motor_result(val);
+            motor_result();
         }
 
         // if(!(old_val == val)) {