File content as of revision 0:64f17142aa89:

#include "mbed.h"
#include "Motor.h"
#include "rtos.h"
#include "stdio.h"
#include "MFRC522.h"

Motor m_r(p24, p17, p18); // pwm, fwd, rev
Motor m_l(p25, p20, p19); // pwm, fwd, rev

MFRC522 RfChip(p5, p6, p7, p8, p9);

AnalogIn IR_right(p15);
AnalogIn IR_left(p16);
DigitalOut ledL(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut ledR(LED4);

RawSerial  pc(USBTX, USBRX);
RawSerial  blue(p28,p27);
DigitalOut led1(LED1);
DigitalOut led4(LED4);

char bnum=0;
char bhit=0;

// Flags for the buttons
int flagFW, flagRight, flagLeft, flagRev;
int flag1, flag2, flag3, flag4;
int flag_autom;

// IR detectors
float ainR, ainL;

// Speed of the robot
float s_max = 0.5;
float s_min = 0.0;

// mutex for the flag that switches mode
Mutex autom_mutex;

// ID of tags - We only compare the 1st element
int white_tag = 115;
int blue_tag = 106;
int tag;


// ------------------- Thread RFID ------------------------//
void thread_RFID(void const *args){
    while (true) {
    
        // Look for new cards
        if ( ! RfChip.PICC_IsNewCardPresent()){
          Thread::wait(200);
          continue;
        }
    
        // Select one of the cards
        if ( ! RfChip.PICC_ReadCardSerial()){
          Thread::wait(200);
          continue;
        }
    
        led2 = 0;
    
        // Print Card UID
        pc.printf("Card UID: ");
        for (uint8_t i = 0; i < RfChip.uid.size; i++){
            //pc.printf(" %X02", RfChip.uid.uidByte[i]);
            pc.printf(" %i", RfChip.uid.uidByte[i]);
            
            // Get the 1st element of the ID
            if(i == 0){
                tag = RfChip.uid.uidByte[i];
            }
        }
        
        pc.printf("\n\r");
        pc.printf("ID = %i\n\r", tag);
    
        if(tag == white_tag){
            autom_mutex.lock();
            flag_autom = 0;
            autom_mutex.unlock();
            pc.printf("Controlled mode\n\r");
        }
        else if(tag == blue_tag){
            autom_mutex.lock();
            flag_autom = 1;
            autom_mutex.unlock();
            pc.printf("Automatic mode\n\r");
        }
        
    
        // Print Card type
        uint8_t piccType = RfChip.PICC_GetType(RfChip.uid.sak);
        pc.printf("PICC Type: %s \n\r", RfChip.PICC_GetTypeName(piccType));
        
        Thread::wait(10); 
    }    
}


// ------------------- Thread Motor ------------------------//
void thread_Motor(void const *args){
    while (true) {
              
        m_r.speed(0); 
        m_l.speed(0);
        
        ainR = IR_right;
        ainL = IR_left;
        
        int autom;
        autom_mutex.lock();
        autom = flag_autom;
        autom_mutex.unlock();
        
        // ------- Control mode ------------------
        if(autom == 0){
                    
            ledL = led2 = led3 = ledR = 0;
               
            // Push FW button 1st      
            while(flagFW == 1){
                ledL =1;
                led2 = led3 = ledR = 0;
                
                if(flagLeft == 1){
                    led2 = 1;
                    m_r.speed(s_max); 
                    m_l.speed(0);
                }
                if(flagRight == 1){
                    led3 = 1;
                    m_r.speed(0); 
                    m_l.speed(s_max);
                }
                if(flagRev == 1){
                    ledR=1;
                    m_r.speed(s_min); 
                    m_l.speed(s_min);
                }
                else{
                    m_r.speed(s_max); 
                    m_l.speed(s_max);
                }
            }
            
            // Push Rev button 1st     
            while(flagRev == 1){
                
                if(flagLeft == 1){
                    m_r.speed(-s_max); 
                    m_l.speed(-s_min);
                }
                if(flagRight == 1){
                    m_r.speed(-s_min); 
                    m_l.speed(-s_max);
                }
                if(flagFW == 1){
                    m_r.speed(-s_min); 
                    m_l.speed(-s_min);
                }
                else{
                    m_r.speed(-s_max); 
                    m_l.speed(-s_max);
                }
            }
            
            // Push Left button 1st    
            while(flagLeft == 1){
                
                if(flagFW == 1){
                    m_r.speed(s_max); 
                    m_l.speed(s_min);
                }
                if(flagRight == 1){
                    m_r.speed(s_min); 
                    m_l.speed(s_min);
                }
                if(flagRev == 1){
                    m_r.speed(-s_max); 
                    m_l.speed(-s_min);
                }
                else{
                    m_r.speed(s_max); 
                    m_l.speed(-s_max);
                }
            }
            
            // Push Right button 1st     
            while(flagRight == 1){
                
                if(flagFW == 1){
                    m_r.speed(s_min); 
                    m_l.speed(s_max);
                }
                if(flagLeft == 1){
                    m_r.speed(s_min); 
                    m_l.speed(s_min);
                }
                if(flagRev == 1){
                    m_r.speed(-s_min); 
                    m_l.speed(-s_min);
                }
                else{
                    m_r.speed(-s_max); 
                    m_l.speed(s_max);
                }
            }
        }   
        
        // ------- Automatic mode ------------------
        while(autom == 1) {
            
            //int autom;
            autom_mutex.lock();
            autom = flag_autom;
            autom_mutex.unlock();
            
            // No obstacle --> FW with
            if((IR_right < 0.2) && (IR_left < 0.2)){
                m_r.speed(s_max); 
                m_l.speed(s_max);
                ledR = 0;
                ledL = 0;
            }
            
            // Obstacle on the right --> turn left and reduce speed
            else if((IR_right >= 0.2) && (IR_left < 0.2)){
                m_r.speed(s_max); 
                m_l.speed(0);
                ainR = IR_right;
                ainL = IR_left;
                printf("Right: %0.2f - Left: %0.2f\n", ainR, ainL);
                ledR = 1;
                ledL = 0; 
            }
            
            // Obstacle on the left --> turn right and reduce speed
            else if((IR_right < 0.2) && (IR_left >= 0.2)){
                m_r.speed(0);  
                m_l.speed(s_max);
                ainR = IR_right;
                ainL = IR_left;
                printf("Right: %0.2f - Left: %0.2f\n", ainR, ainL);
                ledR = 0;
                ledL = 1;
            }
            
            // Obstacle in front --> stop, move back and turn
            else if((IR_right >= 0.2) && (IR_left >= 0.2)){
                m_r.speed(0);  
                m_l.speed(0);
                wait(0.5);    // Stop for 0.5s
                ainR = IR_right;
                ainL = IR_left;
                printf("Right: %0.2f - Left: %0.2f\n", ainR, ainL);
                ledR = 1;
                ledL = 1;
                
                if(IR_left > IR_right){
                    m_r.speed(-s_max); 
                    m_l.speed(-s_max);
                    Thread::wait(500);     // Back for 0.5s
                    
                    m_r.speed(-s_max); 
                    m_l.speed(s_max);
                    Thread::wait(500);     // turn for 0.5s
                }
                else{
                    m_r.speed(-s_max); 
                    m_l.speed(-s_max);
                    Thread::wait(500);     // Back for 0.5s
                    
                    m_r.speed(s_max); 
                    m_l.speed(-s_max);
                    Thread::wait(1000);     // turn for 1s
                }
            }
        } 
                      
        Thread::wait(10);
    }
}


// -------------------- Callback Blue ---------------------- //
void blue_recv(){
    led1 = !led1;
    while(blue.readable()) {
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                pc.putc(bnum);
                pc.putc(bhit);
                pc.printf("\n");
                
                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                    switch (bnum) {
                        case '1': //number button 1
                            if (bhit=='1') {
                                flag1 = 1;
                                s_max = 0.3;
                                pc.printf("Speed max 1: %.2f\n", s_max);
                            } else {
                                flag1 = 0;
                            }
                            break;
                        case '2': //number button 2
                            if (bhit=='1') {
                                flag2 = 1;
                                s_max = 0.6;
                                pc.printf("Speed max 2: %.2f\n", s_max);
                            } else {
                                flag2 = 0;
                            }
                            break;
                        case '3': //number button 3
                            if (bhit=='1') {
                                flag3 = 1;
                                s_max = 0.8;
                                pc.printf("Speed max 3: %.2f\n", s_max);
                            } else {
                                flag3 = 0;
                            }
                            break;
                        case '4': //number button 4
                            if (bhit=='1') {
                                flag4 = 1;
                                s_max = 1.0;
                                pc.printf("Speed max 4: %.2f\n", s_max);
                            } else {
                                flag4 = 4;
                            }
                            break;
                        case '5': //button 5 up arrow
                            if (bhit=='1') {
                                flagFW = 1;
                            } else {
                                flagFW = 0;
                            }
                            break;
                        case '6': //button 6 down arrow
                            if (bhit=='1') {
                                flagRev = 1;
                            } else {
                                flagRev = 0;
                            }
                            break;
                        case '7': //button 7 left arrow
                            if (bhit=='1') {
                                flagLeft = 1;
                            } else {
                                flagLeft = 0;
                            }
                            break;
                        case '8': //button 8 right arrow
                            if (bhit=='1') {
                                flagRight = 1;
                            } else {
                                flagRight = 0;
                            }
                            break;
                        default:
                            break;
                    }              
                }
            }
        }
    }
}


// -------------------- Callback PC ---------------------- //
void pc_recv(){
    led4 = !led4;
    while(pc.readable()) {
        blue.putc(pc.getc());
    }
}


// -------------------- Main ---------------------- //
int main(){
    pc.baud(9600);
    blue.baud(9600);

    pc.attach(&pc_recv, Serial::RxIrq);
    blue.attach(&blue_recv, Serial::RxIrq);
    
    RfChip.PCD_Init();
    
    flag_autom = 0;
    pc.printf("hello babe\n");
    
    Thread t1(thread_RFID);
    Thread t2(thread_Motor);

    while(1) {
        Thread::wait(500);   // wait 0.5s
    }
}