//---------------------------------------------------------------------- // MUKU V Ver0.55 中部地区大会/2004 // // 直進姿勢制御 // 検索走行、戻走行 // 第２走行（第２走行） // スラローム // スピードアップ // 前壁直前・壁端で位置（パルス数）の補正 // Copyright (C) 2004 Mizuno Kazuo //----------------------------------------------------------------------- #include <3052.h> #include #include void search1(unsigned char nn); void Try3(); void clear_data(); void speedUP(int p); void makekabe(); void makekabe2(); void test_kabe(); // test Wall Data void makeMap1(); void posXY(); void runST1(int P); void runST2(int P); void turn90L(); void turn90R(); void turn180L(); void smap_PC(); void kabe_PC(); void mymx_PC(); void etc_PC(); void Sturn90R(); void Sturn90L(); // 迷路パラメータ ************************* unsigned char nose; // 進行方向 1：上 2：右 3：下 4：左 unsigned char mx; // 現在位置Ｘ座標 unsigned char my; // 現在位置Ｙ座標 unsigned char kabe[16][16]; //壁の有無 1：上 2：右 4：下 8：左 unsigned char smap[16][16]; //歩数マップ unsigned char Gy,Gx; //目標ゴール座標 unsigned char wL,wR; //================================================= // メイン //================================================= void main() { char c; //コマンド受信 IO_init(); //I/Oポート 初期設定 SCI1_init(br57600); //通信ポート初期設定 LED_right_side(2); LED_right_front(2); LED_left_front(2); LED_left_side(2); speaker(2); runData_init(); //走行パラメータの初期設定 EI; // 割り込み開始 while(1){ //コマンド受信---------------------- if (SCI1.SSR.BIT.RDRF==1) { c = SCI1.RDR; // データ取り出し if (c=='s') { // 光センサー値 TCNT_SW(1); // カウント開始 wait_ms(200); TCNT_SW(0); // カウント停止 sens_PC(); } if (c=='h') { // 歩数マップ smap_PC(); } if (c=='k') { // 壁マップ kabe_PC(); } if (c=='p') { // 現在位置＆方向 mymx_PC(); } if (c=='e') { // その他 TCNT_SW(1); // カウント開始 MpcntR=MpcntL=0; //モーター パルスカウントクリアー wait_ms(200); McwR=McwL=1; //モーター 正転 runST1(pBLK1/2); //半区画直進 stop_Free(); TCNT_SW(0); // カウント停止 etc_PC(); } SCI1.SSR.BIT.RDRF = 0; // 受信フラグクリア SCI1.SSR.BYTE &= 0x80; // エラーフラグクリア c=' '; // 受信データクリア } 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=2;Gx=3; //目標ゴール座標 search1(1); //探索走行（局地回転） if(crash==0) { Run_Level=1; //探索走行完了 wait_ms(2000); Gy=0;Gx=0; //スタート地点座標 search1(1); //探索戻走行（局地回転） } } //------reStart---------------------- if (Run_Level>=1) { if(crash==0) { wait_ms(2000); count_down1(); //Start Count Down makekabe2(); //未探査区画の壁は４方壁とする Gy=2;Gx=3; //目標ゴール座標 makeMap1(); //最短歩数マップの作成 search1(2); //最短走行（局地回転） if(crash==0) { Run_Level=2; //最短走行完了 wait_ms(2000); Gy=0;Gx=0; //スタート地点座標 makeMap1(); //最短歩数マップの作成 // search1(2); //最短戻走行（局地回転） Try3(); //最短戻走行（スラローム） } } } } if(Mode_sw==0) { crash=0; nose=1; // 進行方向（上） mx=my=0; // 現在位置 //-------Try slalom------------------ if (Run_Level>=2) { speedUP(0); //スピードアップ（％） wait_ms(1000); count_down1(); //Start Count Down Gy=2;Gx=3; //目標ゴール座標 makeMap1(); //最短歩数マップの作成 Try3(); //最短走行（スラローム） if(crash==0) { Run_Level=3; //スラローム走行完了 wait_ms(2000); Gy=0;Gx=0; //スタート地点座標 makeMap1(); //最短歩数マップの作成 Try3(); //最短戻走行（スラローム） } } } } DI; // 割り込み終了 } // スラローム(右90°)転回 -------------------------------- void Sturn90R() { ITU1.GRA=Mclock*2.20; // 右モーター回転速度 ITU2.GRA=Mclock/1.10; // 左モーター回転速度 MpcntL=MpcntR=0; // モーター パルスカウントクリアー McwR=3; // 正回転 McwL=3; // 正回転 while(MpcntL<530) {} nose++; // マウス進行方向 if(nose==5) nose=1; } // スラローム(左90°)転回 -------------------------------- void Sturn90L() { ITU1.GRA=Mclock/1.10; // 右モーター回転速度 ITU2.GRA=Mclock*2.20; // 左モーター回転速度 MpcntL=MpcntR=0; // モーター パルスカウントクリアー McwR=3; // 正回転 McwL=3; // 正回転 while(MpcntR<530) {} nose--; // マウス進行方向 if(nose==0) nose=4; } // スピードアップ (％) --------------------- void speedUP(int p) { Mclock=Mclock0-Mclock0/100*p; // クロック設定 adv_clock=Mclock*3/150; // 発進加速度クロック ITU1.GRA=Mclock; // 右モーター 割り込み時間 ITU2.GRA=Mclock; // 左モーター 割り込み時間 } //++++++++++++++++++++++++++++++++++++++++++++++++++++++ //第３走行 スラローム //++++++++++++++++++++++++++++++++++++++++++++++++++++++ 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) { cont_g=cont_g0; // 姿勢制御 ゲイン runST1(pBLK1/2); //半区画直進 } else { cont_g=cont_g0*2.5; // 姿勢制御 ゲイン MpcntR=MpcntL=pBLK1/2; } posXY(); //現在位置座標の設定 //衝突時の強制停止 if(crash==1) { TCNT_SW(0); // カウント停止 stop_Free(); return; } // ゴールの判定 if ((my==Gy) && (mx==Gx)) { runST2(pBLK1); turn180L(); stop_Free(); 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 Fopen2) || (adFL > Fopen2)) { 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>Sopen2) { 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>Sopen2) { 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 runST1(int P) { unsigned char kR1,kL1; //壁の有無 0:無 1:有 unsigned char kR2,kL2; kR1=kL1=0; wR=wL=0; if (adSR>Sopen2) { kR1=1; } //右壁が有る if (adSL>Sopen2) { kL1=1; } //左壁が有る while(MpcntR

