File content as of revision 13:f9867c3b97b0:

#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;

/*------------運動時の各速度の設定----------*/
double st_speed = 500; // 移動開始時の速度
double max_speed = 2500; // 直線移動時の最大速度
double mid_speed = 2000; // 直線移動時の中間速度
double turn_speed = 1000; // 旋回時の速度
double end_speed = 200; // 停止直前の速度
double fruit_speed = 200; // 果物を取る時の速度
/*--------------------------------------*/

//-----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) {
        //id1_value[0] = 1;
        switch(id1_value[0]) {
//-----auto mode----------------------------------------------------------------------------------------------------------------------//
            case 1:
                //id1_value[6] = 0;
                switch(id1_value[6]) {
                    case 0:
                        //-----right mode-------------------------------------------------------------------------------------------------------------//
                        //未修正

                        flag = 0;
                        UserLoopSetting_enc_right();
                        if(T1 == 0) {  //スタート位置からみかんの木まで移動

                            gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
                            gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
                            set_cond(2,1,1050,1,1076);
                            gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);
                            pos_correction(1672,1600,0,0,0,25);
                            enc_correction(1,1);
                            gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);

                            //printf("t1 = 0\n\r");
                            //wait(1);
                            T1++;
                        }
                        if(T1 == 1) {
                            while(1) {
                                wait(0.1);
                                //calc_xy(0,0,0);
                                if(T1 == 2) {
                                    //  enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 2) {  //みかんの木から三宝置き場まで移動

                            gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90);
                            gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90);
                            gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90);
                            MaxonControl(0,0,0,0);
                            set_cond(2,1,2462,0,6000);
                            pos_correction(2962,4837.5,90,0,0,10);
                            gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            //printf("t1 = 2\n\r");
                            //wait(1);
                            T1++;
                        }
                        if(T1 == 3) {
                            while(1) {
                                wait(0.1);
                                //calc_xy(90,0,0);
                                if(T1 == 4) {
                                    //  enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 4) {  //三宝置き場からりんごの木まで移動

                            gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90);
                            purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90);
                            purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90);
                            purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90);
                            gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90);
                            MaxonControl(0,0,0,0);
                            set_cond(1,0,0,1,6038);
                            pos_correction(2500,6610,90,1,0,8);
                            set_cond(2,0,3162,1,6038);
                            gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            // printf("t1 = 4\n\r");
                            // wait(1);
                            T1++;
                        }
                        if(T1 == 5) {
                            while(1) {
                                wait(0.1);
                                //calc_xy(90,0,0);
                                if(T1 == 6) {
                                    //  enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 6) {  //りんごの木からお供え台まで移動

                            set_cond(1,0,0,0,6962);
                            gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90);
                            gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90);
                            gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90);
                            enc_correction(0,1);
                            set_cond(2,1,19,0,6962); //要修正
                            purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正
                            enc_correction(1,0);
                            set_cond(0,1,19,0,0);
                            gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180);
                            gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180);
                            set_cond(2,0,1000,1,4000);
                            gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180);
                            MaxonControl(0,0,0,0);
                            pos_correction(620,4370,180,0,0,30);
                            enc_correction(1,1);
                            MaxonControl(0,0,0,0);

                            // printf("t1 = 6\n\r");
                            // wait(1);
                            T1++;
                        }
                        if(T1 == 7) {
                            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;
                        printf("start\n\r");
                        UserLoopSetting_enc_left();
                        if(T1 == 0) {  //スタート位置からみかんの木まで移動

                            gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
                            gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
                            set_cond(2,0,-1050,1,1076);
                            gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);
                            pos_correction(-1672,1600,0,0,0,25);
                            enc_correction(1,1);
                            gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);

                            T1++;
                        }
                        if(T1 == 1) {
                            while(1) {
                                wait(0.1);
                                calc_xy(0,0,0);
                                if(T1 == 2) {
                                    enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 2) {  //みかんの木から三宝置き場まで移動
                            gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,5,0.1,10,0.1,600,-90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
                            gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90);
                            gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90);
                            MaxonControl(0,0,0,0);
                            set_cond(2,0,-2462,0,6000);
                            pos_correction(-2962,4837.5,-90,0,0,10);
                            gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            T1++;
                        }
                        if(T1 == 3) {
                            while(1) {
                                wait(0.1);
                                calc_xy(-90,0,0);
                                if(T1 == 4) {
                                    enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 4) {  //三宝置き場からりんごの木まで移動
                            gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90);
                            purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90);
                            purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90);
                            purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90);
                            gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            set_cond(1,0,0,1,6038);
                            pos_correction(-2500,6610,-90,1,0,8);
                            set_cond(2,1,-3162,1,6038);
                            gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            T1++;
                        }
                        if(T1 == 5) {
                            while(1) {
                                wait(0.1);
                                calc_xy(-90,0,0);
                                if(T1 == 6) {
                                    enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 6) {  //りんごの木からお供え台まで移動
                            set_cond(1,0,0,0,6962);
                            gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
                            gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90);

                            gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90);
                            enc_correction(0,1);
                            set_cond(2,0,-19,0,6962);
                            purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正
                            enc_correction(1,0);
                            set_cond(0,0,-19,0,0);
                            gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
                            gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
                            set_cond(2,1,-1000,1,4000);
                            gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180);
                            MaxonControl(0,0,0,0);
                            pos_correction(-620,4370,-180,0,0,30);
                            enc_correction(1,1);
                            MaxonControl(0,0,0,0);

                            T1++;
                        }
                        if(T1 == 7) {
                            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;
                }
//-----wait mode----------------------------------------------------------------------------------------------------------------------//
            case 0:

                // printf("wait\n\r");
                calc_xy(0,1,1);
                ashi_led();
                MaxonControl(0,0,0,0);
                go_waitmode = 0;

                break;
