File content as of revision 1:f4db9b0bb5e2:

#include "mbed.h"
#include "m3pimaze.h"

BusOut leds(LED1,LED2,LED3,LED4);
m3pi m3pi(p23,p9,p10);
char path[1000] = "";
unsigned char path_length = 0; // the length of the path so far
int sensors[5];
DigitalIn pb(p21);

// Minimum and maximum motor speeds
#define MAX 0.25
#define MIN 0

// PID terms
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

/*=================================================================
            FOLLOW LINE
===================================================================*/
void follow_line()
 {

    m3pi.locate(0,1);
    m3pi.cls();
    m3pi.printf("GO!!");

    //wait(2.0);

    //m3pi.sensor_auto_calibrate();//don't want this here.

    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 foundjunction=0;
    int countdown=100; //make sure we don't stop for a junction too soon after starting
    while (foundjunction==0)
     {

        // 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);
        
        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] < 400) && (sensors[2] < 400) && (sensors[3] < 400))
        {
            // There is no line visible ahead, and we didn't see any
            // intersection.  Must be a dead end.
            foundjunction=1;
            return;
        }
            else if((sensors[0] > 700) || (sensors[4] > 700))
            {
                // Found an intersection.
                foundjunction=1;
                return;
            }
                else foundjunction=0;
                  
}}}
/*===========================================================================================================
            TURN
=============================================================================================================*/
// 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';
}
/*============================================================================================================
            DO TURN
==============================================================================================================*/

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;
}
/*=============================================================================================
            SIMPLIFY
===============================================================================================*/
// 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;
}
/*==============================================================================================================
            PRESSED
================================================================================================================*/
void pressed()
{
     
    // wait 15s to give time to turn off, or put the robot back to the start
    wait(2);
    // 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.2);
            wait(0.2);

            // Make a turn according to the instruction stored in
            // path[i].
            doturn(path[i]);
        }
        
        // Follow the last segment up to the finish.
        follow_line();
        m3pi.forward(0.2);
             wait(0.6);
       return;

        // Now we should be at the finish!  Restart the loop.
    
}

/*========================================================================================================
        MAZESOLVE
==========================================================================================================*/
// 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;
        unsigned char found_back=0;
    int sensors[5];
    // Loop until we have solved the maze.
    while(1)
    {
    
        // Follow the line until an intersection is detected
        follow_line();

        // Bump forward a bit in case sensor was triggered at an angle
        m3pi.forward(0.1);
             wait(0.1);
        
        found_left=0;found_forward=0;found_right=0;
        
         m3pi.readsensor(sensors);//sensors are turned off outside the follow line function so we must call sensors eachtime they are needed
        
         // Check for left and right exits.
        if((sensors[1]<700) && (sensors [2]<700) && (sensors[3]<700))
        {
            found_back=1;
        }
            
         if(sensors[0] > 700)
            {
             // Drive straight a bit more - this is enough to line up our
             // wheels with the intersection.
             m3pi.forward(0.2);
             wait(0.2);
              m3pi.readsensor(sensors);//what's beyond the intersection
             // Check for the ending spot.
             // If all five sensors are on dark black, we have
             // solved the maze.
             if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900))
                {
                //move upon the home pad to show off
                    m3pi.forward(0.2);
                    wait(0.4);
                    m3pi.printf("Finished");
                    break;
                }
                    else found_left = 1; 
                    
            }
                 
        else if(sensors[4] > 700 )
            {
            //move wheels to intersection
             m3pi.forward(0.2);
             wait(0.2);
             //what is past the intersection
              m3pi.readsensor(sensors);
             // Check for the ending spot.
             // If all five sensors are on dark black, we have
             // solved the maze.
             if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900))
                {
                    m3pi.forward(0.2);
                    wait(0.4);
                    m3pi.printf("Finished");
                    break;
                }
                //can we go forward
             else if((sensors[1] > 700 )|| (sensors[2] > 700) || (sensors[3] > 700))
                {
                found_forward = 1;
                
                }
                //then go right
             else 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");
        if (found_back==1)
           m3pi.printf("B");
        //wait (3);
               

        unsigned char dir = turn(found_left, found_forward, found_right);

        // Make the turn indicated by the path.
        //doturn(dir);
        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);
     pb.mode(PullUp);
       
        if(pb)
        {
            do
            {
           
            }
            while(pb);
            pressed();
           }                 
        
    }
       

}

/*========================================================================================================
                    MAIN
=========================================================================================================*/
int main() {
 //  int sensors[5]; 
    m3pi.locate(0,1);
    m3pi.sensor_auto_calibrate();
    m3pi.printf("MazeSolve");

    wait(2.0);

  mazesolve();

m3pi.forward(0.0);
}