Diff: main.cpp

Revision:

11:af3d70d153bb

Parent:

10:86cb40bf1c17

--- a/main.cpp	Mon Jun 02 08:44:08 2014 +0000
+++ b/main.cpp	Mon Jun 02 09:47:01 2014 +0000
@@ -126,73 +126,81 @@
     }    
 }
 
-void smoothFollow(float position, float speed)//, int direction)
+void turnAround(float speed)
+{
+    float pos = myLinePos();
+    huey.right(speed/2);
+    while(pos > -0.1 && pos < 0.1)
+    {
+         pos = myLinePos();
+    }
+    pos = myLinePos();
+    while(pos > 0.05 || pos < -0.05)
+    {    
+        pos = myLinePos();
+    }
+    while(pos > -0.1 && pos < 0.1)
+    {
+         pos = myLinePos();
+    }
+    pos = myLinePos();
+    while(pos > 0.05 || pos < -0.05)
+    {    
+        pos = myLinePos();
+    }  
+}
+
+//returns 1 for all turns, 0 for non turns, 2 for end of a task;
+int smoothFollow(float position, float speed, char direction)
 {
     float u = speed;
     u = u * position;
-
-        if(rightCorner() == 1)
+    
+        if(rightCorner() == 1 && direction == 'R')
         {         
             rightTurn(speed);
-            /*int side = sideSensorsLight();
-            huey.forward(speed/2);
-            while(side == 0)
-            {
-                side = sideSensorsLight();
-            }
-            wait(0.20);
-            float pos = myLinePos();
-            huey.right(speed/2);
-            while(pos > -0.1 && pos < 0.1)
-            {
-                pos = myLinePos();
-            }
-            pos = myLinePos();
-            while(pos > 0.05 || pos < -0.05)
-            {    
-                pos = myLinePos();
-            }*/
+            huey.printf("R");
+            return 1;
         }
-        else if(leftCorner() == 1)
+        else if(leftCorner() == 1 && direction == 'L')
         {
             leftTurn(speed);
-            /*
+            huey.printf("L");
+            return 1;
+        }
+        else if(sideSensorsDark() == 1 && direction == 'E')
+        {
+            turnAround(speed);
+            huey.stop();
+            return 2;  
+        }
+        else if((rightCorner() == 1 || leftCorner() == 1) && direction == 'S')
+        {
             int side = sideSensorsLight();
             huey.forward(speed/2);
             while(side == 0)
             {
                 side = sideSensorsLight(); 
             }
-            wait(0.20);
-            
-            float pos = myLinePos();
-            huey.left(speed/2);
-            while(pos > -0.1 && pos < 0.1)
-            {
-                pos = myLinePos();
-            }
-            pos = myLinePos();
-            while(pos > 0.05 || pos < -0.05)
-            {    
-                pos = myLinePos();
-            }*/
+            return 1;
         }  
         else if(speed+u > 1)
         {
             huey.stop();
             huey.printf("Fast Er");
-            //return 0;
+            return 0;
         }
         else
         {
             huey.right_motor(speed-u);
             huey.left_motor(speed+u);
+            return 0;
         }
 }
 
 
 
-
+/*
 void slowStop(float speed, float waitTime, int steps)
 {
     float i;
@@ -204,7 +212,7 @@
     }
     huey.stop();
 }
-
+*/
 
 int pushToStart(void)
 {
@@ -220,17 +228,17 @@
     
     
     //Matrix of paths from one point to another: Right = R, Left = L, Straight = S
-    char* map[6][6] =  
+    char map[6][6][20] =  
     {
-        {"", "LRLLR", "LRLSS", "LRLSRLL", "LRRLSLRRRL", "LRRLR"},
-        {"LRRLR", "", "LLS", "LLRLL", "LSRLRRL", "LRSLR"},
-        {"SSRLR", "SRR", "", "LLL", "LLRSRL", "SSSLR"}, 
-        {"RLSRLR", "RLRR", "RR", "", "SSRL", "SRLRL"}, 
-        {"RLLLRSRLLR", "RLLRLSR", "RLSLR", "RLSS", "", "RLLLRL"}, 
-        {"LRLLR", "LRSLR", "LRSSS", "RLRLS", "RLRRRL", ""}   
-    };    
+        {"", "LRLLRE", "LRLSSE", "LRLSRLLE", "LRRLSLRRRLE", "LRRLRE"},
+        {"LRRLRE", "", "LLSE", "LLRLLE", "LSRLRRLE", "LRSLRE"},
+        {"SSRLRE", "SRRE", "", "LLLE", "LLRSRLE", "SSSLRE"}, 
+        {"RLSRLRE", "RLRRE", "RRE", "", "SSRLE", "SRLRLE"}, 
+        {"RLLLRSRLLRE", "RLLRLSRE", "RLSLRE", "RLSSE", "", "RLLLRLE"}, 
+        {"LRLLRE", "LRSLRE", "LRSSSE", "RLRLSE", "RLRRRLE", ""}   
+    };
     
-    
+    int points[3] = {1,2,4};    
     
     
     
@@ -333,14 +341,30 @@
     {
              
         
-        
-        pos = myLinePos();
-
-        smoothFollow(pos, speed);
-           
+        huey.cls();
+        for(int p = 1; p < sizeof(points); p ++)
+        {
+            int s = points[p-1] - 1;
+            int f = points[p] - 1;
+            for(int i = 0; i < sizeof(map[s][f]); i++)
+            {  
+                int change = 0;
+                while(change == 0)
+                {
+                    pos = myLinePos();
+                    change = smoothFollow(pos, speed, map[s][f][i]);
+                }
+                if(change == 2)
+                {
+                    huey.stop();
+                    wait(1);
+                    break;
+                }
+            }
+        } 
         if(m3pi_IN[0]==0)
         {
-            slowStop(speed, 0.05, 3);
+            //slowStop(speed, 0.05, 3);
             huey.stop();
             btbee.printf("Object in Front");
             while(m3pi_IN[0]==0)