Diff: main.cpp

Revision:

9:493011bacdcd

Parent:

8:09f4495696d2

Child:

10:86cb40bf1c17

--- a/main.cpp	Fri May 23 06:53:45 2014 +0000
+++ b/main.cpp	Mon May 26 09:50:52 2014 +0000
@@ -2,6 +2,7 @@
 #include "m3pi_ng.h"
 #include "DigitalIn.h"
 #include "btbee.h"
+#include "time.h"
 
 
 DigitalOut myled(LED1);
@@ -56,40 +57,113 @@
         return 0;
 }
 
+//direction varriable is between 0 and 1, 0 meaning left 1 meaning right
+//returns 1 if it turned, 0 otherwise
+/*int crossing(float speed, int direction)
+{
+    if(rightCorner() == 1 && leftCorner() == 1)
+    {
+        if(direction == 0)
+        {
+            huey.left(speed*2);
+            return 1;
+        }
+        else
+        {
+            huey.right(speed*2);
+            return 1;    
+        } 
+    }
+    else
+    {
+        if(rightCorner() == 1)
+        {
+            huey.right(speed*2);
+            return 0;
+        }
+        else if(leftCorner() == 1)
+        {
+            huey.left(speed*2);
+            return 0;
+        }
+        else
+            return 0;
+    }
 
-void smoothFollow(float position, float speed)
+}
+*/
+void smoothFollow(float position, float speed)//, int direction)
 {
     float u = speed;
     u = u * position;
     
-    if(rightCorner() == 1)
-    {
-            huey.right(speed*2);
-            //wait(0.2);
-    }
-    else if(leftCorner() == 1)
+    /*if(rightCorner() == 1 && leftCorner() == 1)
     {
-        huey.left(speed*2);
-        //wait(0.2);
-    }  
-    else if(speed+u > 1)
-    {
-        huey.stop();
-        huey.printf("Fast Er");
+        if(direction == 0)
+        {
+            huey.left(speed*2);
+            return 1;
+        }
+        else
+        {
+            huey.right(speed*2);
+            return 1;    
+        } 
     }
     else
-    {
-        huey.right_motor(speed-u);
-        huey.left_motor(speed+u);
-    }
+    {*/
+        if(rightCorner() == 1)
+        {
+            int sensors[5];
+            huey.calibrated_sensor(sensors);
+            while(sensors[4] > 100)
+            {
+                huey.forward(speed);
+                huey.calibrated_sensor(sensors);  
+            }
+            //Stops and semi-properly turns at corners and t's but gets screwed up at + intersections because he turns until the middle sensors read black
+            // But at a + intersection the sensors are black already so he doesnt turn
+            huey.stop();
+            wait(0.1);
+            float pos = -1;
+            while(pos > 0.05 || pos < -0.05)
+            {    
+                huey.right(speed);
+                pos = myLinePos();
+            }
+            
+            //return 0;
+        }
+        else if(leftCorner() == 1)
+        {
+            huey.left(speed*2);
+            //return 0;
+        }  
+        else if(speed+u > 1)
+        {
+            huey.stop();
+            huey.printf("Fast Er");
+            //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;
     for(i = speed; i > 0; i = i - (speed/steps))
     {
+        //FIGURE THIS OUT KJALSHDFKJASHFLKJASHFLKJASHDLKJASHDLKJHASDLKJHASLKDJH
         smoothFollow(huey.line_position(),i);
         wait(waitTime);      
     }
@@ -108,8 +182,10 @@
 
 /////////////////////////////////////////////////////////////////
 int main() {
+    //time_t startt, endt;
+    //time(&startt);
+    //clock_t clock;
     
-    /*
      //btbee trial
     // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
     huey.locate(0,1);
@@ -143,16 +219,18 @@
     char arr_read[30]; // this should be long enough to store any reply coming in over bt.
     int  chars_read;   // number of chars read in a bt reply
     
-    while (true) {
+    while (iline < 3) {
         // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
         if ( btbee.writeable() ) {
-            if (iline==6) {
-                btbee.printf("end\n");
+            if (iline==2) {
+                //btbee.printf("end\n");
                 iline++;
             }//if
             else {
-                if (iline <6){
-                btbee.printf("Line %0.3d \n",iline);
+                if (iline <2){
+                //time(&endt);
+                //btbee.printf("%f seconds - ", clock);
+                btbee.printf("Battery Voltage: %f\n", huey.battery());
                 m3pi_led[0]=0;
                 huey.locate(0,0);
                 huey.printf("Sent %0.3d",iline);
@@ -172,7 +250,7 @@
         }//while_readable
         wait(0.1);
     }//while_true
-    */
+    
 //main
 //end of btbee trial
    
@@ -197,21 +275,49 @@
     float speed = 0.25;
     float pos;
     int z=1;
+    int turns[3];
+    turns[0] = 0;
+    turns[1] = 1;
+    turns[2] = 0;
+    
+    
     while(z==1)
     {
-        //huey.right_motor(speed);
-        pos = myLinePos();//huey.line_position();
+        
+        //Trying to get it to follow a sequence of turns
+        /*for(int q = 0; q < sizeof(turns); q++)
+        {
+            int change = 0;
+            while(change == 0)
+            {
+                //change = crossing(speed, turns[q]);
+                pos = myLinePos();
+                change = smoothFollow(pos, speed, turns[q]);
+                    
+            }  
+        }*/
+        
+        
+        
+        pos = myLinePos();
+
         smoothFollow(pos, speed);
+           
         if(m3pi_IN[0]==0)
         {
             slowStop(speed, 0.05, 3);
             huey.stop();
+            btbee.printf("Object in Front");
             while(m3pi_IN[0]==0)
             {
-                huey.printf("Stuck");    
+                huey.stop();    
             }
         }
-
+        if(pushToStart() == 0)
+        {
+                btbee.printf("end\n");
+                break;
+        }
     }