Revision 0:85792b24326d, committed 2012-06-26

Comitter:

yueee_yt

Date:

Tue Jun 26 03:02:06 2012 +0000

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi.lib	Tue Jun 26 03:02:06 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/chris/code/m3pi/#4b7d6ea9b35b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 26 03:02:06 2012 +0000
@@ -0,0 +1,374 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+m3pi m3pi;
+
+// Minimum and maximum motor speeds
+#define MAX 0.3
+#define MAX2 0.6
+#define MIN 0
+
+// PID terms
+#define P_TERM 0.6
+#define I_TERM 0
+#define D_TERM 20
+DigitalIn sw(p21);
+char path[100] = "";
+unsigned char path_length = 0;
+
+void follow_segment(void);
+void follow_segment2(void);
+void m3pi_play( char x[]) {
+    m3pi.putc(0xb3);
+    int i;
+    m3pi.putc(strlen(x));
+    for (i=0; i<strlen(x); i++) {
+        m3pi.putc(x[i]);
+    }
+}
+void read_line(unsigned int sensors[5]) {
+    unsigned char x[10];
+    int i;
+    m3pi.putc(0x87);
+    for (i=0; i<10; i++) {
+        x[i]=m3pi.getc();
+    }
+    sensors[0]=(x[1]*0x100+x[0])*2;
+    sensors[1]=(x[3]*0x100+x[2])*2;
+    sensors[2]=(x[5]*0x100+x[4])*2;
+    sensors[3]=(x[7]*0x100+x[6])*2;
+    sensors[4]=(x[9]*0x100+x[8])*2;
+}
+
+char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right) {
+    // Make a decision about how to turn.  The following code
+    // implements a left-hand-on-the-wall strategy, where we always
+    // turn as far to the left as possible.
+    if (found_left)
+        return 'L';
+    else if (found_straight)
+        return 'S';
+    else if (found_right)
+        return 'R';
+    else
+        return 'B';
+}
+void simplify_path() {
+    // only simplify the path if the second-to-last turn was a 'B'
+    if (path_length < 3 || path[path_length-2] != 'B')
+        return;
+
+    int total_angle = 0;
+    int i;
+    for (i=1; i<=3; i++) {
+        switch (path[path_length-i]) {
+            case 'R':
+                total_angle += 90;
+                break;
+            case 'L':
+                total_angle += 270;
+                break;
+            case 'B':
+                total_angle += 180;
+                break;
+        }
+    }
+
+    // Get the angle as a number between 0 and 360 degrees.
+    total_angle = total_angle % 360;
+
+    // Replace all of those turns with a single one.
+    switch (total_angle) {
+        case 0:
+            path[path_length - 3] = 'S';
+            break;
+        case 90:
+            path[path_length - 3] = 'R';
+            break;
+        case 180:
+            path[path_length - 3] = 'B';
+            break;
+        case 270:
+            path[path_length - 3] = 'L';
+            break;
+    }
+
+    // The path is now two steps shorter.
+    path_length -= 2;
+}
+
+void turn(char dir) {
+    float a=0.0347;
+    switch (dir) {
+        case 'L':
+            // Turn left.
+            m3pi_play("T240ec");
+            m3pi.left(0.2);
+            wait(0.3125+a);
+            break;
+        case 'R':
+            // Turn right.
+            m3pi_play("T240ce");
+            m3pi.right(0.2);
+            wait(0.3125+a);
+            break;
+        case 'B':
+            // Turn around.
+            m3pi_play("T240cc");
+
+            m3pi.left(0.2);
+            wait(0.625+a);
+            break;
+        case 'S':
+            // Don't do anything!
+            break;
+    }
+}
+
+void turn2(char dir) {
+    float a=0.0347;
+    switch (dir) {
+        case 'L':
+            // Turn left.
+            m3pi_play("T240ec");
+            m3pi.right_motor(0.0);
+            m3pi.left_motor(0.6);
+            wait(0.223);
+
+            break;
+        case 'R':
+            // Turn right.
+            m3pi_play("T240ce");
+            m3pi.right_motor(0.6);
+            m3pi.left_motor(0);
+            wait(0.223);
+            break;
+        case 'B':
+            // Turn around.
+            m3pi_play("T240cc");
+
+            m3pi.left(0.2);
+            wait(0.625+a);
+            break;
+        case 'S':
+            // Don't do anything!
+            m3pi.forward(0.3);
+            wait(0.2+a);
+            break;
+    }
+}
+
+
+
+int main() {
+    unsigned int sensors[5];
+
+    sw.mode(PullUp);
+    m3pi.locate(0,1);
+    m3pi.printf("LineMaze");
+    m3pi.leds(0);
+    wait(2.0);
+    unsigned char ll=0;
+
+    m3pi.sensor_auto_calibrate();
+    while (1) {
+        follow_segment();
+        m3pi.left_motor(0.2);
+        m3pi.right_motor(0.2);
+        wait(0.02);
+        //m3pi.left_motor(0);m3pi.right_motor(0);
+        unsigned char found_left=0;
+        unsigned char found_straight=0;
+        unsigned char found_right=0;
+        ll=0;
+        read_line(sensors);
+        if (sensors[0] > 1000) {
+            found_left = 1;
+            ll=ll|0x04;
+        }
+        if (sensors[4] > 1000) {
+            found_right = 1;
+            ll=ll|0x01;
+        }
+        wait(0.186);
+        read_line(sensors);
+        if (sensors[1] > 1000 || sensors[2] > 1000 || sensors[3] > 1000) {
+            found_straight = 1;
+            ll=ll|0x02;
+        }
+        if (sensors[1] > 1000 && sensors[2] > 1000 && sensors[3] > 1000) {
+            ll=ll|0x08;
+            break;
+//        return;
+        }
+        unsigned char dir = select_turn(found_left, found_straight, found_right);
+        turn(dir);
+        path[path_length] = dir;
+        path_length ++;
+
+// You should check to make sure that the path_length does not
+// exceed the bounds of the array.  We'll ignore that in this
+// example.
+
+// Simplify the learned path.
+        simplify_path();
+        m3pi.leds(ll);
+    }
+    m3pi.stop();
+
+    while (1) {
+        if (sw==0)break;
+    }
+    wait(1);
+    //2nd loop
+    int i;
+    for (i=0; i<path_length; i++) {
+        // SECOND MAIN LOOP BODY
+        follow_segment2();
+// Drive straight while slowing down, as before.
+//        m3pi.left_motor(0.2);
+//        m3pi.right_motor(0.2);
+//        wait(0.02);
+//        read_line(sensors);
+//        wait(0.186);
+
+
+// Make a turn according to the instruction stored in
+// path[i].
+        turn2(path[i]);
+
+    }
+
+    // Follow the last segment up to the finish.
+    follow_segment2();
+
+    m3pi.stop();
+
+
+}
+void follow_segment() {
+    unsigned int sensors[5];
+    float right;
+    float left;
+    float current_pos_of_line = 0.0;
+    float previous_pos_of_line = 0.0;
+    float derivative,proportional,integral = 0;
+    float power;
+    float speed = MAX;
+    int k=0;
+    while (1) {
+
+        // Get the position of the line.
+        current_pos_of_line = m3pi.line_position();
+        proportional = current_pos_of_line;
+
+        // Compute the derivative
+        derivative = current_pos_of_line - previous_pos_of_line;
+
+        // Compute the integral
+        integral += proportional;
+
+        // Remember the last position.
+        previous_pos_of_line = current_pos_of_line;
+
+        // Compute the power
+        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+
+        // Compute new speeds
+        right = speed+power;
+        left  = speed-power;
+
+        // limit checks
+        if (right < MIN)
+            right = MIN;
+        else if (right > MAX)
+            right = MAX;
+
+        if (left < MIN)
+            left = MIN;
+        else if (left > MAX)
+            left = MAX;
+
+        // set speed
+        m3pi.left_motor(left);
+        m3pi.right_motor(right);
+        //Read Sensor
+        if (k>200) {
+            read_line(sensors);
+            if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) {
+                // There is no line visible ahead, and we didn't see any
+                // intersection.  Must be a dead end.
+                m3pi.leds(0x07);
+                return;
+            } else if (sensors[0] > 1000 || sensors[4] > 1000) {
+                // Found an intersection.
+                m3pi.leds(0x0f);
+                return;
+            }
+        }
+        k++;
+    }
+}
+void follow_segment2() {
+    unsigned int sensors[5];
+    float right;
+    float left;
+    float current_pos_of_line = 0.0;
+    float previous_pos_of_line = 0.0;
+    float derivative,proportional,integral = 0;
+    float power;
+    float speed = MAX2;
+    int k=0;
+    while (1) {
+
+        // Get the position of the line.
+        current_pos_of_line = m3pi.line_position();
+        proportional = current_pos_of_line;
+
+        // Compute the derivative
+        derivative = current_pos_of_line - previous_pos_of_line;
+
+        // Compute the integral
+        integral += proportional;
+
+        // Remember the last position.
+        previous_pos_of_line = current_pos_of_line;
+
+        // Compute the power
+        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+
+        // Compute new speeds
+        right = speed+power;
+        left  = speed-power;
+
+        // limit checks
+        if (right < MIN)
+            right = MIN;
+        else if (right > MAX2)
+            right = MAX2;
+
+        if (left < MIN)
+            left = MIN;
+        else if (left > MAX2)
+            left = MAX2;
+
+        // set speed
+        m3pi.left_motor(left);
+        m3pi.right_motor(right);
+        //Read Sensor
+        if (k>100) {
+            read_line(sensors);
+            if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) {
+                // There is no line visible ahead, and we didn't see any
+                // intersection.  Must be a dead end.
+                m3pi.leds(0x07);
+                return;
+            } else if (sensors[0] > 1000 || sensors[4] > 1000) {
+                // Found an intersection.
+                m3pi.leds(0x0f);
+                return;
+            }
+        }
+        k++;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Jun 26 03:02:06 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e