Diff: main.cpp

Revision:

2:b996c95912b5

Parent:

1:e854d5c12659

--- a/main.cpp	Sat Apr 05 11:26:42 2014 +0000
+++ b/main.cpp	Mon Apr 07 06:24:19 2014 +0000
@@ -8,20 +8,21 @@
  HCSR04 sensor(PTA13, PTD5, PTD0, PTD2);
     
 Wheel robot; 
-
+int control = 0;
+int L1 = 0;
+int L2 = 0;
 int main(){
     
 
     pc.baud(9600);
    
-    while(1){                
+    while(control == 0){                
         robot.init(); 
         char c; 
         pc.printf(" speed 1, 2 & 3, frem w, bagud s, venstre a & hoejre d"); 
           
             { 
-            c = pc.getc(); 
-              
+            c = pc.getc();  
             switch (c) 
                 {  
                     case 0x31: 
@@ -39,24 +40,29 @@
                         pc.printf("3"); 
                         break; 
                           
-                    case 0x77: 
+                    case 's': 
                         robot.FW(); 
-                        pc.printf("w"); 
+                        pc.printf("s"); 
                         break; 
                       
-                    case 0x73: 
+                    case 'w': 
                         robot.BW(); 
-                        pc.printf("s"); 
+                        pc.printf("w"); 
                         break; 
                           
                     case 0x61: 
                         robot.left();  
                         pc.printf("a"); 
+                        wait(0.1);
+                        robot.stop();
+                        
                         break; 
                           
                     case 0x64: 
                         robot.right(); 
-                        pc.printf("d"); 
+                        pc.printf("d");
+                        wait(0.1);
+                        robot.stop(); 
                         break; 
                         
                     case 'n':
@@ -78,13 +84,61 @@
                         robot.saenk();
                         pc.printf("b");
                         break;
+                        
+                        case 'p':
+                        control = 1;
+                        break;                        
+                        
                       
                     default : 
                         robot.stop(); 
                         break; 
                 }
     }
+        }
+        {
+       
+    
+    
+        
+        
         
     } // while
+    
+    while(control == 1){                
+        
+        L1 = sensor.Distance(CM); // Højre
+        L2 = sensor.distance(CM); // Venstre
+        
+        pc.printf("L1: %ld  L2: %ld \r\n", L1, L2);
+        
+        
+        if(L1 >= L2 && L1 > 50){
+        robot.venSelv3(); // Højre skarpt
+        }
+        
+        if(L2 >= L1 && L2 > 50){
+        robot.venSelv4(); // Venstre skarpt
+        }
+        
+        if(L1 >= L2){
+        robot.venSelv1(); // Højre
+        }
+        
+        
+        
+        if(L2 > L1){
+        robot.venSelv2(); // Venstre
+        }
+        
+        if(L1 > 65){
+            control = 0;
+            }
+            }
+    
+    
+    
+    
+    
 } // main