Diff: main.cpp

Revision:

0:c6cbcb90e098

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 12 12:57:20 2010 +0000
@@ -0,0 +1,163 @@
+#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);
+
+LocalFileSystem local("local");
+InterruptIn w(p16);
+Timer timer;
+
+void backward (float);
+void forward (float);
+void right (void);
+void left (void);
+void reset (void);
+void stop (void);
+
+
+//servo vars
+int i = 0;
+int j = 0;
+
+//timer/sensor vars
+int count;
+int begin;
+int end;
+int writeBegin;
+int writeEnd;
+int single_spin;
+int ws;
+//const int wheel = 257; //in mm
+//int Tdist = 0;
+
+//intrupt program exicuted
+void trigger() {
+
+    begin = timer.read_ms();
+    single_spin = begin - end;
+    end = timer.read_ms();
+    
+    
+    writeBegin = timer.read_ms();
+    FILE *fp = fopen("/local/out.csv", "a");
+    count = count + 1;
+    //myled2 = !myled2;
+    fprintf(fp, "%d, %d,%d\n", count, single_spin, ws);
+    fclose(fp);
+    
+    writeEnd = timer.read_ms();       
+    ws = writeEnd - writeBegin;
+    end = end - ws;
+
+}
+
+
+int main() {
+
+    timer.start(); //start timer
+    w.rise(&trigger); //set trigger
+
+    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