File content as of revision 0:22b8909431a3:

#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','s','s','s','s'};
int cmd_i=1;
int cmd_i_ant=0;
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;
//float speed=0.2;



void flip(void)
{
    if (rn42.readable()) {
        cmd_i_ant++;
        cmd_i_ant=cmd_i_ant%5;
        cmd_i++;
        cmd_i=cmd_i%5;
        cmd[cmd_i]=rn42.getc();
        speed_flag=0;
        display_flag=0;
        menu_flag=0;
        marca_detectada=0;
    }
    //return;
}

int main()
{

    m3pi.sensor_auto_calibrate();
    rn42.baud(115200);

    rn42.attach(&flip,Serial::RxIrq);

    m3pi.cls();

    while (1) {
        //if (rn42.readable()) {
        //cmd=rn42.getc();
        //rn42.putc(cmd);
        // m3pi.printf("v= %c", cmd);

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


        m3pi.locate(0,0);
        if(cmd[cmd_i]=='w') {
            m3pi.left_motor(speed);
            m3pi.right_motor(speed);
        } else if(cmd[cmd_i]=='d') {
            m3pi.left_motor(0.0);
            m3pi.right_motor(speed);
        } else if(cmd[cmd_i]=='a') {
            m3pi.left_motor(speed);
            m3pi.right_motor(0.0);
        } else if(cmd[cmd_i]=='x') {
            m3pi.backward(speed);
        } else if(cmd[cmd_i]=='+') {
            if(speed_flag==0) {
                speed_flag=1;
                speed+=0.1;
                if(speed>1.0)
                    speed=1.0;
            }
        } else if(cmd[cmd_i]=='-') {
            if(speed_flag==0) {
                speed_flag=1;
                speed-=0.1;
                if(speed<-1.0)
                    speed=-1.0;
            }
        } 
        else if(cmd[cmd_i]=='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" );
            cmd_i--;
            if(cmd_i<0)
                cmd_i=4;
            cmd_i_ant--;
            if(cmd_i_ant<0)
                cmd_i_ant=4;
            }
        }else if(cmd[cmd_i]=='z') {
            //wait(2.0);

            do {
                //if (rn42.readable()) {
                // cmd=rn42.getc();
                //rn42.putc(cmd);}
                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) {
                        if(cmd[cmd_i_ant]=='q'){
                            num_marcas--;
                            if(num_marcas<=0){
                                cmd_i_ant++;
                                cmd_i_ant=cmd_i_ant%5;
                                cmd_i++;
                                cmd_i=cmd_i%5;
                                cmd[cmd_i]='s';
                            }
                        }
                        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[cmd_i]!='s');
            m3pi.left_motor(0.0);
            m3pi.right_motor(0.0);
            m3pi.cls();
            m3pi.printf("saiu");
            m3pi.locate(0,0);
        }  else if(cmd[cmd_i_ant]=='q'){
            num_marcas=atoi(&cmd[cmd_i]);
            cmd[cmd_i]='z';
        }
        else {
            m3pi.left_motor(0.0);
            m3pi.right_motor(0.0);
        }
        sleep();
    } 
}