Revision 1:234edfc8cac1, committed 2016-01-26

Comitter:

andreashatzl

Date:

Tue Jan 26 18:03:33 2016 +0000

Parent:

0:5a51d1a48be4

Commit message:

gjh

Changed in this revision

diff -r 5a51d1a48be4 -r 234edfc8cac1 HCSR04.lib
--- a/HCSR04.lib	Mon Dec 14 19:41:19 2015 +0000
+++ b/HCSR04.lib	Tue Jan 26 18:03:33 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/antoniolinux/code/HCSR04/#86b2086be101
+https://developer.mbed.org/teams/Diplomarbeit_blimp/code/aktuell26012016/#f91efabe8d8a

diff -r 5a51d1a48be4 -r 234edfc8cac1 main.cpp
--- a/main.cpp	Mon Dec 14 19:41:19 2015 +0000
+++ b/main.cpp	Tue Jan 26 18:03:33 2016 +0000
@@ -9,27 +9,30 @@
 Serial HC06(p13, p14);
 HCSR04 sensor(p12, p11);
 PwmOut servo1(p21),servo2(p22),servo3(p23),servo4(p24);
-Ticker t1,t2,t3;
+Ticker t1,t2,t3,t4,t5;
 
 void servomovedown();
 void servomoveup();
 void automode();
 void checkdistance();
+void distancesecure();
+void writedistance();
 
 
 
-int wert;
-int servotickerstatus =0;
-float distance;
+int wert,distancecheck=0;
+int servotickerstatus =0,automovestatus=0,righttickerstatus=0,lefttickerstatus=0;
+float power;
+long distance,distance1,distance2,distance3;
 
