File content as of revision 2:09ea66e396c1:

#include "mbed.h"
#include "motor.h"

DigitalOut  motor_mode(D9);
DigitalOut  LED(A6);
Motor       motorL(D5, D6);
Motor       motorR(D3, D4);
InterruptIn pg1(D12);
InterruptIn pg2(D11);
AnalogIn    reflectorF(A2);
AnalogIn    reflectorL(A1);
AnalogIn    reflectorR(A0);

Thread thread_trace;
Thread thread_motor;

const int   THREAD_FLAG_TRACE(1);
const int   THREAD_FLAG_MOTOR(2);
const float INTVAL_REFLECTOR (0.01);
const float INTVAL_MOTOR     (0.25);

float dist = 0.0;
float standard_rpm = 0;

void count_pg1()
{
    motorL.count();
}

void count_pg2()
{
    motorR.count();
} 

void line_trace()
{
    //const float fast_rpm = 2000;
    //const float standard_rpm = 1200;
    //const float slow_rpm = 1000;
    const float K = 750;//850;
    const int   L = 0;
    const int   R = 1;
    
    int flag;
    
    motorL.Set_phase(0);
    motorR.Set_phase(1);
    
    while(1)
    {
        if( /*reflectorF<=0.4 &&*/ reflectorR<=0.4 && reflectorL<= 0.4 )
        {      
            if( flag == L )
            {
               motorL.Set_phase(0);//1);
               motorL.Set_target(0);//slow_rpm);
               motorR.Set_phase(1);
               motorR.Set_target(standard_rpm);//slow_rpm);
               //printf("turnL\n");
            }
            else
            {
               motorL.Set_phase(0);
               motorL.Set_target(standard_rpm);//slow_rpm);
               motorR.Set_phase(1);//0);
               motorR.Set_target(0);//slow_rpm);
               //printf("turnR\n");
            }  
        }
        else
        {
            if( reflectorF > 0.4 )
            {
                motorL.Set_phase(0);
                motorL.Set_target(fast_rpm);
                motorR.Set_phase(1);
                motorR.Set_target(fast_rpm);
            }
            else
            {
                if( reflectorL.read() >= reflectorR.read() )
                {
                    float voltage_gap = reflectorL.read() - reflectorR.read();
                    motorR.Set_phase(1);
                    motorR.Set_target(standard_rpm + K * voltage_gap);
                    motorL.Set_phase(0);
                    motorL.Set_target(standard_rpm - K * voltage_gap);
                    flag = L;
                }
                else
                {
                    float voltage_gap = reflectorR.read() - reflectorL.read();
                    motorR.Set_phase(1);
                    motorR.Set_target(standard_rpm - K * voltage_gap);
                    motorL.Set_phase(0); 
                    motorL.Set_target(standard_rpm + K * voltage_gap);
                    flag = R;
                }
            }
            wait(INTVAL_REFLECTOR);
        }
        
        //printf("F: %f L: %f R: %f flag: %d \n", reflectorF.read(), reflectorL.read(), reflectorR.read(), flag);
    }
}

void odometry()
{
    const float pi         = 3.14159265359;
    const float gear_ratio = 1/38.2;
    const float tire_size  = 57.0/2;
    
    static float v_pre      = 0.0;
    static float dist_count = 0.0;

    float vl = motorL.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
    float vr = motorR.Get_rpm()/60 * gear_ratio * 2 * pi * tire_size;
    float v  = (vl + vr)/2;
    
    dist       += ( v + v_pre ) * INTVAL_MOTOR/2;
    dist_count += ( v + v_pre ) * INTVAL_MOTOR/2;
    v_pre = v;
    
    if     (dist<=2000) standard_rpm = 2000;
    else if(dist<=2400) standard_rpm = 1200;
    else if(dist<=4400) standard_rpm = 2000;
    else                standard_rpm = 1000;
    
    printf("%f\n", dist);
    
    if( dist_count >= 200 )
    {
        LED = !LED;
        dist_count -= 200;
    }
    
    if(dist>2147483000)
    {
        dist = 0.0;
    }
}

void control_motor()
{
    while(1)
    {
        motorL.drive();
        motorR.drive();
        odometry();
        wait(INTVAL_MOTOR);
    }
}   

int main()
{   
    motor_mode = 1;
    LED = 1;

    pg1.rise(&count_pg1);
    pg1.fall(&count_pg1);
    pg2.rise(&count_pg2);
    pg2.fall(&count_pg2);
    
    thread_trace.start(callback(line_trace));
    thread_motor.start(callback(control_motor));
}