File content as of revision 1:3ac7462953df:

#include "mbed.h"
#include "m3pimaze.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);

}