//----------------------------------------------------------------------
//    MUKU �Wb Ver 4.221
//
//    探索走行、戻走行
//    等加減速
//    第２走行
//    壁端で距離を補正
//    モーター１・２相励磁
//    スピードアップ
//    スラローム
//
//    Copyright (C) 2005 Mizuno Kazuo
//-----------------------------------------------------------------------

#include <3052.h>
#include <Myfunc_3052.c>
#include <muku4b.c>


//***** 直進姿勢制御 *****
void ST_cont() {
    int cont_g=2;   // 姿勢制御倍率
    int cont_p=0;   // 姿勢制御量
    
    if ((ACCflag==0) && (Mclock>MclockT)) Mclock=Mclock-ACCclock;  //等加速
    if ((ACCflag==1) && (Mclock<Mclock0)) Mclock=Mclock+ACCclock;  //等減速


    if(adSL>open_side && adSR>open_side) {       // 両壁で制御
        cont_p = (adSL - SL_bas) - (adSR - SR_bas);
    } else 
        if(adSL>open_side) {                     // 左壁だけで制御
            cont_p = (adSL - SL_bas) *2;
    }else 
        if(adSR>open_side) {                     // 右壁だけで制御
            cont_p = (SR_bas - adSR) *2;
    }

    cont_p=cont_p*cont_g;      // 姿勢制御量を決定
    ITU1.GRA=Mclock + cont_p;  // 右モーター
    ITU2.GRA=Mclock - cont_p;  // 左モーター
}


//****** タイマ ITU1 割り込み関数 （右モーター） *****
void int_imia1(void) {

  switch (McwR) {
    case 0:         // 停止
      break;
    case 1:         // 正回転
      MsR++;
      if(MsR>8) MsR=1;
      Motor = MpatL[MsL] | MpatR[MsR];
      MpcntR++;
      if (McwL==1) ST_cont();    // 直進の時だけ姿勢制御
      break;
    case 2:         // 逆回転
      MsR--;
      if(MsR<1) MsR=8;
       Motor = MpatL[MsL] | MpatR[MsR];
      MpcntR++;
      break;
    case 3:         // スラローム（正回転）
      MsR++;
      if(MsR>8) MsR=1;
      Motor = MpatL[MsL] | MpatR[MsR];
      MpcntR++;
      break;
    default:break;
  }
  ITU1.TSR.BIT.IMFA=0;    // 割り込みフラグクリア
}



//****** タイマ ITU2 割り込み関数 （左モーター） *****
void int_imia2(void) {

  switch (McwL) {
    case 0:         // 停止
      break;
    case 1:         // 正回転
      MsL++;
      if(MsL>8) MsL=1;
      Motor = MpatL[MsL] | MpatR[MsR];
      MpcntL++;
      break;
    case 2:         // 逆回転
      MsL--;
      if(MsL<1) MsL=8;
       Motor = MpatL[MsL] | MpatR[MsR];
      MpcntL++;
      break;
    case 3:         // スラローム（正回転）
      MsL++;
      if(MsL>8) MsL=1;
      Motor = MpatL[MsL] | MpatR[MsR];
      MpcntL++;
      break;
    default:break;
  }
  ITU2.TSR.BIT.IMFA=0;    // 割り込みフラグクリア
}

// 直進（前半） ----------------------------------
void runST1(int P) {
unsigned char kR1,kL1;  //壁の有無   0:無 1:有 2:壁端で距離を補正済み
    
    MpcntR=MpcntL=0;   //モーター パルスカウントクリアー 
    McwR=McwL=1;       //モーター 正転 
    
    kR1=kL1=0;
    if (adSR>open_side) { kR1=1; }   //右壁が有る
    if (adSL>open_side) { kL1=1; }   //左壁が有る

    while(MpcntR<P && MpcntL<P) {
        //右の壁端で距離を補正
        if (kR1==1) {
            if (adSR<open_side) {  //右壁が無くなった
                kR1=kL1=2; 
                MpcntR=MpcntL=100;
            }
        }
        if (kR1==0) {
            if (adSR>open_side) {  //右壁が見つかった
                kR1=kL1=2; 
                MpcntR=MpcntL=50;
            }
        }
        //左の壁端で距離を補正
        if (kL1==1) {
            if (adSL<open_side) {  //左壁が無くなった
                kL1=kR1=2; 
                MpcntL=MpcntR=110;
            }
        }
        if (kL1==0) {
            if (adSL>open_side) {  //左壁が見つかった
                kL1=kR1=2; 
                MpcntL=MpcntR=55;
            }
        }
    }
}

