Diff: main.cpp

Revision:

0:47165eb4e54d

Child:

1:e854d5c12659

diff -r 000000000000 -r 47165eb4e54d main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 05 07:59:04 2014 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"
+#include "HCSR04.h"
+#include "SG90.h"
+#include "PwmOut.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
+
+
+
+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);
+        
+        
+        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
+