File content as of revision 25:c740e6fd5dab:

#include "mbed.h"
#include "debug.h"
#include "microinfinity.h"
#include "sensors.h"
#include "moves.h"

void StateFlow(int i);
int main()
{
    can1.frequency(1000000);
    SetupMoves();
    set_gyro();
    int start_state=0;

    printf("reset standby\r\n");

    /*while(1) {
        printf("forward:%.3f back:%.3f\r\n", get_dist_forward(), get_dist_back());
        wait(0.01);
    }*/

    reset();
    
    /*
    
    while(1)
    {
        motor_lo.setDutyLimit(0.3);
        motor_li.setDutyLimit(0.3);
        onestraight();
    }
    */
    
    printf("bus standby\r\n");
    while(1) {
        if(bus_in.read() == 1) break;
    }
    printf("bus is %d\r\n", bus_in.read());
    wait(0.5);
    motor_lo.setDutyLimit(0.1);
    motor_li.setDutyLimit(0.1);
    straight();
    
    while(mode4.read() == 1) {
        start_state = (get_dist_back()-10)/10;
        int tmp = start_state;
        if(start_state>6 || start_state<0) start_state = 0;
        //printf("%d\r\n",start_state);
        led2 = start_state/4;
        start_state = start_state%4;
        led3 = start_state/2;
        start_state = start_state%2;
        led4 = start_state;
        start_state = tmp;
        
    }
    
    wait_gerege();
    hand_mode = GEREGE;
    wait(1.0);
    

    //直進スタート
    /*while(1)
    {
        printf("angle:%.3f\r\n", degree0);
        wait(0.1);
    }*/

    /*
    motor_lo.setDutyLimit(0.2);
    motor_li.setDutyLimit(0.2);
    while(1)
    {
        backLeft();
    }*/
    

    
    if(start_state == 0) theta0 = -degree/100.0;
    else if(start_state == 1) theta0 = -degree/100.0 - 45;
    else if(start_state == 2) theta0 = -degree/100.0 - 90;
    else if(start_state == 3) theta0 = -degree/100.0 + 90;
    else printf("state_error");

    StateFlow(start_state);

}

void StateFlow(int i)
{
    switch(i) {
        case 0://最初の直線
            printf("go straight!!!!!!!!!!\r\n");
            
            motor_lo.setDutyLimit(0.6);
            motor_li.setDutyLimit(0.6);
            
            for(int i = 0; i < 25; i++) {
                straightAndInfinity(0, 3);
                //printf("not dist mode angle:%.3f backdist:%.2f ec:%d\r\n", degree0, get_dist_back(), ec_lo.getCount());
            }
            printf("get distance mode");
            for(int i=0; i<1; ++i) {
                while(get_dist_back() < 270) {//320
                    straightAndInfinity(0, 3);
                }
            }
            //段差前旋回
            
            motor_lo.setDutyLimit(0.5);//0.5
            motor_li.setDutyLimit(0.5);
            turn(35.0);
        case 1://段差前
            printf("climb!!!!!!!!!!\r\n");
            //段差乗り越え
            
            motor_lo.setDutyLimit(0.3);//0.5
            motor_li.setDutyLimit(0.3);
            while(get_dist_forward() < 60) {
                straightAndInfinity(45, 5);
            }
            for(int i= 0; i<14; ++i) straight();
            while(get_dist_back() > 80) straight();
            //段差後旋回
            printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
            turn(90.0);
        case 2://段差直後
            printf("go lope!!!!!!!!!!\r\n");
            printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back());
            //前進
            motor_lo.setDutyLimit(0.6);
            motor_li.setDutyLimit(0.6);
            for(int i=0; i<3; ++i) {
                while(get_dist_forward() > 60) {
                    straightAndInfinity(90.0, 5.0);
                }
            }
            //ロープ前旋回
            printf("before roop deg:%.3f\r\n",degree0);
            turn(0);

            //ロープ位置ジャストまで前進

            for(int i=0; i<3; ++i) {
                straightAndInfinity(0.0, 5.0);
            }
            for(int i=0; i<2; ++i) {
                while(get_dist_back() < 100) {
                    straightAndInfinity(0.0, 5.0);
                }
            }

            //wait(10);

            //ロープ
            while(mode4.read() == 0) {
                straightAndInfinity(0, 5);
                //straight();
            }
        case 3://坂
            printf("uhai stand by ok!!!!!!!!!!\r\n");
            while(get_dist_back() > 40.0) {
                wait(0.01);
            }
            while(get_dist_back() < 40.0) {
                wait(0.01);
            }

            printf("last spart!!!!!!!!");
            theta0 = -degree/100.0+90;
            motor_lo.setDutyLimit(0.3);//0.3
            motor_li.setDutyLimit(0.3);

            for(int i=0; i<15; ++i)straight();//climbAndInfinity(-90,5);
            printf("wall standby");
            while(get_dist_back() < 250) {
                straight();
                climbAndInfinity(-90,5);
//        DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
            }
            for(int i=0; i<1; ++i) {
                while(get_dist_forward() > 70) {
                    //straight();
                    climbAndInfinity(-90,5);
//            DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
                }
            }
            hand_mode = GOAL;
            straight();
            printf("uhai!!!!!!!!!!!!!!!");
    }
}