// 直進 （後半）----------------------------------
void runST2(int P) {
    int s2;
          
    McwR=McwL=1;   //モーター 正転 
    
    s2=P-25;
    while(MpcntR<s2 && MpcntL<s2) {
    }
    while(MpcntR<P && MpcntL<P) {
        // 前壁に接近のため緊急停止
        if ((adFR>near_wall) || (adFL>near_wall)) {
            MpcntR=MpcntL= P;
            LED_center(1);    // 点灯
        }
    }
}

// 迷路データのクリアー  ++++++++++++++++++++++++++
void clear_data() {
    unsigned char x,y;

    //壁の有無のクリアー
    for (y=0;y<16;y++) {
        for (x=0;x<16;x++) {
            kabe[y][x]=0x00;
        }
    }

    //外周壁を設定
    for (x=0;x<16;x++) {
        kabe[0][x] =0x04;   //下壁
        kabe[15][x]=0x01;   //上壁
    }
    for (y=0;y<16;y++) {
        kabe[y][0] =kabe[y][0]  | 0x08;   //左壁
        kabe[y][15]=kabe[y][15] | 0x02;   //右壁
    }
    kabe[0][0]=0x1e;   //スタートブロック
    kabe[0][1]=0x0c;

    nose=1;    // 進行方向（上）
    mx=my=0;   // 現在位置 
}


// 現在位置座標の設定 +++++++++++++++++++++++++
void posXY() {
    switch(nose){
        case 1:  // マウスは上向き 
            my++;
            break;
        case 2:  // マウスは右向き  
            mx++;
            break;
        case 3:  // マウスは下向き  
            my--;
            break;
        case 4:  // マウスは左向き  
            mx--;
            break;
    }
}


// 壁マップの作成 +++++++++++++++++++++++++++++++++
void makekabe() {

    kabe[my][mx] |= 0x10;  //探査済み

    //前壁有り
    if ((adFR > open_front) || (adFL > open_front)) {
        switch(nose){
            case 1:  // マウスは上向き 
                kabe[my][mx] |= 0x01;
                if (my!=15) kabe[my+1][mx] |= 0x04;
                break;
            case 2:  // マウスは右向き  
                kabe[my][mx] |= 0x02;
                if (mx!=15) kabe[my][mx+1] |= 0x08;
                break;
            case 3:  // マウスは下向き  
                kabe[my][mx] |= 0x04;
                if (my!=0) kabe[my-1][mx] |= 0x01;
                break;
            case 4:  // マウスは左向き  
                kabe[my][mx] |= 0x08;
                if (mx!=0) kabe[my][mx-1] |= 0x02;
                break;
        }
    }

    //右壁有り
    if (adSR>open_side) {
        switch(nose){
            case 1:  // マウスは上向き 
                kabe[my][mx] |= 0x02;
                if (mx!=15) kabe[my][mx+1] |= 0x08;
                break;
            case 2:  // マウスは右向き  
                kabe[my][mx] |= 0x04;
                if (my!=0) kabe[my-1][mx] |= 0x01;
                break;
            case 3:  // マウスは下向き  
                kabe[my][mx] |= 0x08;
                if (mx!=0) kabe[my][mx-1] |= 0x02;
                break;
            case 4:  // マウスは左向き  
                kabe[my][mx] |= 0x01;
                if (my!=15) kabe[my+1][mx] |= 0x04;
                break;
        }
    }

    //左壁有り
    if (adSL>open_side) {
        switch(nose){
            case 1:  // マウスは上向き 
                kabe[my][mx] |= 0x08;
                if (mx!=0) kabe[my][mx-1] |= 0x02;
                break;
            case 2:  // マウスは右向き  
                kabe[my][mx] |= 0x01;
                if (my!=15) kabe[my+1][mx] |= 0x04;
                break;
            case 3:  // マウスは下向き  
                kabe[my][mx] |= 0x02;
                if (mx!=15) kabe[my][mx+1] |= 0x08;
                break;
            case 4:  // マウスは左向き  
                kabe[my][mx] |= 0x04;
                if (my!=0) kabe[my-1][mx] |= 0x01;
                break;
        }
    }

}


