Diff: main.cpp

Revision:

0:6457ead43290

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 07 12:50:01 2015 +0000
@@ -0,0 +1,59 @@
+#include "mbed.h"
+#include "ur_Bertl.h"
+#include "const.h"
+#include "Robot.h"
+
+Serial bt(p9, p10);
+
+
+void main ()
+{
+//    int xy = 5;
+    
+//    while(true){
+//        bt.printf("Hello Bertl: %d\n", xy);
+//        wait(1);}
+
+    ur_Bertl karel;
+
+    int count = 0;
+    int dist = 0;
+
+
+    while (true) {
+
+        
+
+        if (!karel.NextToABeeper()) {
+            karel.Move();
+            dist++;
+                    
+            bt.printf("Move: %d\n", dist);
+            wait(1);
+        }
+
+        if (karel.NextToABeeper()) {
+            karel.Move();
+            dist++;
+            count++;
+            
+            bt.printf("Move: %d\n", dist);
+            bt.printf("Beeper: %d\n", count);
+            wait(1);
+        }
+
+        if (count == 3) {
+            karel.TurnLeft();
+            karel.Move();
+            karel.TurnLeft();
+            for ( int x = 0; x < count; x++)
+                karel.Move();
+            karel.TurnLeft();
+            karel.Move();
+            karel.TurnLeft();
+        }
+    }
+        
+    
+    
+}
\ No newline at end of file