Diff: main.cpp

Revision:

8:09f4495696d2

Parent:

7:d0c19729e7fd

Child:

9:493011bacdcd

diff -r d0c19729e7fd -r 09f4495696d2 main.cpp
--- a/main.cpp	Thu May 22 12:51:22 2014 +0000
+++ b/main.cpp	Fri May 23 06:53:45 2014 +0000
@@ -1,11 +1,17 @@
 #include "mbed.h"
 #include "m3pi_ng.h"
 #include "DigitalIn.h"
+#include "btbee.h"
+
 
 DigitalOut myled(LED1);
 m3pi huey;
+btbee btbee;
+
 DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor
-
+DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
+DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
+DigitalIn m3pi_pb(p21);
 
 void printBattery()
 {
@@ -30,11 +36,43 @@
     
 }
 
+int rightCorner()
+{
+    int sensors[5];
+    huey.calibrated_sensor(sensors);
+    if(sensors[4] > 900)
+        return 1;
+    else
+        return 0;
+}
+
+int leftCorner()
+{
+    int sensors[5];
+    huey.calibrated_sensor(sensors);
+    if(sensors[0] > 900)
+        return 1;
+    else
+        return 0;
+}
+
+
 void smoothFollow(float position, float speed)
 {
     float u = speed;
     u = u * position;
-    if(speed+u > 1)
+    
+    if(rightCorner() == 1)
+    {
+            huey.right(speed*2);
+            //wait(0.2);
+    }
+    else if(leftCorner() == 1)
+    {
+        huey.left(speed*2);
+        //wait(0.2);
+    }  
+    else if(speed+u > 1)
     {
         huey.stop();
         huey.printf("Fast Er");
@@ -67,9 +105,78 @@
 }
 
 
+
 /////////////////////////////////////////////////////////////////
 int main() {
     
+    /*
+     //btbee trial
+    // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
+    huey.locate(0,1);
+    btbee.reset();
+    for (int i = 0; i <4; i++) {
+        mbed_led[i] = 0;
+    }
+    for (int i = 0; i <8; i++) {
+        m3pi_led[i]=0;
+    }
+    m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
+
+    // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
+
+    huey.locate(0,0);
+    huey.printf("B:%0.3f%V",huey.battery());
+    wait(0.3);
+    
+    huey.locate(0,0);
+    huey.printf("%s","btTest");
+    huey.locate(0,1);
+    huey.printf("%s","PBonLNK");
+
+    // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
+    while(m3pi_pb) {
+        m3pi_led[0]=!m3pi_led[0];
+        wait(0.1);
+    }
+
+    int iline=1;
+    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) {
+        // 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");
+                iline++;
+            }//if
+            else {
+                if (iline <6){
+                btbee.printf("Line %0.3d \n",iline);
+                m3pi_led[0]=0;
+                huey.locate(0,0);
+                huey.printf("Sent %0.3d",iline);
+                iline++;
+                }
+            }//else
+        }//if_write
+
+        // check for answers after each write /not write
+        while ( btbee.readable() ) {
+            m3pi_led[7]=1;
+            btbee.read_all(arr_read, 30, &chars_read );
+            m3pi_led[6]=1;
+            huey.locate(0,1);
+            huey.print(arr_read,chars_read);
+            m3pi_led[5]=1;
+        }//while_readable
+        wait(0.1);
+    }//while_true
+    */
+//main
+//end of btbee trial
+   
+    
     
     m3pi_IN[0].mode(PullUp);
     m3pi_IN[1].mode(PullUp);