File content as of revision 9:337fe0747940:

#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 //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
#define HARUROBO_TEST_MODE
//#define HARUROBO_MAIN_MODE

int go_waitmode = 0;

//-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
//DigitalOut cansend_led(LED1);  //canread -> on                //can.cpp
//DigitalOut canread_led(LED2);  //cansend -> 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

#ifdef HARUROBO_TEST_MODE
    T1 = 0;
    printf("start\n\r");
    //printf("T1 = %d\n\r",T1);
    if(T1 == 0) {

        //スタート位置からみかんの木まで移動
        printf("T1 = 0\n\r");
        T1++;
        //printf("t = %d\n\r",T1);
    }

    if(T1 == 1) {
        while(1) {
            printf("T1 = 1\n\r");
            wait(0.5);
            if(T1 == 2) {
                break;
            }
        }
    }

    if(T1 == 2) {

        //みかんの木から三宝置き場まで移動
        printf("T1 = 2\n\r");
        T1++;
    }

    if(T1 == 3) {
        while(1) {
            printf("T1 = 3\n\r");
            if(T1 == 4) {
                
                break;
            }
        }
    }

    if(T1 == 4) {

        //三宝置き場からりんごの木まで移動
        printf("t = 4");
        T1++;
    }

    if(T1 == 5) {
        while(1) {
            if(T1 == 6) {
                break;
            }
        }
    }

    if(T1 == 6) {

        //りんごの木からお供え台まで移動
        printf("t = 6");
        T1++;
    }
#endif

#ifdef HARUROBO_MAIN_MODE
    while(1) {

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

                id1_value[6] = 1;
                switch(id1_value[6]) {
                    case 0:
                        //-----right mode-------------------------------------------------------------------------------------------------------------//

                        break;
                    case 1:
                        //-----left mode--------------------------------------------------------------------------------------------------------------//

                        go_waitmode = 0;
                        if(go_waitmode == 0) {



                            //can_start();
                            gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
                            purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
                            purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
                            gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
                            MaxonControl(0,0,0,0);
                            set_cond(2,0,-1243,1,1076);
                            pos_correction(-1672,1400,0,0,0);
                            enc_correction(1,1);
                            wait(0.5);

                            gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
                            purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
                            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,1000,1000,5,0.1,10,0.1,600,-90);
                            gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
                            MaxonControl(0,0,0,0);
                            pos_correction(-2962,4500,-90,1,1);

                            set_cond(0,0,-2462,0,0);
                            gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            pos_correction(-2850,4500,-90,0,1);
                            enc_correction(1,0);
                            wait(0.5);

                            gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
                            purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
                            purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
                            purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90);
                            gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90);
                            set_cond(2,1,-3500,1,6324);
                            gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
                            MaxonControl(0,0,0,0);
                            pos_correction(-2700,6647,-90,0,0);
                            enc_correction(1,1);
                            wait(0.5);

                            gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90);
                            gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90);
                            purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
                            gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
                            set_cond(2,1,1243,1,4000);
                            gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
                            MaxonControl(0,0,0,0);
                            pos_correction(-519,4500,-180,0,0);
                            MaxonControl(0,0,0,0);

                            go_waitmode = 1;

                        } else if(go_waitmode == 1) {

                            MaxonControl(0,0,0,0);

                        }
                        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;
        }
//------------------------------------------------------------------------------------------------------------------------------------//
    }
#endif
}