-int main() 
+int main()
 {
-    servo1.period_ms(20); 
+    servo1.period_ms(20);
     servo2.period_ms(20);
     servo3.period_ms(20);
     servo4.period_ms(20);
-    
-    
+
+
     char serChar, count;
     pc.baud(115200);
     pc.printf("\r\n HC06 is activ!n");
@@ -38,57 +41,110 @@
     wait(0.6);
     led_1 = 0;
     wait(0.6);
-    led_1= led_2= 1; count=0; wert = 1500;
-    
+    led_1= led_2= 1;
+    count=0;
+    wert = 1500;
+
     HC06.baud(9600);
     HC06.printf("mbed started!\n");
-    
+
     wert=1500;
-    
-    while(1) 
-    {
-        if (HC06.readable()) 
-        {   count++;
+
+    while(1) {
+        if (HC06.readable()) {
+            count++;
             serChar = HC06.getc();
             pc.putc(serChar);
-            switch (serChar)
-            {   case '1':
-                    {   servo1.pulsewidth_us(1000);
-                        servo2.pulsewidth_us(1000); break;
+            switch (serChar) {
+                case '1': {
+                    servo1.pulsewidth_us(1000);
+                    servo2.pulsewidth_us(1000);
+                    break;
+                }
+                case '2':
+                case '3':
+                case '4':
+                case '5':
+                case '6':
+                case '7':
+                case '8':
+                case '9': {
+                    int mal = serChar - '0';
+                    HC06.printf("Der Wert ist: %d\n",mal);
+                    servo1.pulsewidth_us(1140+(50*mal));
+                    servo2.pulsewidth_us(1020+50*mal);
+                    break;
+                }
+                case 'b': {
+                    if(servotickerstatus == 1) {
+                        t1.detach();
+                        servotickerstatus=0;
+                        break;
                     }
-                case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9':
-                    {   
-                        int mal = serChar - '0';
-                        pc.printf("Der Wert ist: %d\n",mal);
-                        servo1.pulsewidth_us(1060+(50*mal));
-                        servo2.pulsewidth_us(1060+50*mal); 
+                    if(servotickerstatus == 0) {
+                        servotickerstatus = 1;
+                        t1.attach(&servomovedown, 0.01);
                         break;
                     }
-                case 'b':
-                        { 
-                        if(servotickerstatus == 1)
-                         { t1.detach();servotickerstatus=0;break;}
-                        if(servotickerstatus == 0)
-                          {servotickerstatus = 1;t1.attach(&servomovedown, 0.01);break;}
-                        }                    
-                    //wert= wert+100;servo4.pulsewidth_us(wert); break;   
-                case 'f':
-                        { 
-                        if(servotickerstatus == 1)
-                         { t1.detach();servotickerstatus=0;break;}
-                        if(servotickerstatus == 0)
-                          {servotickerstatus = 1;t1.attach(&servomoveup, 0.01);break;}
-                        }
+                }
+                //wert= wert+100;servo4.pulsewidth_us(wert); break;
+                case 'f': {
+                    if(servotickerstatus == 1) {
+                        t1.detach();
+                        servotickerstatus=0;
+                        break;
+                    }
+                    if(servotickerstatus == 0) {
+                        servotickerstatus = 1;
+                        t1.attach(&servomoveup, 0.01);
+                        break;
+                    }
+                }
                 case 'l':
-                    servo3.pulsewidth_us(1000); break;
+                {
+                     if(lefttickerstatus == 1) {
+                        servo3.pulsewidth_us(1730);
+                        wait(1);
+                        servo3.pulsewidth_us(1500);
+                        lefttickerstatus=0;
+                        break;
+                    }
+                    if(lefttickerstatus == 0) {
+                        lefttickerstatus = 1;
+                        servo3.pulsewidth_us(1150);
+                        break;
+                    }}
                 case 'r':
-                    servo3.pulsewidth_us(1650); break;
+                    { if(righttickerstatus == 1) {
+                        servo3.pulsewidth_us(1150);
+                        wait(1);
+                        servo3.pulsewidth_us(1500);
+                        righttickerstatus=0;
+                        break;
+                    }
+                    if(righttickerstatus == 0) {
+                        righttickerstatus = 1;
+                        servo3.pulsewidth_us(1730);
+                        break;}}
                 case 's':
-                    servo3.pulsewidth_us(1500); break;
+                    servo3.pulsewidth_us(1500);
+                    break;
                 case 'a':
-                    automode();break;  
+                    if(automovestatus == 1) {
+                        t3.detach();
+                        t4.detach();
+                        automovestatus=0;
+                        break;
+                    }
+                    if(automovestatus == 0) {
+                        automovestatus = 1;
+                        automode();
+                        break;
+                    }
                 default:
-                    led_1=led_3=0; led_2=led_4=1; break;
+                    led_1=led_3=0;
+                    led_2=led_4=1;
+                    break;
             }
         }
     }
@@ -98,45 +154,69 @@
 {
     wert = wert + 10;
     servo4.pulsewidth_us(wert);
-    if (wert > 2200)
-    {t1.detach();servotickerstatus=0;}
+    if (wert > 2200) {
+        t1.detach();
+        servotickerstatus=0;
     }
-    
+}
+
 void servomoveup()
 {
     wert = wert - 10;
     servo4.pulsewidth_us(wert);
-    if (wert < 870)
-    {t1.detach();servotickerstatus=0;}
+    if (wert < 870) {
+        t1.detach();
+        servotickerstatus=0;
     }
-    
+}
+
 void automode()
 {
-    servo4.pulsewidth_us(870);
-    servo1.pulsewidth_us(1100);
-    servo2.pulsewidth_us(1100);
-    t3.attach(&checkdistance, 2.0);
-    
-    }
-    
+    servo4.pulsewidth_us(900);
+    servo1.pulsewidth_us(1120);
+    servo2.pulsewidth_us(1050);
+    t4.attach(&distancesecure,0.04);
+    t3.attach(&checkdistance, 0.2);
+    t5.attach(&writedistance, 2);
+    return;
+
+}
+
 void checkdistance()
 {
-        long distance = sensor.distance();
-        pc.printf("Distance = %d\n", distance);
-            if (distance < 50)
-                {
-                servo1.pulsewidth_us(1100);
-                servo2.pulsewidth_us(1100);
-                pc.printf("Distance ok"); 
-                pc.printf("Distance = %d\n", distance);  
-                }
-            else 
-                {
-                 servo1.pulsewidth_us(1300);
-                 servo2.pulsewidth_us(1300);
-                 pc.printf("Distance nicht ok");
-                 pc.printf("Distance = %d\n", distance);   
-                }  
+   // if (distance > 150) {
+     //   servo1.pulsewidth_us(1120);
+      //  servo2.pulsewidth_us(1050);
+   // } else {
+        power = 1733-((distance*10)/3);
+        servo1.pulsewidth_us(power);
+        servo2.pulsewidth_us(power-80);
+   // }
+}
+
+void writedistance()
+{
+     HC06.printf(" %d", distance);
+     pc.printf("%d\n", distance);
+    }
+
+void distancesecure()
+{
+    distancecheck ++;
+    if (distancecheck == 1) {
+        distance1 = sensor.distance();
+    }
+    if (distancecheck == 2) {
+        distance2 = sensor.distance();
+    }
+    if (distancecheck == 3) {
+        distance3 = sensor.distance();
+    }
+    if (distancecheck == 4) {
+        distancecheck = 0;
+        distance = ((distance1+distance2+distance3)/3);
+    }
+
 }