File content as of revision 9:87df9f0ec367:

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

DigitalOut  motor_mode(D9);
DigitalOut  LED(D10);
Motor       motorL(D5, D6);
Motor       motorR(D3, D4);
InterruptIn pg1(D12);
InterruptIn pg2(D11);
AnalogIn    reflectorFL(A6);
AnalogIn    reflectorFM(A3);
AnalogIn    reflectorFR(A2);
AnalogIn    reflectorBL(A1);
AnalogIn    reflectorBR(A0);

Thread thread_trace;
Thread thread_motor;

const float INTVAL_REFLECTOR(0.01);//0.01);
const float INTVAL_MOTOR(0.01);

float dist = 0.0;
float x = 0.0;
float y = 0.0;
float fast_rpm = 0;
float standard_rpm = 0;
float slow_rpm = 0;

int curve = 0;

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

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

void line_trace()
{
    //const float fast_rpm = 0;
    //const float standard_rpm = 0;
    //const float slow_rpm = 0;
    static float gap_pre = 0.0;
    const float KP = 700;
    const float KD = 100;
    const int   L = 0;
    const int   R = 1;

    int flag;

    motorL.Set_phase(L);
    motorR.Set_phase(R);

    while (1)
    {
        if(true)//curve==0)
        {
            if(reflectorFL > 0.1)
            {
                flag = L;
            }
            if(reflectorFR > 0.1)
            {
                flag = R;
            }
            
            if( reflectorFM<=0.4 )
            {      
                if( flag == L )
                {
                   motorL.Set_phase(L);
                   motorL.Set_target(0);
                   motorR.Set_phase(R);
                   motorR.Set_target(standard_rpm);
                }
                else
                {
                   motorL.Set_phase(L);
                   motorL.Set_target(standard_rpm);
                   motorR.Set_phase(R);
                   motorR.Set_target(0);
                }  
            }
            else
            {
                if( reflectorFL>0.1 && reflectorFR>0.1 )
                {
                    motorL.Set_phase(L);
                    motorL.Set_target(fast_rpm);
                    motorR.Set_phase(R);
                    motorR.Set_target(fast_rpm);
                }
                else
                {
                    if( reflectorBL.read() >= reflectorBR.read() )
                    {
                        float voltage_gap = reflectorBL.read() - reflectorBR.read();
                        motorR.Set_phase(R);
                        motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        motorL.Set_phase(L);
                        motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        gap_pre = voltage_gap;
                    }
                    else
                    {
                        float voltage_gap = reflectorBR.read() - reflectorBL.read();
                        motorR.Set_phase(R);
                        motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        motorL.Set_phase(L); 
                        motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        gap_pre = voltage_gap;
                    }
                }
            }
        }
        else
        {
            if( reflectorFM<=0.4 )
            {      
                if( flag == L )
                {
                   motorL.Set_phase(L);
                   motorL.Set_target(0);
                   motorR.Set_phase(R);
                   motorR.Set_target(standard_rpm);
                }
                else
                {
                   motorL.Set_phase(L);
                   motorL.Set_target(standard_rpm);
                   motorR.Set_phase(R);
                   motorR.Set_target(0);
                }  
            }
            else
            {
                if( reflectorFL>0.1 && reflectorFR>0.1 )
                {
                    motorL.Set_phase(L);
                    motorL.Set_target(fast_rpm);
                    motorR.Set_phase(R);
                    motorR.Set_target(fast_rpm);
                }
                else
                {
                    if( reflectorBL.read() >= reflectorBR.read() )
                    {
                        float voltage_gap = reflectorBL.read() - reflectorBR.read();
                        motorR.Set_phase(R);
                        motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        motorL.Set_phase(L);
                        motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        gap_pre = voltage_gap;
                    }
                    else
                    {
                        float voltage_gap = reflectorBR.read() - reflectorBL.read();
                        motorR.Set_phase(R);
                        motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        motorL.Set_phase(L); 
                        motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre));
                        gap_pre = voltage_gap;
                    }
                }
            }
        }
        
        //printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag);
        wait(INTVAL_REFLECTOR);
    }
}

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<=6850)
    {
        fast_rpm     = 5000;
        standard_rpm = 4500;
        curve = 0;
    }
    else if(dist<=7350)
    {
        fast_rpm = 4000;
        standard_rpm = 4000;
        curve = 0;
    }
    else
    {
        fast_rpm     = 5000;
        standard_rpm = 4500;
        curve = 0;
    }

    //printf("%f\n", dist);

    if (dist_count >= 200) 
    {
        LED = !LED;
        dist_count -= 200;
    }
}

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