Diff: main.cpp

Revision:

0:6e52eae6412a

Child:

1:3ac7462953df

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 03 09:58:58 2011 +0000
@@ -0,0 +1,337 @@
+#include "mbed.h"
+#include "m3pijon.h"
+
+BusOut leds(LED1,LED2,LED3,LED4);
+m3pi m3pi(p23,p9,p10);
+
+#define MAX 0.5
+#define MIN 0
+
+
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+// Global variables
+// The path array stores the route info. Each element shows what we did at an intersection
+
+//  'L' for left
+//  'R' for right
+//  'F' for forward
+//  'B' for back 
+//
+char path[1000] = "";
+unsigned char path_length = 0; // the length of the path so far
+ 
+
+void follow_line()
+{
+    float right;
+    float left;
+    float position_of_line = 0.0;
+    float prev_pos_of_line = 0.0;
+    float derivative,proportional;
+    float integral = 0;
+    float power;
+    float speed = MAX;
+    int foundjunction=0;
+    int countdown=150; //make sure we don't stop for a junction too soon after starting
+
+    int sensors[5];    
+    while (foundjunction==0) {
+
+        // Get the position of the line.
+        position_of_line = m3pi.line_position();
+        proportional = position_of_line;
+        // Compute the derivative
+        derivative = position_of_line - prev_pos_of_line;
+        // Compute the integral
+        integral += proportional;
+         // Remember the last position.
+        prev_pos_of_line = position_of_line;
+        // Compute
+        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);
+
+    if (countdown>0) countdown--; else {
+       // Next, we are going to use the sensors to look for whether there is still a line ahead
+       // and try to detect dead ends and possible left or right turns.
+       m3pi.readsensor(sensors);
+      
+        if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100)
+        {
+            // There is no line visible ahead, and we didn't see any
+            // intersection.  Must be a dead end.
+            foundjunction=1;
+        }
+        else if(sensors[0] > 200 || sensors[4] > 200)
+        {
+            // Found an intersection.
+            foundjunction=1;
+        }
+       } //else countdown
+     } //while
+// straighten up a bit, by steering opposite direction
+// not sure if this is needed
+//        m3pi.left_motor(right);
+//        m3pi.right_motor(left);
+//       wait(0.02);
+
+}
+
+// This function decides which way to turn during the learning phase of
+// maze solving.  It uses the variables found_left, found_straight, and
+// found_right, which indicate whether there is an exit in each of the
+// three directions, applying the "left hand on the wall" strategy.
+
+char turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right)
+{
+    // The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm
+    if(found_left)
+        return 'L';
+    else if(found_forward)
+        return 'F';
+    else if(found_right)
+        return 'R';
+    else
+        return 'B';
+}
+
+void doturn(unsigned char dir)
+{
+    if (dir=='L')
+        {m3pi.left(0.25);wait(0.28);}   
+     else if(dir=='R')
+      {m3pi.right(0.25);wait(0.28);}  
+     else if(dir=='F')
+      {m3pi.forward(0.3);wait(0.15);}
+     else if(dir=='B')
+      {m3pi.right(0.25);wait(0.6);}
+       
+       m3pi.forward(0.1);wait(0.1);m3pi.forward(0);
+        return;
+}
+
+// change LBL to S (etc), to bypass dead ends
+void simplify()
+{
+    // 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] = 'F';
+        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;
+}
+
+// This function is called once, from main.c.
+void mazesolve()
+{
+    // These variables record whether the robot has seen a line to the    
+    // left, straight ahead, and right, while examining the current
+    // intersection.
+        unsigned char found_left=0;
+        unsigned char found_forward=0;
+        unsigned char found_right=0;
+    int sensors[5];
+    // Loop until we have solved the maze.
+    while(1)
+    {
+    
+        // Follow the line until an intersection is detected
+        follow_line();
+
+        // Inch forward a bit.  This helps us in case we entered the
+        // intersection at an angle.
+        found_left=0;found_forward=0;found_right=0;
+     //   m3pi.forward(0.1);
+       // wait(0.05);
+        
+        // Now read the sensors and check the intersection type.
+
+ 
+      
+        m3pi.forward(0.0);    
+      
+        // Check for a forward exit.
+        m3pi.readsensor(sensors);
+        if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
+            found_forward = 1;
+        
+               m3pi.readsensor(sensors);
+
+        // Check for left and right exits.
+        if(sensors[0] > 200)
+            found_left = 1;
+        if(sensors[4] > 200)
+            found_right = 1;
+        
+        //debug code
+        m3pi.cls();
+        if (found_left==1)
+           m3pi.printf("L");
+        if (found_right==1)
+           m3pi.printf("R");
+        if (found_forward==1)
+           m3pi.printf("F");
+        wait (3);
+        // Check for the ending spot.
+        // If all five sensors are on dark black, we have
+        // solved the maze.
+        if(sensors[0]>600 && sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600 && sensors[4]>600)
+            break;
+
+        // Drive straight a bit more - this is enough to line up our
+        // wheels with the intersection.
+        m3pi.forward(0.15);
+        wait(0.02);  
+
+        unsigned char dir = turn(found_left, found_forward, found_right);
+
+        // Make the turn indicated by the path.
+        doturn(dir);
+
+        // Store the intersection in the path variable.
+        path[path_length] = dir;
+        path_length ++;
+
+        // Need to insert check to make sure that the path_length does not
+        // exceed the bounds of the array.  
+
+        // Simplify the learned path.
+        simplify();
+
+    }
+
+    // Solved the maze!
+
+    // Now enter an infinite loop - we can re-run the maze as many
+    // times as we want to.
+    while(1)
+    {
+
+        m3pi.forward(0.0);
+        m3pi.printf("Finished");
+
+    // wait 15s to give time to turn off, or put the robot back to the start
+    wait(15);
+    // ideally we would use a button press here       
+    // but I don't think it can easily be read
+
+        // Re-run the maze.  It's not necessary to identify the
+        // intersections, so this loop is really simple.
+        int i;
+        for(i=0;i<path_length;i++)
+        {
+             
+            follow_line();
+
+            // Drive straight while slowing down
+            //m3pi.forward(0.5);
+            //wait(0.05);
+            m3pi.forward(0.3);
+            wait(0.1);
+
+            // Make a turn according to the instruction stored in
+            // path[i].
+            doturn(path[i]);
+        }
+        
+        // Follow the last segment up to the finish.
+        follow_line();
+
+        // Now we should be at the finish!  Restart the loop.
+    }
+}
+
+ void checksensors()
+{
+int sensors[5];
+while (1) {
+      m3pi.readsensor(sensors);
+        m3pi.cls();
+        if (sensors[0]>200)
+           m3pi.printf("D");
+           else  m3pi.printf("L");
+        if (sensors[1]>200)
+           m3pi.printf("D");
+           else  m3pi.printf("L");     
+             if (sensors[2]>200)
+           m3pi.printf("D");
+           else  m3pi.printf("L");
+             if (sensors[3]>200)
+           m3pi.printf("D");
+           else  m3pi.printf("L");
+             if (sensors[4]>200)
+           m3pi.printf("D");
+           else  m3pi.printf("L");
+    }
+}
+int main() {
+ //  int sensors[5]; 
+    m3pi.locate(0,1);
+    m3pi.sensor_auto_calibrate();
+    m3pi.printf("MazeSolve");
+
+    wait(2.0);
+
+  mazesolve();
+
+m3pi.forward(0.0);
+
+}