File content as of revision 1:092dad68a533:

#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 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);

       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;
        }
        
        // you need to put some code here to look for a left or right junction
        
        
        }
  
     

}

// 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];

        // Follow the line until an intersection is detected
        follow_line();

      
        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;

        // Check for left and right exits.

        
        //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);
}

 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);

}