Diff: main.cpp

Revision:

0:eff3ddf69354

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 05 12:25:12 2010 +0000
@@ -0,0 +1,124 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "SRF05.h"
+
+Servo motor (p21);
+Servo turn (p22);
+
+SRF05 front (p24,p23);
+SRF05 back (p26,p25);
+
+DigitalOut b (p20);
+DigitalOut led1 (LED1);
+DigitalOut led2 (LED2);
+
+void backward (float);
+void forward (float);
+void right (void);
+void left (void);
+void reset (void);
+void stop (void);
+
+
+int i = 0;
+int j = 0;
+
+float sample_delay = 0.01;
+
+int main() {
+
+    b = 0;
+    reset();
+
+    //main loop
+    while (1) {
+
+        //init
+        i = front.read();
+        j = back.read();
+
+        
+           //can go forward?
+        if (i > 100) {
+            while (i > 100) {
+
+                forward(20);
+                i = front.read();
+                j = back.read();
+            }
+
+            stop();
+        }
+        
+        //can it turn to avoid?
+        if (i > 10 && i < 100) {
+            led1 = 1;
+            while (i > 10 && i < 100) {
+                
+                forward(20);
+                turn.write(0);
+                
+                i = front.read();
+                j = back.read();
+            }
+            led1 = 0;
+            stop();
+        }
+        
+        //is there room to ververse and turn?
+        if (i < 20 && j > 10) {
+        led2 = 1;
+            while (i < 20 && j > 10) {
+            
+                backward(20);
+                turn.write(1);
+                
+                i = front.read();
+                j = back.read();
+            }
+            led2 = 0;
+            stop();
+        }
+    }
+}
+
+void reset (void) {
+    turn.write(0.5);
+    motor.write(0.48);
+}
+
+// 0.5 = 0 speed & 0 = full speed
+void forward (float speed) {
+    float y = speed / 100;
+    float x = y * 0.5;
+    float z = 0.5 - x;
+
+    motor.write(z);
+}
+
+// 0.5 = 0 speed & 1 = full speed
+
+void backward (float speed) {
+    float y = speed / 100;
+    float x = y * 0.5;
+    float z = 0.5 + x;
+
+    motor.write(z);
+}
+
+// 0 = right 0.5 = straight 1 = left
+void left (void) {
+    turn.write(1);
+}
+void right (void) {
+    turn.write(0);
+}
+
+void stop (void) {
+    b = 1;
+    wait(0.3);
+    reset();
+    wait(0.1);
+    b = 0;
+    wait(1);
+}
\ No newline at end of file