File content as of revision 5:17314fc7b175:

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

BMX055 bmx(D0, D1);

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


Thread thread_trace;
Thread thread_motor;

Thread sensor_thread;

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 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 = 5000;//850;
    const float KD = 3000;
    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 (true)
    {
        current_azimuth = bmx.get_azimuth_machineframe();
        
        if(current_azimuth>3.1416)
        {
            current_azimuth-=6.2832;
        }
        
        curvature = 0.01 / (target_azimuth - current_azimuth);

        //motorL.Set_target(3000 - (1 - tread/curvature));
        //motorR.Set_target(3000 - (1 + tread/curvature));
        
        motorL.Set_target(3000.0);
        motorR.Set_target(3000.0);
    }

    /*while (1)
    {
        float gap_min = 0.0;
        int i_min = 0;
        for (int i = 0; i < 500; i++)///////////////////n; i++)
        {
            float dist_gap = dist - way[i][3];
            if (dist_gap < gap_min)
            {
                gap_min = dist_gap;
                i_min = i;
            }
        }



        if (reflectorFL > 0.1)
        {
            flag = L;
        }
        if (reflectorFR > 0.1)
        {
            flag = R;
        }
        if (reflectorFM <= 0.4 && reflectorFL <= 0.1 && reflectorFR <= 0.1)
            if (true)
            {
                if (flag == L)
                    if (true)
                    {
                        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.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;
                        flag = L;
                    }
                    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;
                        flag = R;
                    }
                }
            }
        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 = 0.57 / 2;

    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 = 1500;
    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(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;
        }
    }*/
}