leuk

Dependencies:   mbed HCSR04

Files at this revision

API Documentation at this revision

Comitter:
mirteholm
Date:
Wed May 29 15:28:52 2019 +0000
Parent:
0:7d87166a1170
Commit message:
leuk

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 7d87166a1170 -r a8c5e92c6cbf main.cpp
--- a/main.cpp	Tue Apr 09 07:25:01 2019 +0000
+++ b/main.cpp	Wed May 29 15:28:52 2019 +0000
@@ -2,34 +2,218 @@
 // the result of the printf on PC (USB Virtual Com)
 // I suggest to use TeraTerm on PC.
 // TeraTerm configuration must be: 9600-8-N-1 FlowControl None
- 
+
 /* EXAMPLE */
 #include "mbed.h"
 #include "hcsr04.h"
 
-DigitalIn IR(PA_4);
-DigitalOut led(PA_3);
 Serial pc(USBTX, USBRX);
- 
-//D12 TRIGGER D11 ECHO
-HCSR04 sensor(D12, D11); 
-int main() 
+
+
+DigitalOut linksachter(D9);
+DigitalOut rechtsachter(A6);
+DigitalOut rechtsvoor(D7);
+DigitalOut linksvoor(D10);
+PwmOut pwm(D6); // links
+PwmOut pwm2(D11);
+
+HCSR04 sensor(D4, A0);
+
+
+
+DigitalIn lijnsensor4(D1);
+DigitalIn lijnsensor3(D0);
+DigitalIn lijnsensor1(D2);
+DigitalIn lijnsensor2(D3);
+
+
+HCSR04 sensor2(D4, A2);
+//HCSR04 sensor2(D4, A0);
+//HCSR04 sensor3(D4, A0);
+//HCSR04 sensor4(D4, A0);
+
+
+
+Timer timer;
+Timer timer2;
+Timer timer3;
+Timer timer4;
+Timer timer5;
+Timer timer6;
+
+
+enum ROBOTSTATE {vooruit, lijn_detect_linksvoor, lijn_detect_rechtsvoor, lijn_detect_linksachter, lijn_detect_rechtsachter, lijn_detect_voor, lijn_detect_achter, robot_detect_voor, robot_detect_rechts, robot_detect_links, robot_detect_achter, start,};
+ROBOTSTATE myState;
+int main()
 {
-    while(1) 
-    {
-        int a = IR;
-        if(a==1)
-        {
-            led=0;
+     pwm.period_ms(10/6);
+    pwm.pulsewidth_ms(1);
+    pwm2.period_ms(10/6);
+    pwm2.pulsewidth_ms(1);
+
+    while(1) {
+        
+          
+
+       switch(myState) {
+
+  
+
+    
+
+            case vooruit:
+              pwm.period_ms(10/6);
+    pwm.pulsewidth_ms(1);
+    pwm2.period_ms(10/6);
+    pwm2.pulsewidth_ms(1);
+            
+                pwm=0.4;
+                pwm2=0.4;
+                linksvoor=1;
+                rechtsvoor=1;
+                linksachter=0;
+                rechtsachter=0;
+                if(lijnsensor1==0) {
+                    myState = lijn_detect_linksvoor;
+                }
+                if(lijnsensor2==0) {
+                    myState=lijn_detect_rechtsvoor;
+                }
+                if (lijnsensor3 ==0) {
+                    myState=lijn_detect_linksachter;
+                }
+                if (lijnsensor4==0) {
+                    myState = lijn_detect_rechtsachter;
+                }
+                if (sensor.distance()<50) { myState = robot_detect_voor; }
+                if (sensor2.distance()<30) {myState = robot_detect_achter;}
+           break;
+               
+             
+
+            case lijn_detect_linksvoor:
+  
+                linksvoor=1;
+                rechtsvoor=0;
+                linksachter=0;
+                rechtsachter=0;
+                pwm=0.9;
+                pwm2=0.9;
+                wait_ms(2000);
+                linksvoor=1;
+                rechtsvoor=1;
+                wait_ms(500);
+                myState = vooruit;
+                break;
+
+            case lijn_detect_rechtsvoor:
+     
+
+                linksvoor=0;
+                rechtsvoor=1;
+                linksachter=0;
+                rechtsachter=0;
+                pwm=0.9;
+                pwm2=0.9;
+                wait_ms(2000);
+                linksvoor=1;
+                rechtsvoor=1;
+                wait_ms(500);
+                myState= vooruit;
+                break;
+
+
+            case lijn_detect_linksachter:
+   
+                timer3.start();
+                linksvoor=0;
+                rechtsvoor=1;
+                linksachter=0;
+                rechtsachter=0;
+                pwm=0.7;
+                pwm2=0.8;
+                if(timer3.read_ms() >= 500 ) {
+                    timer3.stop();
+                    timer3.reset();
+                    myState = vooruit;
+                }
+                break;
+
+            case lijn_detect_rechtsachter:
+    
+                timer4.start();
+                linksvoor=1;
+                rechtsvoor=0;
+                linksachter=0;
+                rechtsachter=0;
+                pwm=0.7;
+                pwm2=0.8;
+                if(timer4.read_ms() >= 500 ) {   // voor twee seconden draait hij
+                    timer4.stop();
+                    timer4.reset();
+                    myState = vooruit;
+                }
+                break;
+
+        
+            case robot_detect_voor:
+ 
+              
+            timer5.start();
+            linksvoor=1;
+            rechtsvoor=1;
+            linksachter=0;
+            rechtsachter=0;
+            pwm=1;
+            pwm2=1;
+            
+             if(lijnsensor1==0) {
+                    timer5.stop(); timer5.reset();  myState = lijn_detect_linksvoor;
+                }
+                if(lijnsensor2==0) {
+                   timer5.stop(); timer5.reset();  myState=lijn_detect_rechtsvoor;
+                }
+                if (lijnsensor3 ==0) {
+                   timer5.stop(); timer5.reset();  myState=lijn_detect_linksachter;
+                }
+                if (lijnsensor4==0) {
+                    timer5.stop(); timer5.reset();  myState = lijn_detect_rechtsachter;
+                }
+            if (timer5.read_ms() >= 2000) { timer5.stop(); timer5.reset(); myState = vooruit; }
+            break;
+
+    case robot_detect_achter:
+
+
+         timer6.start();
+         pwm=1;
+         pwm2=1;
+            linksvoor=1;
+            rechtsvoor=0;
+           linksachter=0;
+           rechtsachter=0;
+           
+             if(lijnsensor1==0) {
+                    timer6.stop(); timer6.reset();  myState = lijn_detect_linksvoor;
+              }
+              if(lijnsensor2==0) {
+                 timer6.stop(); timer6.reset();  myState=lijn_detect_rechtsvoor;
+              }
+             if (lijnsensor3 ==0) {
+                  timer6.stop(); timer6.reset();  myState=lijn_detect_linksachter;
+               }
+               if (lijnsensor4==0) {
+                   timer6.stop(); timer6.reset();  myState = lijn_detect_rechtsachter;
+                }
+            if (timer6.read_ms() >= 1000) { timer6.stop(); timer6.reset(); myState = vooruit; }
+   
+           
+
+
+
+
+
+
         }
-        else
-        {
-            led=1;
-        }
- 
-     long distance = sensor.distance();   
-      printf("distance  %d\r\n",distance);
-      wait_ms(50); // 1 sec  
- 
     }
 }
\ No newline at end of file