//***** ゴール **********
void finish() {
    unsigned char n;
    
	TCNT_SW(0);     // カウント停止

    speaker(1);
    for (n=0;n<3;n++) {
        LED_right(1);
        LED_center(1);
        LED_left(1);
        wait_ms(250);
        LED_right(0);
        LED_center(0);
        LED_left(0);
        wait_ms(250);
    }

}

// 探索歩数マップの作成 ++++++++++++++++++++++++	
void makeMap1() {
    unsigned char xx,yy;
    unsigned char ps0,ps1;   //歩数
    unsigned char pe;        //作成終了＝１
    unsigned char Qkabe;

    for (yy=0;yy<16;yy++) {
        for (xx=0;xx<=15;xx++) {
            smap[yy][xx]=255;      // 歩数マップのクリアー
        }
    }
    smap[Gy][Gx]=0;  //ゴール区画を０にする


    ps0=0;
    pe=0;
    while(pe==0) {
        ps1=ps0+1;
        for (yy=0;yy<16;yy++) {
            for (xx=0;xx<16;xx++) {
                if (smap[yy][xx]==ps0) {
                    Qkabe=kabe[yy][xx];
                    if ((Qkabe & 0x01)==0x00) { //上壁開口
                        if (smap[yy+1][xx]==255) smap[yy+1][xx]=ps1;
                    }
                    if ((Qkabe & 0x02)==0x00) { //右壁開口
                        if (smap[yy][xx+1]==255)smap[yy][xx+1]=ps1;
                    }
                    if ((Qkabe & 0x04)==0x00) { //下壁開口
                        if (smap[yy-1][xx]==255) smap[yy-1][xx]=ps1;
                    }
                    if ((Qkabe & 0x08)==0x00) { //左壁開口
                        if (smap[yy][xx-1]==255) smap[yy][xx-1]=ps1;
                    }
                    if ((my==yy)&&(mx==xx)) pe=1;  //マウスの現在位置まで作成したので終了						
                }
            }
        }
        ps0++;
    }
}



//++++++++++++++++++++++++++++++++++++++++++++++++++++++
//探索走行     １：第一探索走行  ２：第一(try)走行
//++++++++++++++++++++++++++++++++++++++++++++++++++++++
void search1(unsigned char nn) {
    unsigned char mpF,mpR,mpB,mpL;  //４方向の探索歩数
    unsigned char mpM;  //現在位置の探索歩数
    unsigned char Qkabe;
    unsigned char way;  // 進行方向 1：前 2：右 3：戻る 4：左 
    
	TCNT_SW(1);        // カウント開始
    stop_Grip(200);    //モーターロック
    while(1){ 
        ACCflag=0;         //等加速
        runST1(pBLK1/2);   //半区画直進

        posXY();          //現在位置座標の設定
        
        if (nn==1) makekabe();   //壁マップの作成
        
        //衝突時の強制停止
        if(crash==1) {
            TCNT_SW(0);     // カウント停止
            stop_Free(250);
            return;
        }
        
        // ゴールの判定
        if ((my==Gy) && (mx==Gx)) {
            ACCflag=1;        //等減速
            runST2(pBLK1);
            turn180L();
            stop_Free(250);
            finish();
            return;
        }
        
        if (nn==1) makeMap1();   //探索歩数マップの作成

        //４方向の探索歩数を抽出
        mpF=mpR=mpB=mpL=255;
        Qkabe=kabe[my][mx];
        switch(nose){
            case 1:  // マウスは上向き 
                if ((Qkabe & 0x01)!=0x01) mpF=smap[my+1][mx];   //前方壁無し
                if ((Qkabe & 0x02)!=0x02) mpR=smap[my][mx+1];   //右方壁無し
                if ((Qkabe & 0x04)!=0x04) mpB=smap[my-1][mx];   //後方壁無し
                if ((Qkabe & 0x08)!=0x08) mpL=smap[my][mx-1];   //左方壁無し
                break;
            case 2:  // マウスは右向き  
                if ((Qkabe & 0x02)!=0x02) mpF=smap[my][mx+1];   //前方壁無し
                if ((Qkabe & 0x04)!=0x04) mpR=smap[my-1][mx];   //右方壁無し
                if ((Qkabe & 0x08)!=0x08) mpB=smap[my][mx-1];   //後方壁無し
                if ((Qkabe & 0x01)!=0x01) mpL=smap[my+1][mx];   //左方壁無し
                break;
            case 3:  // マウスは下向き  
                if ((Qkabe & 0x04)!=0x04) mpF=smap[my-1][mx];   //前方壁無し
                if ((Qkabe & 0x08)!=0x08) mpR=smap[my][mx-1];   //右方壁無し
                if ((Qkabe & 0x01)!=0x01) mpB=smap[my+1][mx];   //後方壁無し
                if ((Qkabe & 0x02)!=0x02) mpL=smap[my][mx+1];   //左方壁無し
                break;
            case 4:  // マウスは左向き  
                if ((Qkabe & 0x08)!=0x08) mpF=smap[my][mx-1];   //前方壁無し
                if ((Qkabe & 0x01)!=0x01) mpR=smap[my+1][mx];   //右方壁無し
                if ((Qkabe & 0x02)!=0x02) mpB=smap[my][mx+1];   //後方壁無し
                if ((Qkabe & 0x04)!=0x04) mpL=smap[my-1][mx];   //左方壁無し
                break;
            }
            
        //進行方向を決定
        way=1;  mpM=mpF;                   // 前
        if (mpR<mpM) { way=2;  mpM=mpR; }  // 右
        if (mpB<mpM) { way=3;  mpM=mpB; }  // 後
        if (mpL<mpM) { way=4;  mpM=mpL; }  // 左            
        switch(way){
            case 1:  // 前に進む 
                runST2(pBLK1); 
                break;
            case 2:  // 右に曲がる
                ACCflag=1;        //等減速
                runST2(pBLK1);
                turn90R();
                LED_center(0);    // 前壁に接近のため緊急停止表示 消灯
                break;
            case 3:  // 戻る
                ACCflag=1;        //等減速
                runST2(pBLK1);
                turn180L();
                LED_center(0);    // 前壁に接近のため緊急停止表示 消灯
                break;
            case 4:  // 左に曲がる
                ACCflag=1;        //等減速
                runST2(pBLK1);
                turn90L();
                LED_center(0);    // 前壁に接近のため緊急停止表示 消灯
                break;
            }
    }
}