Sopen2) { kR2=1; } else { kR2=0; } } if (kL1!=2) { if (adSL>Sopen2) { kL2=1; } else { kL2=0; } } if (kR1!=kR2) { //右の壁端で１度だけ距離補正 // if (kR1==0) { // wR=MpcntR; //test data // MpcntL=MpcntL-MpcntR+70; // MpcntR=70; // } if (kR1==1) { wR=MpcntR; //test data MpcntL=MpcntL-MpcntR+70; MpcntR=70; } kR1=kR2=2; } if (kL1!=kL2) { //右の壁端で１度だけ距離補正 // if (kL1==0) { // wL=MpcntL; //test data // MpcntR=MpcntR-MpcntL+70; // MpcntL=70; // } if (kL1==1) { wL=MpcntL; //test data MpcntR=MpcntR-MpcntL+70; MpcntL=70; } kL1=kL2=2; } } adv=1; // 直進中 } // 直進 （後半）---------------------------------- void runST2(int P) { int C; C=cont_c-20; while(MpcntR

comp_wall) && (adFL > comp_wall)) { // 前壁に接近 if ((MpcntRC) && (MpcntL>C)) { MpcntR=MpcntL=cont_c; // 距離補正 C=P+20; } } } } adv=1; // 直進中 } // 局地(左90°)回転 ---------------------------------- void turn90L() { stop_Grip(200); ITU1.GRA=ITU2.GRA=Mclock0/3*5; // 回転速度 設定 3/5 MpcntL=MpcntR=0; // モーター パルスカウントクリアー McwR=1; // 正回転 McwL=2; // 逆回転 while(MpcntL=0;y--) { SCI1_out_String ("ss,"); for (x=0;x<16;x++) { SCI1_out_String (IntToDec(smap[y][x],3,s)); SCI1_out_String (","); } SCI1_out_String ("\n\r"); } } // 壁マップの出力 -------------------------------- void kabe_PC() { char s[5]; int y,x; speaker(2); SCI1_out_String ("kabe_send \n\r"); for (y=15;y>=0;y--) { SCI1_out_String ("ks,"); for (x=0;x<16;x++) { SCI1_out_String (IntToDec(kabe[y][x],2,s)); SCI1_out_String (","); } SCI1_out_String ("\n\r"); } } // その他データの出力 -------------------------------- void etc_PC() { char s[5]; speaker(2); SCI1_out_String ("wL "); SCI1_out_String (IntToDec(wL,2,s)); SCI1_out_String ("\n\r"); SCI1_out_String ("wR "); SCI1_out_String (IntToDec(wR,2,s)); SCI1_out_String ("\n\r"); }