Diff: main.cpp

Revision:

20:823f05f5ffd5

Parent:

18:6e8eae661cf3

Child:

21:bef848ac1894

--- a/main.cpp	Mon Mar 23 01:01:52 2015 +0000
+++ b/main.cpp	Tue Mar 24 15:55:31 2015 +0000
@@ -35,10 +35,10 @@
 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 l_turn_f = 0;
+int r_turn_f = 0;
+int l_180_f = 0;
+int r_180_f = 0;
 int forwards = 0;
 int backwards = 0;
 int spin = 0;
@@ -47,6 +47,7 @@
 
 void set_direction( int direction, float duty_l, float duty_r)
 {
+    blue.printf("Change in direction; %f, %f \n", duty_l, duty_r);
     switch( direction ) {
         case 0x11: { //forward
             motor_r.write( duty_r);
@@ -95,67 +96,112 @@
         }
     }
 }
+int change =0;
 
-void motor_result() //val_r and val_l are global
+void r_normal()
 {
-    //This needed to be completely changed. We need sequential actions, a case statement is too basic (unforetuneately)
-    led= 0;
-    switch(val_r) {
+    //  blue.printf("0110\n");
+    if (val_l == 0) {
+        //check flags
+        if( l_180_f  ) {
+            set_direction(0x11, 0.2, 0.6); //Needs to test
+            spin =1; //new state is spin
+            forwards = 0;
+            //start_timer?
+
+        } else if (l_turn_f) { //
+            set_direction(0x01, 0.3,0.3); //CHANGE DUTIES AFTER TESTING
+            spin =1 ; //reached the end, nothign else matters
+            forwards =0;
+        }
+    } else if(val_l == 7) {
+        l_turn_f =1;
+        if(!forwards) {
+            forwards =1;
+            set_direction(0x11, 0.3,0.3);
+        } else if(!forwards) {
+            forwards =1;
+
+            set_direction(0x11, 0.3,0.3);
+        }
+    }
+
+
+}
+void motor_result()  //val_r and val_l are global
+{
+    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.
+            r_normal();
             break;
-        case 0: //lost or turn
-            //  blue.printf("0000\n");
-            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);
+        case 7: // 0111
+            r_turn_f =1;                        //Either misaligned or getting ready for a turn
+            if (val_l == 14) { //1110 0111
+                l_turn_f =1;
+            } else if(val_l ==0) { //0000 0111
+                if (l_turn_f) { //special case ****************************** WON'T HAPPEN**********************
+                    blue.printf("Double Turn?");
+                    set_direction(0x01, 0.3,0.3);
+                    forwards =0;
+                    spin =1;
+                } else if(l_180_f) {
+                    blue.printf("You've got a junction\n");
+                } else {
+                    l_180_f =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;
 
 
-            //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE MISALIGNED*********************************
-        case 1:
-            //  blue.printf("0001\n");
-            set_direction(0x11, 0.3,0.1);
+        case 0: //lost or turn
+            //  blue.printf("0000\n");
+            if(val_l == 14) {
+                //This probably won't happen, but if it does , 180
+                r_180_f =1;
+                l_turn_f =1;
+            } else if(val_l == 0) {
+                switch (junction_cnt) {
+                    default:
+                        set_direction(0x11, 0,0);
+                        forward = 0;
+                }
+            } else if(r_turn_f) {
+                set_direction(0x10, 0.3 0.3);
+                forward = 0;
+                spin = 1;
+            } else if {r_180_f) {
+                set_direction(0x11, 0.6, 0.2);
+                forward = 0;
+                spin =1;
+            }
+            else {
+                r_180_f =1;
+            }
             break;
+
+
         case 2:
             // blue.printf("0010\n");
-            set_direction(0x11, 0.3,0.2);
+            r_normal();
             break;
         case 4:
             // blue.printf("0100\n");
-            set_direction(0x11, 0.2,0.3);
+            r_normal();
             break;
+
+            //***************************IF THERE IS A SINGLE BIT ONLY; WE ARE very MISALIGNED*********************************
+        case 1:
+            //  blue.printf("0001\n");
+            set_direction(0x11, 0,0);
+            forward=0;
+            break;
+
         case 8:
             //blue.printf("1000\n");
-            set_direction(0x11, 0.1,0.3);
+            set_direction(0x11, 0,0);
+            forward = 0;
             break;
             //******************************END OF SINGLE BIT ALIGNMENT********************************************
 
@@ -163,99 +209,184 @@
         case 12:
             //   blue.printf("1100\n");
             set_direction(0x11, 0.2,0.3);
+            forward = 0;
             break;
         case 3:
             // blue.printf("0011\n");
             set_direction(0x11, 0.3,0.2);
+            forward=0;
             break;
             //********************************END OF DOUBLE BIT ALIGNMENT********************************************
 
-
+        case 15:
+            set_direction(0x11, 0,0);
+            forward =0;
+            break;
 
         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
+/*
+       //This needed to be completely changed. We need sequential actions, a case statement is too basic (unforetuneately)
+       led= 0;
+       switch(val_r) {
+           case 4: //Normal mode
+               if (!forwards) {
+                   blue.printf("i've got %i on the right\n", val_r);
+                   set_direction(0x11, 0.3,0.3);
+                   forwards =1;
+                   change =1;
+                   break;
+               }
+           case 12:
+               blue.printf("i've got %i on the right\n", val_r);
 
-            } 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
+               set_direction(0x11, 0.3, 0.2);
+               forwards =0;
+               break;
+               change =1;
 
-            } 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);
+           case 8:
+               blue.printf("i've got %i on the right\n", val_r);
+
+               set_direction(0x11, 0.2, 0.3);
+               forwards =0;
+               change =1;
 
-            } else {                                //probably lost
-                set_direction(0x00, 0,0);
-            }
-            break;
+               break;
+           default:
+               if (!forwards) {
+                   blue.printf("i've got %i on the right\n", val_r);
+
+                   set_direction(0x11, 0.3,0.3);
+                   forwards =1;
+                   break;
+               }
+
+       }
+       if(!change) {
+           switch(val_l) {
+               case 2:
+                   if (!forwards) {
+                       blue.printf("i've got %i on the left\n", val_l);
 
-            //***************************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.3,0.1);
-            break;
-            //******************************END OF SINGLE BIT ALIGNMENT********************************************
+                       set_direction(0x11, 0.3,0.3);
+                       forwards =1;
+                       break;
+                   }
+               case 3:
+                   blue.printf("i've got %i on the left\n", val_l);
 
-            //*************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********************************************
+                   set_direction(0x11, 0.3, 0.2);
+                   forwards =0;
+                   break;
+               case 1:
+                   blue.printf("i've got %i on the left\n", val_l);
+
+                   set_direction(0x11, 0.3, 0.2);
+                   forwards =0;
+                   break;
 
 
 
-        default:
-            //blue.printf("%i\n", val);
-            set_direction(0x00, 0,0);
-    }
+               default:
+                   blue.printf("i've got %i on the left\n", val_l);
+                   if (!forwards) {
+                       // blue.printf("i've got %i on the left\n", val_l);
+
+                       set_direction(0x11, 0.3,0.3);
+                       forwards =1;
+                       break;
+                   }
+           }
+       }*/
+//********************************************************START******************************************************
+
+
+//***************************************LEFT SHOULD BE PRETTY MUCH MIRRORED***********************************************************
+/*   switch(val_l) {
+       case 7: //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_f = 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_f & l_turn_f) {        //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_f) {               //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_f = 1;                     //raise 180 flag in prep for next time
 
-}
+           } else if(l_180_f) {
+               //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.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);
+   }
+*/
 void sensor_read()
 {
 
     int x = 0;
     int sens_r[4] = {0};
     int sens_l[4] = {0};
-
+    val_r =0;
+    val_l=0;
     for (x = 0; x < 8; x++) {
         switch(x) {
                 //0 is the robot's rightest sensor and 7 is the robot's leftmost sensor
@@ -302,11 +433,14 @@
             val_r = val_r + sens_r[x];
         }
     }
-    //blue.printf("%i\n",val);
+    //blue.printf("ValL %i \t      ValR %i\n",val_l , val_r);
 
 }
 int main()
 {
+    wait(0.5);
+    led = !led;
+    blue.printf("Starting Code!!!!!!\n");
     level = 0.58; //Analogout Level for black line into comparator
 //Set PWM frequency to 1000Hz
     motor_l.period( 1.0f / (float) PWM_FREQ);
@@ -321,6 +455,7 @@
     //int old_val=0;*********
 
     while(1) { //PRINTS A STRING 0110    0110 FOR LEFT AND RIGHT RESPECTIVELY
+        led=!led;
         old_val_r = val_r;
         old_val_l = val_l;
         sensor_read();
@@ -329,17 +464,16 @@
             string binary_l = "";
             int mask = 1;
             for(int i = 0; i < 4; i++) {
-                if(!(mask&val_r))             binary_r = "1"+binary_r;  //(mask&val) >= 1
+                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
+                if((mask&val_l))             binary_l = "1"+binary_l;  //(mask&val) >= 1
                 else                        binary_l = "0"+binary_l;
                 mask = mask << 1;
             }
-            blue.printf("%s\t%s\n", binary_l, binary_r);
-            wait(0.2);
+            blue.printf("L  %s\t      R  %s\n", binary_l, binary_r);
             motor_result();
         }
-
+        wait(0.2);
         // if(!(old_val == val)) {
         //     motor_result(val);
         // }