Diff: main.cpp

Revision:

1:b77a3bd2e93e

Parent:

0:cedc1d2ef037

Child:

2:5c47df2771ec

--- a/main.cpp	Fri Jan 09 08:57:41 2015 +0000
+++ b/main.cpp	Sat Jan 10 04:18:22 2015 +0000
@@ -1,7 +1,12 @@
 #include "mbed.h"
 
+Timer t;
+Serial pc(USBTX,USBRX);
 Serial trans(p9,p10);
 DigitalOut myled(LED1);
+DigitalOut led2(LED2);
+DigitalOut trig(p19);
+DigitalIn echo(p20);
 BusOut mc(p23,p24,p25,p26);
 PwmOut m1(p21);
 PwmOut m2(p22);
@@ -14,9 +19,12 @@
     int l = 6;
     int r = 9;
     int stop = 0;
-    double mp1,mp2;
+    double mp1,mp2,mp1b;
     mp1 = (c-1)/100.0;
+    mp1b = (1-c)/100.0;
     mp2 = 1.0-((d-1)/100.0);
+    //pc.printf("mp1*mp2--->%f\n\r",mp1*mp2);
+    //pc.printf("mp1--->%f",mp1);
     if(d>b+width)
     {
         if(c>b+width)
@@ -28,7 +36,7 @@
         {
            mc = gyaku;
            m2 = 1-mp1;
-           m1 = mp1*mp2;
+           m1 = (1-mp1)*mp2;
         }else{
             mc = l;
             m1 = 1-mp2;
@@ -43,7 +51,7 @@
         }else if(c<b-width)
         {
            mc = gyaku;
-           m2 = mp1*mp2;
+           m2 = 1-(1-mp1)*mp2;
            m1 = 1-mp1;
         }else{
             mc = r;
@@ -81,7 +89,24 @@
         updown = 0;
     }
 }
+double sonic(double so)
+{
+    t.reset();
+    trig = 1;
+    wait_us(10);
+    trig = 0;
+    if(echo == 1){
+        t.start();
+    }
+    if(echo == 0){
+        t.stop();
+        so = t.read();
+        so = (so/2.0)*34000.0;
+    }
+    return so;
+}
 int main() {
+    double dis = 0;
     int8_t nx,ny,x,y,down_val,up_val;
     while(1) {
         if (trans.getc()== 255){
@@ -92,8 +117,7 @@
             up_val = trans.getc();
             down_val = trans.getc();
             move(nx,ny,x,y);
-            up_down(up_val,down_val);
+            up_down(up_val,down_val);                    
         }
-    }
-        
+    }      
 }