Diff: main.cpp

Revision:

1:e854d5c12659

Parent:

0:47165eb4e54d

Child:

2:b996c95912b5

--- a/main.cpp	Sat Apr 05 07:59:04 2014 +0000
+++ b/main.cpp	Sat Apr 05 11:26:42 2014 +0000
@@ -1,72 +1,90 @@
 #include "mbed.h"
 #include "HCSR04.h"
-#include "SG90.h"
 #include "PwmOut.h"
+#include "hack_motor.h" 
 
 
 Serial pc(USBTX, USBRX);
-servo myservo;
-
- int i = 0;
-int G0 = 0;
-int G15 = 0;
-int G30 = 0;
-int G45 = 0;
-int G60 = 0;
-int G75 = 0;
-int G90 = 0;
-int G105 = 0;
-int G120 = 0;
-int G135 = 0;
-int G150 = 0;
-int G165 = 0;
-int G180 = 0;
-
-float m = 0.5;
-
-PwmOut left(PTE23 );
-PwmOut right(PTE22 );
-int delay = 200; // Delay mellem målingerne
-double B = 0; // Vinklen B Tegning 1
-int A = 179; // Vinkel mellem de to målinger
-
-
+ HCSR04 sensor(PTA13, PTD5, PTD0, PTD2);
+    
+Wheel robot; 
 
 int main(){
+    
+
     pc.baud(9600);
-    HCSR04 sensor(PTA13, PTD5);
-    left.period(0.0066);
-    right.period(0.0066);
-    while(1){
-        myservo.position(1, 0);
-        wait_ms(delay);
-        G0 = sensor.distance(CM);
-        i = i + A;
-        myservo.position(1, 180);
-        G180 = sensor.distance(CM);
-        wait_ms(delay);
-        i = i + A;
-        
-        pc.printf("G0: %ld  G15: %ld G30: %ld G45: %ld G60: %ld G75: %ld G90: %ld G105: %ld G120: %ld G135: %ld G150: %ld G165: %ld G180: %ld \r\n", G0, G15, G30, G45, G60, G75, G90, G105, G120, G135, G150, G165, G180);
-        
+   
+    while(1){                
+        robot.init(); 
+        char c; 
+        pc.printf(" speed 1, 2 & 3, frem w, bagud s, venstre a & hoejre d"); 
+          
+            { 
+            c = pc.getc(); 
+              
+            switch (c) 
+                {  
+                    case 0x31: 
+                        robot.speed = 1.0; 
+                        pc.printf("1"); 
+                        break; 
+                          
+                    case 0x32: 
+                        robot.speed = 0.5; 
+                        pc.printf("2"); 
+                        break; 
+                          
+                    case 0x33:  
+                        robot.speed = 0.25; 
+                        pc.printf("3"); 
+                        break; 
+                          
+                    case 0x77: 
+                        robot.FW(); 
+                        pc.printf("w"); 
+                        break; 
+                      
+                    case 0x73: 
+                        robot.BW(); 
+                        pc.printf("s"); 
+                        break; 
+                          
+                    case 0x61: 
+                        robot.left();  
+                        pc.printf("a"); 
+                        break; 
+                          
+                    case 0x64: 
+                        robot.right(); 
+                        pc.printf("d"); 
+                        break; 
+                        
+                    case 'n':
+                        robot.open();
+                        pc.printf("n");
+                        break;
+                        
+                        case 'm':
+                        robot.close();
+                        pc.printf("m");
+                        break;
+                        
+                        case 'h':
+                        robot.hejs();
+                        pc.printf("h");
+                        break;
+                        
+                        case 'b':
+                        robot.saenk();
+                        pc.printf("b");
+                        break;
+                      
+                    default : 
+                        robot.stop(); 
+                        break; 
+                }
+    }
         
-        if(G0 > G180){
-            left.write(0.4);
-            right.write(0.2);
-            }
-        else if(G180 > G0){
-            left.write(0.2);
-            right.write(0.4);
-            }
-
-        
-        
-        
-        
-        
-        if(i >= 180 || i+20 >= 180){
-            i=0;
-            }//if
     } // while
 } // main