//++++++++++++++++++++++++++++++++++++++++++++++++++++++
//第３走行     スラローム
//++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Try3() {
    unsigned char mpF,mpR,mpB,mpL;  //４方向の探索歩数
    unsigned char mpM;  //現在位置の探索歩数
    unsigned char Qkabe;
    unsigned char way;  // 進行方向 1：前 2：右 3：戻る 4：左 
    unsigned char slm;  //スラローム  0:直進 1:スラローム
    
    TCNT_SW(1);       // カウント開始
    stop_Grip(200);   // モーターロック
    slm=0;            //スラローム  0:直進
    
    while(1){ 
        MpcntR=MpcntL=0;   //モーター パルスカウントクリアー 
        McwR=McwL=1;       //モーター 正転 

        if (slm==0) {
            runST1(pBLK1/2);    //半区画直進
        } else {
            MpcntR=MpcntL=pBLK1/2;
        }
        
        posXY();    //現在位置座標の設定
        
        //衝突時の強制停止
        if(crash==1) {
            TCNT_SW(0);     // カウント停止
            stop_Free(250);
            return;
        }

        // ゴールの判定
        if ((my==Gy) && (mx==Gx)) {
            runST2(pBLK1);
            turn180L();
            stop_Free(250);
            finish();
            return;
        }

        //４方向の探索歩数を抽出
        mpF=mpR=mpB=mpL=255;
        Qkabe=kabe[my][mx];
        switch(nose){
            case 1:  // マウスは上向き 
                if ((Qkabe & 0x01)!=0x01) mpF=smap[my+1][mx];   //前方壁無し
                if ((Qkabe & 0x02)!=0x02) mpR=smap[my][mx+1];   //右方壁無し
//                if ((Qkabe & 0x04)!=0x04) mpB=smap[my-1][mx];   //後方壁無し
                if ((Qkabe & 0x08)!=0x08) mpL=smap[my][mx-1];   //左方壁無し
                break;
            case 2:  // マウスは右向き  
                if ((Qkabe & 0x02)!=0x02) mpF=smap[my][mx+1];   //前方壁無し
                if ((Qkabe & 0x04)!=0x04) mpR=smap[my-1][mx];   //右方壁無し
//                if ((Qkabe & 0x08)!=0x08) mpB=smap[my][mx-1];   //後方壁無し
                if ((Qkabe & 0x01)!=0x01) mpL=smap[my+1][mx];   //左方壁無し
                break;
            case 3:  // マウスは下向き  
                if ((Qkabe & 0x04)!=0x04) mpF=smap[my-1][mx];   //前方壁無し
                if ((Qkabe & 0x08)!=0x08) mpR=smap[my][mx-1];   //右方壁無し
//                if ((Qkabe & 0x01)!=0x01) mpB=smap[my+1][mx];   //後方壁無し
                if ((Qkabe & 0x02)!=0x02) mpL=smap[my][mx+1];   //左方壁無し
                break;
            case 4:  // マウスは左向き  
                if ((Qkabe & 0x08)!=0x08) mpF=smap[my][mx-1];   //前方壁無し
                if ((Qkabe & 0x01)!=0x01) mpR=smap[my+1][mx];   //右方壁無し
//                if ((Qkabe & 0x02)!=0x02) mpB=smap[my][mx+1];   //後方壁無し
                if ((Qkabe & 0x04)!=0x04) mpL=smap[my-1][mx];   //左方壁無し
                break;
            }
            
        //進行方向を決定
        way=1;  mpM=mpF;                   // 前
        if (mpR<mpM) { way=2;  mpM=mpR; }  // 右
//        if (mpB<mpM) { way=3;  mpM=mpB; }  // 後
        if (mpL<mpM) { way=4;  mpM=mpL; }  // 左            
        switch(way){
            case 1:  // 前に進む 
                runST2(pBLK1);
                slm=0;
                break;
            case 2:  // 右に曲がる
                Sturn90R();
                slm=1;
                break;
            case 4:  // 左に曲がる
                Sturn90L();
                slm=1;
                break;
            }
    }
}




