File content as of revision 53:4673b85e1d2a:

#include "mbed.h"
#include "debug.h"
#include "microinfinity.h"
#include "sensors.h"
#include "moves.h"
#include <stdlib.h>


#define BADRULEMODE

void StateFlow(int i);
void SetMode();
void Start();
void NoHandSignal();


int start_state=0;



int main()
{
    LoadParameter();
    //setup関連
    can1.frequency(1000000);
    SetupMoves();
    set_gyro();
    printf("hand read:%d\r\n",hand.read());
    /*while(1) 
    {
        printf("forward:%.2f back:%.2f\r\n",get_dist_forward(),get_dist_back());
        wait(0.01);
    }*/
    
    printf("reset standby\r\n");
    reset();//足をリセット位置に移動
    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();
//    mode選択
    Start();
#ifndef BADRULEMODE
    if(RightOrLeft == 0) {
        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");
    } else {
        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");
    }
#endif
#ifdef BADRULEMODE
    printf("BAD RULE!!!!!!!!!\r\n");
    if(RightOrLeft == 0) {
        if(start_state == 0) theta0 = -degree/100.0;
        else if(start_state == 1) {
            theta0 = -degree/100.0 - 90;
            printf("degree is %.2f for 90\r\n", degree0);
            motor_lo.setDutyLimit(0.6);//0.5
            motor_li.setDutyLimit(0.6);
            turn(55.0);
        } else if(start_state == 2) {
            theta0 = -degree/100.0 - 135;
            printf("degree is %.2f for 135\r\n", degree0);
            motor_lo.setDutyLimit(0.6);//0.5
            motor_li.setDutyLimit(0.6);
            turn(100.0);
        } else if(start_state == 3) theta0 = -degree/100.0 + 90;
        else printf("state_error");
    } else {
        if(start_state == 0) theta0 = -degree/100.0;
        else if(start_state == 1) {
            theta0 = -degree/100.0 + 90;
            printf("degree is %.2f for -90\r\n", degree0);
            //段差前旋回
            motor_lo.setDutyLimit(0.6);//0.5
            motor_li.setDutyLimit(0.6);
            turn(-55.0);
        } else if(start_state == 2) {
            theta0 = -degree/100.0 + 135;
            printf("degree is %.2f for -135\r\n", degree0);
            motor_lo.setDutyLimit(0.6);//0.5
            motor_li.setDutyLimit(0.6);
            turn(-100.0);
        } else if(start_state == 3) theta0 = -degree/100.0 - 90;
        else printf("state_error");
    }
#endif
    FileOpen();
    printf("LR:%d StateFlow:%d\r\n", RightOrLeft,start_state);
    StateFlow(start_state);
    FileClose();
}

