File content as of revision 0:c327ac576b6a:

#include "mbed.h"
#include "m3pi.h"

#define MAX 0.4
#define MIN 0
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

AnalogIn   ain(p20);
Serial rn42(p28,p27);
m3pi m3pi;

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;
float obstaculo;
char cmd='s';
int sensors[5];
int marcas=0;
int marca_detectada=0;
int speed_flag=0;
int display_flag=0;
int menu_flag=0;
int num_marcas=0;
char cmd_ant='s';




void flip(void)
{
    if (rn42.readable()) {
        cmd_ant=cmd;
        cmd=rn42.getc();
        speed_flag=0;
        display_flag=0;
        menu_flag=0;
        marca_detectada=0;
    }
}

int main()
{

    m3pi.sensor_auto_calibrate();
    rn42.baud(115200);
    rn42.attach(&flip,Serial::RxIrq);
    m3pi.cls();

    while (1) {

        if(display_flag==0) {
            display_flag=1;
            rn42.putc( cmd );
            m3pi.cls();
            m3pi.printf("%c", cmd);
        }

        m3pi.locate(0,0);
        if(cmd=='w') {
            m3pi.left_motor(speed);
            m3pi.right_motor(speed);
        } else if(cmd=='d') {
            m3pi.left_motor(0.0);
            m3pi.right_motor(speed);
        } else if(cmd=='a') {
            m3pi.left_motor(speed);
            m3pi.right_motor(0.0);
        } else if(cmd=='x') {
            m3pi.backward(speed);
        } else if(cmd=='+') {
            if(speed_flag==0) {
                speed_flag=1;
                speed+=0.1;
                if(speed>1.0)
                    speed=1.0;
            }
        } else if(cmd=='-') {
            if(speed_flag==0) {
                speed_flag=1;
                speed-=0.1;
                if(speed<-1.0)
                    speed=-1.0;
            }
        } else if(cmd=='m') {
            if(menu_flag==0) {
                menu_flag=1;
                rn42.printf( "\nControlo Manual:\n\tw: Frente\n\tx:Tras1\n\td:Direita1\n\ta:Esquerda\n\nControlo Automatico:\n\tz: Activar\n\tx: Ir ate marca numero x\n" );
            }
        } else if(cmd=='z') {

            do {
                obstaculo=ain.read();
                m3pi.readsensor(sensors);
                if(((sensors[0]>900 && sensors[1]>900 && sensors[2]>900 && sensors[3]>900) || (sensors[1]>900 && sensors[2]>900 && sensors[3]>900 && sensors[4]>900))) {
                    if(marca_detectada==0) {
                        marcas++;
                        marca_detectada=1;
                    }
                } else {
                    marca_detectada=0;
                }

                m3pi.locate(0,0);
                m3pi.printf("m: %d", marcas);


                if(obstaculo<0.025)
                    m3pi.stop();
                else {
                    // 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.aas
                    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);
                }
            } while(cmd!='s');
            m3pi.left_motor(0.0);
            m3pi.right_motor(0.0);
            m3pi.cls();
            m3pi.printf("saiu");
            m3pi.locate(0,0);
        } else if(cmd=='s') {
            m3pi.left_motor(0.0);
            m3pi.right_motor(0.0);
        }
    }
}