// スピードアップ  ---------------------
void speedUP(unsigned char P) {
int Msu;
    
    Msu=Mclock0-Mclock0/10*P;            //スピードアップ Mode.SW*10(%)
    Mclock=Mclock0;                      //スタート時モータークロック
    MclockT=Msu/2;                       //走行時最高モータークロック
    Mclock1=Mclock0;                     //局地回転時モータークロック
    ACCclock=(Mclock0-MclockT)/ACCstep;  //等加速時の増加クロック
    ACCflag=0;                           //等加速
    
    ITU1.GRA=Mclock0;          // 右モーター 割り込み時間
    ITU2.GRA=Mclock0;          // 左モーター 割り込み時間
    
}

// スラロームスピードsetUP  ---------------------
void SLspeed(unsigned char P) {
int Msu;
    
    Msu=Mclock0-Mclock0/100*P;        //スピードアップ P(%)
    Mclock=Msu;                      //スタート時モータークロック
    MclockT=Msu/2;                   //走行時最高モータークロック
    Mclock1=Msu;                     //回転時モータークロック
    ACCclock=(Msu-MclockT)/ACCstep;  //等加速時の増加クロック
    ACCflag=9;                           //加減速なし
    
    ITU1.GRA=Msu;          // 右モーター 割り込み時間
    ITU2.GRA=Msu;          // 左モーター 割り込み時間
    
}


// 未探査区画の壁は４方壁とする ++++++++++++++++++++++++	
void makekabe2() {
    unsigned char xx,yy;

    for (yy=0;yy<16;yy++) {
        for (xx=0;xx<=15;xx++) {
            if ((kabe[yy][xx] & 0x10)==0x00) {
                kabe[yy][xx] |= 0x0f;
                if (yy!=15) kabe[yy+1][xx] |= 0x04;
                if (xx!=15) kabe[yy][xx+1] |= 0x08;
                if (yy!=0)  kabe[yy-1][xx] |= 0x01;
                if (xx!=0)  kabe[yy][xx-1] |= 0x02;
            }
        }
    }
}




