File content as of revision 7:ce2920da874b:

//本命
#include "mbed.h"
#include "motor.h"
#include "BMX055.h"
#include "way.h"

BMX055      bmx(D0, D1);
DigitalIn   button(D2);
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;
Thread sensor_thread;

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

const float pi = 3.14159265359;
const float gear_ratio = 1 / 38.2;
const float tire_size = 0.57 / 2;

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

bool lock = false;

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 = 850;
    const float KD = 3000;
    const float K  = 0.01;
    const int   L = 0;
    const int   R = 1;

    int flag;

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

    float target_azimuth = 0.0;
    float current_azimuth = 0.0;
    float curvature = 0.0;
    float tread = 0.097;

    while (1)
    {
        /*
        if (reflectorFL>0.15)
        {
            flag=L;
        }
        if (reflectorFR>0.15)
        {
            flag=R;
        }
        
        if ( reflectorFM<=0.4 && reflectorFL<=0.15 && reflectorFR<=0.15 )
        {
            if (flag==L)
            {
                motorL.Set_phase(L);
                motorL.Set_target(0);
                motorR.Set_phase(R);
                motorR.Set_target(slow_rpm);
                printf("turnL\n");
            }
            else
            {
                motorL.Set_phase(L);
                motorL.Set_target(slow_rpm);
                motorR.Set_phase(R);
                motorR.Set_target(0);
                printf("turnR\n");
            }
        }
        else
        {
            if ( reflectorFM>0.4 )
            {
                motorL.Set_phase(L);
                motorL.Set_target(fast_rpm);
                motorR.Set_phase(R);
                motorR.Set_target(fast_rpm);
            }
            else
            {
                if ( reflectorBL>0.15 )
                {
                    motorL.Set_phase(L);
                    motorL.Set_target(standard_rpm - standard_rpm*0.18);
                    motorR.Set_phase(R);
                    motorR.Set_target(standard_rpm + standard_rpm*0.18);
                }
                else
                {
                    motorL.Set_phase(L);
                    motorL.Set_target(standard_rpm + standard_rpm*0.18);
                    motorR.Set_phase(R);
                    motorR.Set_target(standard_rpm - standard_rpm*0.18);
                }
            }
        }*/
        
        float gap_min = 0.0;
        int i_min = 0;
        for (int i = 0; i < 478; i++)
        {
            float dist_gap = dist - way[i][2];
            if (dist_gap < gap_min)
            {
                gap_min = dist_gap;
                i_min = i;
            }
        }
        
        float l    = sqrt( pow((x-way[i_min][0]), 2) + pow((y-way[i_min][1]), 2) );
        float dv   = K*l;
        float drpm = dv*60 / ( gear_ratio * 2 * pi * tire_size );
        
        motorL.Set_target( standard_rpm + drpm );
        motorR.Set_target( standard_rpm - drpm );
        
        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()
{
    static float v_pre = 0.0;
    static float x_dot_pre = 0.0;
    static float y_dot_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;
    float x_dot = v * cos(bmx.get_azimuth_machineframe());
    float y_dot = v * sin(bmx.get_azimuth_machineframe());

    dist += (v + v_pre) * INTVAL_MOTOR / 2;
    dist_count += (v + v_pre) * INTVAL_MOTOR / 2;
    x += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
    y += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2;
    v_pre = v;
    x_dot_pre = x;
    y_dot_pre = y;

    fast_rpm = 1200;
    standard_rpm = 1200;
    slow_rpm = 1200;

    /*
    if     (dist<=2000)
    {
        fast_rpm     = 2500;
        standard_rpm = 2000;
        slow_rpm     = 1200;
    }
    else if(dist<=2400)
    {
        fast_rpm     = 2000;
        standard_rpm = 1500;
        slow_rpm     = 1200;
    }
    else if(dist<=4400)
    {
        fast_rpm     = 2500;
        standard_rpm = 2000;
        slow_rpm     = 1200;
    }
    else
    {
        fast_rpm     = 1500;
        standard_rpm = 1200;
        slow_rpm     = 1200;
    }*/

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

void update()
{
    while (true)
    {
        if (!lock) 
        {
            bmx.Update_posture();
        }
    }
}

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

    pg1.rise(&count_pg1);
    pg1.fall(&count_pg1);
    pg2.rise(&count_pg2);
    pg2.fall(&count_pg2);

    while (true)
    {
        if (button == 0)
        {
            wait(1);
            break;
        }
    }

    bmx.mag_calibration(button);

    printf("準備完了\n");
    while (true)
    {
        if (button == 0)
        {
            wait(1);
            break;
        }
    }

    bmx.set_initial_mag();
    bmx.set_initial_acc();

    sensor_thread.start(callback(update));
    thread_trace.start(callback(line_trace));
    thread_motor.start(callback(control_motor));

    while (true)
    {
        if (button == 0)
        {
            bmx.posture.w = 1;
            bmx.posture.x = 0;
            bmx.posture.y = 0;
            bmx.posture.z = 0;
            lock = true;
            wait(0.1);
            bmx.set_initial_mag();
            bmx.set_initial_acc();
            lock = false;
        }
    }
}