void StateFlow(int i)
{
    switch(i) {
        case 0://最初の直線
            printf("go straight!!!!!!!!!!\r\n");
            motor_lo.setDutyLimit(params[0].duty);
            motor_li.setDutyLimit(params[0].duty);
            
            while(checkUW(params[1].condition, degree0, ec_lo.getCount()) == 0){
                straightAndInfinity(params[0].argument[0], params[0].argument[1]);
            }
            printf("get distance mode\r\n");
            
            motor_lo.setDutyLimit(params[1].duty);
            motor_li.setDutyLimit(params[1].duty);
            for(int i=0; i<1; ++i) {
                while(Hcsr04BackWithEc() < params[1].condition) {//300
                    straightAndInfinity(params[1].argument[0], params[1].argument[1]);
                }
            }
            //段差前旋回
            motor_lo.setDutyLimit(params[2].duty);
            motor_li.setDutyLimit(params[2].duty);
            if(RightOrLeft == 0) turn(params[2].argument[0]);
            else turn(-params[2].argument[0]);
        case 1://段差前
            printf("climb!!!!!!!!!!\r\n");
            
            motor_lo.setDutyLimit(params[3].duty);
            motor_li.setDutyLimit(params[3].duty);
            //段差乗り越え
            while(get_dist_forward() > params[3].condition) {
                if(RightOrLeft == 0) straightAndInfinity(params[3].argument[0], params[3].argument[1]);
                else straightAndInfinity(-params[3].argument[0], params[3].argument[1]);
            }
            motor_lo.setDutyLimit(params[4].duty);
            motor_li.setDutyLimit(params[4].duty);
            if(RightOrLeft == 0) phasing(params[4].argument[0]);
            else phasing(-params[4].argument[0]);
            motor_lo.setDutyLimit(params[5].duty);
            motor_li.setDutyLimit(params[5].duty);
            for(int i= 0; i<(int)params[5].condition; ++i) straight();
            
            motor_lo.setDutyLimit(params[6].duty);
            motor_li.setDutyLimit(params[6].duty);
            printf("start finding back block\r\n"); 
            while(get_dist_back() > params[6].condition) straight();
            straight();
            int i=0;
            while(1)
            {
                i++;
                led1 = i%2;
                wait(0.5);
            }
            //段差後旋回
            motor_lo.setDutyLimit(params[7].duty);
            motor_li.setDutyLimit(params[7].duty);
            if(RightOrLeft == 0) turn(params[7].argument[0]);
            else turn(-params[7].argument[0]);

        case 2://段差直後
            printf("go lope!!!!!!!!!!\r\n");
            //前進
            motor_lo.setDutyLimit(params[8].duty);
            motor_li.setDutyLimit(params[8].duty);
            for(int i=0; i<(int)params[8].condition; ++i) {
                if(RightOrLeft == 0) straightAndInfinity(params[8].argument[0], params[8].argument[1]);
                else straightAndInfinity(-params[8].argument[0], params[8].argument[1]);
            }
            
            motor_lo.setDutyLimit(params[9].duty);
            motor_li.setDutyLimit(params[9].duty);
            for(int i=0; i<3; ++i) {
                while(get_dist_forward() > params[9].condition) {
                    if(RightOrLeft == 0) straightAndInfinity(params[9].argument[0], params[9].argument[1]);
                    else straightAndInfinity(-params[9].argument[0], params[9].argument[1]);
                }
            }
            //ロープ前旋回
            printf("before roop deg:%.3f\r\n",degree0);
            motor_lo.setDutyLimit(params[10].duty);
            motor_li.setDutyLimit(params[10].duty);
            if(RightOrLeft == 0) turn(params[10].argument[0]);
            else turn(-params[10].argument[0]);

            //ロープ位置ジャストまで前進
            motor_lo.setDutyLimit(params[11].duty);
            motor_li.setDutyLimit(params[11].duty);
            for(int i=0; i<3; ++i) {
                straightAndInfinity(params[11].argument[0], params[11].argument[1]);
            }
            for(int i=0; i<2; ++i) {
                while(get_dist_back() < params[11].condition) {
                    straightAndInfinity(params[11].argument[0], params[11].argument[1]);
                }
            }
            
            motor_lo.setDutyLimit(params[12].duty);
            motor_li.setDutyLimit(params[12].duty);
            phasing(params[12].argument[0]);
            
            //ロープ
            motor_lo.setDutyLimit(params[13].duty);
            motor_li.setDutyLimit(params[13].duty);
            while(switch_modes[2].read() == 0) {
                lopeAndInfinity(params[13].argument[0], params[13].argument[1]);
                //straight();
            }
            printf("uhai stand by ok!!!!!!!!!!\r\n");
            NoHandSignal();
        case 3://坂
            printf("last spart!!!!!!!!");

            if(RightOrLeft == 0) theta0 = -degree/100.0+90;
            else theta0 = -degree/100.0-90;
            motor_lo.setDutyLimit(params[14].duty);
            motor_li.setDutyLimit(params[14].duty);
            for(int i=0; i<(int)params[14].condition; ++i) {
                if(RightOrLeft == 0) straightAndInfinity(params[14].argument[0], params[14].argument[1]);//straight();//climbAndInfinity(-90,5);
                else straightAndInfinity(-params[14].argument[0], params[14].argument[1]);
            }
            printf("wall standby");
            motor_lo.setDutyLimit(params[15].duty);
            motor_li.setDutyLimit(params[15].duty);
            while(get_dist_back() < params[15].condition) {
//                straight();
                if(RightOrLeft == 0) climbAndInfinity(params[15].argument[0], params[15].argument[1]);
                else climbAndInfinity(-params[15].argument[0], params[15].argument[1]);
            }
            
            for(int i=0; i<1; ++i) {
                while(get_dist_forward() > params[16].condition) {
                    //straight();
                    if(RightOrLeft == 0) climbAndInfinity(params[16].argument[0], params[16].argument[1]);
                    else climbAndInfinity(-params[16].argument[0], params[16].argument[1]);
                }
            }
            hand_mode = GOAL;
            stop();
            printf("uhai!!!!!!!!!!!!!!!/r/n");
    }
}
void Start()
{
    if(hand.read()==0) { //開始時すでにスイッチが押されていた場合

        SetMode();
        hand_mode = G_CLOSE;
        NoHandSignal();
    } else {
        SetMode();
        hand_mode = G_OPEN;
        stop();//ここで開くことをcanでslaveに送る
        wait(1);
        wait_gerege();
        hand_mode = G_CLOSE;
        HandMove();
        wait(0.5);
    }
}
void SetMode()
{
    while(get_dist_back() > 50.0) {
        led1 = led2 = led3 = led4 = 0;
        if(switch_modes[2].read()) start_state=3;
        else if(switch_modes[1].read()) start_state=2;
        else if(switch_modes[0].read()) start_state=1;
        else start_state=0;

        if(switch_LR.read()) RightOrLeft = 1;
        else RightOrLeft = 0;

        if(start_state==0)led4 = 1;
        else if(start_state==1)led3 = 1;
        else if(start_state==2)led2 = 1;
        else if(start_state==3)led1 = 1;

    }
    printf("mode is %d  RightOrLeft is %d \r\n", start_state, RightOrLeft);
    led1 = led2 = led3 = led4 = 1;
}
void NoHandSignal()
{
    while(get_dist_back() > 40.0) {
        wait(0.01);
    }
    while(get_dist_back() < 40.0) {
        wait(0.01);
    }
}