//=================================================
// メイン                                        
//=================================================
void main() {
    char cmd;  // コマンド受信
    char s[5];
    //set up
    IO_init();            // I/Oポート 初期設定 
    LED_right(2);
    LED_center(2);
    LED_left(2);
    speaker(2);
    TCNT_SW(0);           // カウント停止
    runData_init();       // 走行パラメータの初期設定
    
    EI; // 割り込み開始
    
    while(1){
    
        // コマンド受信----------------------
        if (SCI1.SSR.BIT.RDRF==1) {
            cmd = SCI1.RDR;     // データ取り出し
            if (cmd=='s') {     // 光センサー値の読み出し
                TCNT_SW(1);
                wait_ms(100);
                sens_PC();
                TCNT_SW(0);
            }
            if (cmd=='r') {       // １区画直進
                TCNT_SW(1);
                runST1(pBLK1);
                stop_Free(200);
                TCNT_SW(0);
            }
            if (cmd=='t') {       // 180度局地回転
                TCNT_SW(1);
                turn180L();
                stop_Free(200);
                TCNT_SW(0);
            }
            if (cmd=='m') {     // モードSW
                speaker(1);
                SCI1_out_String ("------ \n\r");
                SCI1_out_String ("Mode SW ");
                SCI1_out_String (IntToDec(MODE.BYTE,2,s));
                SCI1_out_String ("\n\r");
            }
            SCI1.SSR.BIT.RDRF = 0;   // 受信フラグクリア   
            SCI1.SSR.BYTE &= 0x80;   // エラーフラグクリア
            cmd=' ';                 // 受信データクリア
        }
        
        //モードSWの読み込み
        MODE.BYTE=0x00;
        if(P4.DR.BIT.B0 == 0) MODE.BIT.B0=1;
        if(P4.DR.BIT.B1 == 0) MODE.BIT.B1=1;
        if(P4.DR.BIT.B2 == 0) MODE.BIT.B2=1;
        
        // 走行開始-----------------------------
        if(Start_sw==0) {
            crash=0;
            nose=1;    // 進行方向（上）
            mx=my=0;   // 現在位置 
            
            //探索走行 start------------------
            if (Run_Level==0) {
                wait_ms(1000);
                count_down1();     //Start Count Down
                clear_data();     //迷路データのクリアー
                Gy=7;Gx=7;        //目標ゴール座標
                search1(1);       //探索走行（局地回転）
                if(crash==0) {
                    wait_ms(2000);
                    Gy=0;Gx=0;        //スタート地点座標
                    search1(1);       //探索戻走行（局地回転）
                    Run_Level=1;      //探索走行完了
                }
            }
            
            //第一走行------局地回転----------------
            if (Run_Level==1) {
                if(crash==0) {
                    speedUP(0);     //スピード Reset
                    wait_ms(2000);
                    count_down1();    //Start Count Down
                    makekabe2();      //未探査区画の壁は４方壁とする
                    Gy=7;Gx=7;        //目標ゴール座標
                    makeMap1();       //最短歩数マップの作成
                    search1(2);       //最短走行（局地回転）
                    if(crash==0) {
                        wait_ms(2000);
                        Gy=0;Gx=0;        //スタート地点座標
                        makeMap1();       //最短歩数マップの作成
                        search1(2);       //最短戻走行（局地回転）
                        Run_Level=2;      //最短走行完了
                    }
                }
            }
            
            //最短走行------局地回転----------------
            if ((Run_Level>=3) && (MODE.BYTE<=5)) {
                if(crash==0) {
                    speedUP(MODE.BYTE);     //スピードアップ Mode.SW*10(%)
                    wait_ms(2000);
                    count_down1();    //Start Count Down
                    Gy=7;Gx=7;        //目標ゴール座標
                    makeMap1();       //最短歩数マップの作成
                    search1(2);       //最短走行（局地回転）
                    if(crash==0) {
                        wait_ms(2000);
                        Gy=0;Gx=0;        //スタート地点座標
                        makeMap1();       //最短歩数マップの作成
                        search1(2);       //最短戻走行（局地回転）
                        Run_Level=3;      //最短走行完了
                    }
                }
            }
            
            //最短走行------スラローム----------------
//            if ((Run_Level>=3) && (MODE.BYTE==7)) {
            if (MODE.BYTE>=6) {
                if(crash==0) {
                    if (MODE.BYTE==6) speedUP(0);     //スピードアップ %
                    if (MODE.BYTE==7) speedUP(2);     //スピードアップ %
                    wait_ms(2000);
                    count_down1();    //Start Count Down
                    Gy=7;Gx=7;        //目標ゴール座標
                    makeMap1();       //最短歩数マップの作成
                    Try3();           //最短戻走行（スラローム）
                    if(crash==0) {
                        wait_ms(2000);
                        Gy=0;Gx=0;        //スタート地点座標
                        makeMap1();       //最短歩数マップの作成
                        Try3();          //最短戻走行（スラローム）
                        Run_Level=4;      //最短走行完了
                    }
                }
            }
            
            if (Run_Level==2) Run_Level=3;
        }
    }

    DI; // 割り込み終了 
}