File content as of revision 0:b87fd8dd4322:

#include "EC.h"
#include "R1370P.h"
#include "move4wheel.h"
#include "mbed.h"
#include "math.h"
#include "PathFollowing.h"
#include "movement.h"
#include "maxonsetting.h"
#include "manual.h"
#include "can.h"

#define PI 3.141592

//#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義

int go_waitmode = 0;

//-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
//DigitalOut cansend_led(LED1);  //cansend -> on                //can.cpp
//DigitalOut canread_led(LED2);  //canread -> on                //can.cpp
//DigitalOut debug_led(LED3);    //maxon debug programme -> on  //maxonsetting.cpp

//////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////

int main()
{
    UserLoopSetting_maxon();
    UserLoopSetting_sensor();
    UserLoopSetting_can();

#ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示

    printf("ソースファイル名 : %s¥n", __FILE__);
    printf("作成日付 : %s¥n", __DATE__);
    printf("作成時刻 : %s¥n", __TIME__);

#endif

    while(1) {

        switch(id1_value[0]) {
//-----auto mode----------------------------------------------------------------------------------------------------------------------//
            case 1:

                switch(id1_value[6]) {
                    case 0:
                        //-----right mode-------------------------------------------------------------------------------------------------------------//
                        flag = 0;
                        if(T1 == 0) {  //スタート位置から少し移動

                            wait(0.5);
                            T1++;
                        }
                        if(T1 == 1) {  //みかんの木まで移動して掴む

                            wait(0.5);
                            T1++;
                        }

                        if(T1 == 2) {  //みかんを持ち上げる
                            while(1) {
                                wait(0.1);
                                if(T1 == 3) {
                                    break;
                                }
                            }
                        }
                        if(T1 == 3) {  //みかんの木から三宝置き場まで移動

                            wait(0.5);
                            T1++;
                        }
                        if(T1 == 4) {  //三宝を掴み機体内に取り込み、みかんを三宝に乗せる
                            while(1) {
                                wait(0.1);
                                if(T1 == 5) {
                                    break;
                                }
                            }
                        }
                        if(T1 == 5) {  //三宝置き場からりんごの木まで移動

                            wait(0.5);
                            T1++;
                        }
                        if(T1 == 6) {  //りんごを持ち上げる
                            while(1) {
                                wait(0.1);
                                if(T1 == 7) {
                                    break;
                                }
                            }
                        }
                        if(T1 == 7) {  //りんごの木からお供え台まで移動

                            wait(0.5);
                            T1++;
                        }
                        if(T1 == 8 || T1 == 9) {  //三宝をお供え台に置く
                            while(1) {
                                printf("ashi finished\n\r");
                                MaxonControl(0,0,0,0);
                                if(id1_value[0] != 1)break;  //これらは他のwhileにも入れる必要あり
                                if(id1_value[6] != flag)break;
                            }
                        }
                        break;
                    case 1:
                        //-----left mode--------------------------------------------------------------------------------------------------------------//
                        flag = 1;

                        break;
                }
//-----wait mode----------------------------------------------------------------------------------------------------------------------//
            case 0:

                calc_xy(0,1,1);
                ashi_led();
                MaxonControl(0,0,0,0);
                go_waitmode = 0;

                break;
//-----manual mode--------------------------------------------------------------------------------------------------------------------//
            case 2:

                ManualOut(250,100,500,200);
                go_waitmode = 0;

                break;
        }
//------------------------------------------------------------------------------------------------------------------------------------//
    }
}