//-----manual mode--------------------------------------------------------------------------------------------------------------------//
            case 2:
                switch(id1_value[6]) {
                    case 0:
                        //-----right mode-------------------------------------------------------------------------------------------------------------//
                        //未修正

                        flag = 0;
                        UserLoopSetting_enc_right();
                        if(T1 == 0) {  //スタート位置からみかんの木まで移動

                            gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
                            gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
                            set_cond(2,1,1050,1,1076);
                            gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);
                            pos_correction(1672,1600,0,0,0,25);
                            enc_correction(1,1);
                            gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);

                            //printf("t1 = 0\n\r");
                            //wait(1);
                            T1++;
                        }
                        if(T1 == 1) {
                            while(1) {
                                ManualOut(250,100,500,200);
                                //calc_xy(0,0,0);
                                if(T1 == 2) {
                                    //  enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 2) {  //みかんの木から三宝置き場まで移動

                            gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90);
                            gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90);
                            gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90);
                            MaxonControl(0,0,0,0);
                            set_cond(2,1,2462,0,6000);
                            pos_correction(2962,4837.5,90,0,0,10);
                            gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            //printf("t1 = 2\n\r");
                            //wait(1);
                            T1++;
                        }
                        if(T1 == 3) {
                            while(1) {
                                ManualOut(250,100,500,200);
                                //calc_xy(90,0,0);
                                if(T1 == 4) {
                                    //  enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 4) {  //三宝置き場からりんごの木まで移動

                            gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90);
                            purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90);
                            purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90);
                            purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90);
                            gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90);
                            MaxonControl(0,0,0,0);
                            set_cond(1,0,0,1,6038);
                            pos_correction(2500,6610,90,1,0,8);
                            set_cond(2,0,3162,1,6038);
                            gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            // printf("t1 = 4\n\r");
                            // wait(1);
                            T1++;
                        }
                        if(T1 == 5) {
                            while(1) {
                                ManualOut(250,100,500,200);
                                //calc_xy(90,0,0);
                                if(T1 == 6) {
                                    //  enc_correction(1,1);
                                    break;
                                }
                            }
                        }
                        if(T1 == 6) {  //りんごの木からお供え台まで移動

                            set_cond(1,0,0,0,6962);
                            gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90);
                            gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90);
                            gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90);
                            enc_correction(0,1);
                            set_cond(2,1,19,0,6962); //要修正
                            purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正
                            enc_correction(1,0);
                            set_cond(0,1,19,0,0);
                            gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180);
                            gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180);
                            set_cond(2,0,1000,1,4000);
                            gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180);
                            MaxonControl(0,0,0,0);
                            pos_correction(620,4370,180,0,0,30);
                            enc_correction(1,1);
                            MaxonControl(0,0,0,0);

                            // printf("t1 = 6\n\r");
                            // wait(1);
                            T1++;
                        }
                        if(T1 == 7) {
                            while(1) {
                                ManualOut(250,100,500,200);
                                //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;
                        printf("start\n\r");
                        UserLoopSetting_enc_left();
                        if(T1 == 0) {  //スタート位置からみかんの木まで移動

                            gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
                            gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
                            set_cond(2,0,-1050,1,1076);
                            gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);
                            pos_correction(-1672,1600,0,0,0,25);
                            enc_correction(1,1);
                            gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);

                            T1++;
                        }
                        
                        if(T1 == 1) {
                            while(1) {
                                ManualOut(250,100,500,200);
                                if(T1 == 2) {
                                    break;
                                }
                            }
                        }
                        if(T1 == 2) {  //みかんの木から三宝置き場まで移動
                            gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
                            purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,5,0.1,10,0.1,600,-90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
                            gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90);
                            gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90);
                            MaxonControl(0,0,0,0);
                            set_cond(2,0,-2462,0,6000);
                            pos_correction(-2962,4837.5,-90,0,0,10);
                            gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            T1++;
                        }
                        if(T1 == 3) {
                            while(1) {
                                ManualOut(250,100,500,200);
                                if(T1 == 4) {
                                    break;
                                }
                            }
                        }
                        if(T1 == 4) {  //三宝置き場からりんごの木まで移動
                            gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90);
                            purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90);
                            purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90);
                            purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90);
                            gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            set_cond(1,0,0,1,6038);
                            pos_correction(-2500,6610,-90,1,0,8);
                            set_cond(2,1,-3162,1,6038);
                            gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            enc_correction(1,1);

                            T1++;
                        }
                        if(T1 == 5) {
                            while(1) {
                                ManualOut(250,100,500,200);
                                if(T1 == 6) {
                                    break;
                                }
                            }
                        }
                        if(T1 == 6) {  //りんごの木からお供え台まで移動
                            set_cond(1,0,0,0,6962);
                            gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
                            gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90);
                            gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90);
                            enc_correction(0,1);
                            set_cond(2,0,-19,0,6962);
                            purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正
                            enc_correction(1,0);
                            set_cond(0,0,-19,0,0);
                            gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
                            gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
                            set_cond(2,1,-1000,1,4000);
                            gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180);
                            MaxonControl(0,0,0,0);
                            pos_correction(-620,4370,-180,0,0,30);
                            enc_correction(1,1);
                            MaxonControl(0,0,0,0);

                            T1++;
                        }
                        if(T1 == 7) {
                            while(1) {
                                //printf("ashi finished\n\r");
                                ManualOut(250,100,500,200);
                                //MaxonControl(0,0,0,0);

                                if(id1_value[0] != 1)break;  //これらは他のwhileにも入れる必要あり
                                if(id1_value[6] != flag)break;
                            }
                        }



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

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