Diff: main.cpp

Revision:

0:d429f13fe4be

Child:

1:2859bfed20b4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 03 05:13:08 2014 +0000
@@ -0,0 +1,210 @@
+#include "mbed.h"
+#include "Ultrasonic.h"
+
+AnalogIn Sharp(p20);
+Ultrasonic F_sonic_R(p11,p11);
+Ultrasonic F_sonic_L(p12,p12);
+Ultrasonic F_sonic_F(p8,p8);
+
+PwmOut RMotor_Front(p21);
+PwmOut LMotor_Front(p23);
+PwmOut RMotor_Back(p22);
+PwmOut LMotor_Back(p24);
+DigitalOut RMotor_EN(p5);
+DigitalOut LMotor_EN(p6);
+Timeout motortime;
+Timeout backmotortime;
+Timeout Rightmotortime;
+Timeout Leftmotortime;
+float a,b =0;
+float length;
+void Front()
+{
+    RMotor_EN = 1;
+    LMotor_EN = 1;
+    RMotor_Back.pulsewidth(0);
+    LMotor_Back.pulsewidth(0);
+    RMotor_Front.pulsewidth(0.05);
+    LMotor_Front.pulsewidth(0.05);
+}
+
+void Turn_R()
+{
+    RMotor_EN = 1;
+    LMotor_EN =1;
+    LMotor_Front.pulsewidth(0);
+    RMotor_Back.pulsewidth(0);
+    LMotor_Back.pulsewidth(0.1);
+    RMotor_Front.pulsewidth(0.1);
+}
+
+void Turn_L()
+{
+    LMotor_EN = 1;
+    RMotor_EN = 1;
+    LMotor_Back.pulsewidth(0);
+    RMotor_Front.pulsewidth(0);
+    RMotor_Back.pulsewidth(0.1);
+    LMotor_Front.pulsewidth(0.1);
+}
+void Back()
+{
+    RMotor_EN = 1;
+    LMotor_EN = 1;
+    RMotor_Front.pulsewidth(0);
+    LMotor_Front.pulsewidth(0);
+    RMotor_Back.pulsewidth(0.05);
+    LMotor_Back.pulsewidth(0.05);
+}
+void Break()
+{
+    RMotor_Front.pulsewidth(0);
+    LMotor_Front.pulsewidth(0);
+    RMotor_Back.pulsewidth(0.05);
+    LMotor_Back.pulsewidth(0.05);
+    wait(0.1);
+    RMotor_EN = 0;
+    LMotor_EN = 0;
+}
+void Stop()
+{
+    RMotor_EN = 0;
+    LMotor_EN = 0;
+    RMotor_Front.pulsewidth(0);
+    LMotor_Front.pulsewidth(0);
+    RMotor_Back.pulsewidth(0);
+    LMotor_Back.pulsewidth(0);
+}
+
+
+int dis_L,dis_R,dis_F = 80;
+int F_len;
+int move,measure,flag=1;
+
+void count()
+{
+    dis_F =100;
+    dis_L =100;
+    dis_R =100;
+    move =0;
+    measure =0;
+    flag=1;
+}
+
+void backcount(){
+    move = 1; measure =0;
+    }
+void Rightcount(){
+    move = 2; measure =0;
+    }
+void Leftcount(){
+    move = 3;   measure =0;
+    }
+int main()
+{
+
+    while(1) {
+
+        //거리측정
+        if(dis_F==F_sonic_F.read()/10) {
+            dis_F = 80;}
+        if(dis_R==F_sonic_R.read()/10) {
+            dis_R = 90;}
+        if(dis_L==F_sonic_L.read()/10) {
+            dis_L = 90;}
+                
+        if(dis_F!=F_sonic_F.read()/10) {
+            dis_F = F_sonic_F.read()/10;
+        }
+        if(dis_R!=F_sonic_R.read()/10) {
+            dis_R = F_sonic_R.read()/10;
+        }
+        if(dis_L!=F_sonic_L.read()/10) {
+            dis_L = F_sonic_L.read()/10;
+        }
+       
+        printf("Front: %d\n",dis_F);
+        printf("Right: %d\n",dis_L);
+        printf("Left : %d\n",dis_R);
+        printf("move : %d\n",move);
+
+       /* if(dis_R <50) {
+                if(dis_L <50){
+                    if(dis_F >50){
+                move =0;}}}
+        else if(dis_L <50) {
+                if(dis_R <50){
+                    if(dt is_F >50){
+                move =0;}}}*/
+        
+     
+        switch(measure){
+            case 0:
+               if(dis_F >30) {
+                if(dis_R <50) {
+                if((dis_R + 5) < dis_L){
+                move =2; measure =1;}
+            } else if(dis_L <50) {
+                if((dis_L + 5) < dis_R){
+                move =3;measure =1;}
+            } else {
+                move =0; measure =1;
+            }
+            } else if(dis_F<30) {
+            move =1; measure =1;
+            }
+            break;
+            }
+            
+        
+        if(dis_R <10){move=5;}
+        else if(dis_L <10){ move =6;}
+        else if((dis_R <25) &&(dis_L<25)){move =4;}
+        else if(dis_R <35){move =2;}
+        else if(dis_L <35){move =3;}
+            
+        switch(move) {
+            case 0://Front
+                Front();
+                measure =0;
+                break;
+
+            case 1://Back
+                Stop();
+                measure =0;
+                break;
+
+            case 2://Right
+                Turn_R();
+                if (flag==1) {
+                    flag=0;
+                    motortime.attach(&count,1);
+                }
+                wait(1);
+                break;
+
+            case 3://Left
+                Turn_L();
+                if (flag==1) {
+                    flag=0;
+                    motortime.attach(&count,1);
+                }
+                wait(1);
+                break;
+           
+           case 4://back
+                Back();
+                backmotortime.attach(&backcount,2);
+                break;
+           case 5://Right
+                Back();
+                Rightmotortime.attach(&Rightcount,2);
+                break;
+           case 6://back
+                Back();
+                Leftmotortime.attach(&Leftcount,2);
+                break;
+        }
+    }
+
+}
\ No newline at end of file