//==================================================================== // MUKU Wb CPU H8/3052 25MHz // // include　file // // Copyright (C) 2005 Mizuno Kazuo //==================================================================== #define Start_sw P4.DR.BIT.B7 // start sw P47 #define Mode_sw0 P4.DR.BIT.B0 // mode sw P40 #define Motor PB.DR.BYTE // モーター ポート 左：b0-b3 右：b4-b7 //壁センサー ***************************************** unsigned int adFR,adFL,adSR,adSL; // 壁センサー AD変換値 unsigned int sens_no; // センサー発光順 NO. // モーター ******************************************* unsigned int MpatL[9] = {0x00,0x06,0x04,0x05,0x01,0x09,0x08,0x0a,0x02}; // 左モーター１・２相励磁パターン unsigned int MpatR[9] = {0x00,0x20,0xa0,0x80,0x90,0x10,0x50,0x40,0x60}; // 右モーター１・２相励磁パターン unsigned char MsL=1,MsR=1; // モーター励磁パターン No unsigned char McwL=0,McwR=0; // モーター回転方向 ０：停止 １：正 ２：逆 unsigned int MpcntR,MpcntL; // モーター パルスカウント unsigned int Mclock; // モータークロック unsigned int MclockT; // 走行時最高モータークロック unsigned int MclockS; // スラローム走行時モータークロック unsigned int Mclock1; // 局地回転時モータークロック unsigned int ACCclock,ACCflag; // 等加速時の増加クロック、フラグ ０：加速 1：減速 //モードSW　0-6:スピードアップ(%) 7:スラローム走行 union { unsigned char BYTE; struct { unsigned char B7:1; unsigned char B6:1; unsigned char B5:1; unsigned char B4:1; unsigned char B3:1; unsigned char B2:1; unsigned char B1:1; unsigned char B0:1; } BIT; } MODE; // 走行パラメータ ************************* #define pR90 150 // 右90 °回転パルス数 #define pL90 150 // 左90 °回転パルス数 #define pL180 300 // 左180°回転パルス数 #define pBLK1 485 // １区画直進走行パルス数 (0.368mm/p) #define Mclock0 12500 // スタート時モータークロック (1ms=6250) #define ACCstep 400 // 加速パルス数 #define near_wall 500 // 前壁による緊急停止時センサー値 #define open_side 50 // 横壁有無を判断するセンサー値 #define open_front 50 // 前壁有無を判断するセンサー値 #define SR_bas 250 // 右横壁センサーの中央基準値 #define SL_bas 250 // 左横壁センサーの中央基準値 // 迷路パラメータ ************************* 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 crash; // 衝突時の強制停止 0:走行 1:強制停止 unsigned char Run_Level; // 完走レベル 0:未完走 1:検索 2:最短走行 3:スラローム //***** I/O 初期設定 ******* void IO_init(void) { PB.DDR = 0xff; // Motorポート Motor = 0x00; //クリアー P2.DDR = 0xff; // センサーLED,スピーカー ポート P5.DDR = 0xff; // LEDポート P4.DDR = 0x00; // SWポート P4.PCR.BIT.B7 = 1; // StartSW プルアップ P4.PCR.BIT.B0 = 1; // ModeSW プルアップ P4.PCR.BIT.B1 = 1; // P4.PCR.BIT.B2 = 1; // AD.ADCSR.BIT.CKS = 1; // A/D 高速変換 SCI1_init(br57600); //通信ポート初期設定 //---- 割り込み 壁センサー ITU0 --- ITU0.TCR.BIT.CCLR=1; // ＧＲＡのコンペアマッチで TCNT クリア ITU0.TCR.BIT.TPSC=1; // コンペアマッチ クロック 1:φ/2 2:φ/4 ITU0.TIER.BIT.IMIEA=1; // コンペアマッチ割り込み許可 ITU0.GRA=3000; // 割り込み時間 (0.24ms) //---- 割り込み 右モーター ITU1 --- ITU1.TCR.BIT.CCLR=1; // ＧＲＡのコンペアマッチで TCNT クリア ITU1.TCR.BIT.TPSC=2; // コンペアマッチ クロック 1:φ/2 2:φ/4 ITU1.TIER.BIT.IMIEA=1; // コンペアマッチ割り込み許可 //---- 割り込み 左モーター ITU2 --- ITU2.TCR.BIT.CCLR=1; // ＧＲＡのコンペアマッチで TCNT クリア ITU2.TCR.BIT.TPSC=2; // コンペアマッチ クロック 1:φ/2 2:φ/4 ITU2.TIER.BIT.IMIEA=1; // コンペアマッチ割り込み許可 } //****** タイマ ITU0 割り込み関数 （壁センサー） ***** void int_imia0(void) { sens_no++; if(sens_no>3) sens_no=0; switch(sens_no) { case 0: // 右横 LED AD.ADCSR.BIT.CH = 1; // チャンネル AN1 P2.DR.BIT.B3=1; // on wait(25); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P2.DR.BIT.B3=0; // off adSR=(AD.ADDRB>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; case 1: // 左横 LED AD.ADCSR.BIT.CH = 0; // チャンネル AN0 P2.DR.BIT.B0=1; // on wait(25); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P2.DR.BIT.B0=0; // off adSL=(AD.ADDRA>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; case 2: // 左前 LED AD.ADCSR.BIT.CH = 3; // チャンネル AN3 P2.DR.BIT.B1=1; // on wait(25); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P2.DR.BIT.B1=0; // off adFL=(AD.ADDRD>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; case 3: // 右前 LED AD.ADCSR.BIT.CH = 2; // チャンネル AN2 P2.DR.BIT.B2=1; // on wait(25); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P2.DR.BIT.B2=0; // off adFR=(AD.ADDRC>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; default:break; } ITU0.TSR.BIT.IMFA=0; // 割り込みフラグクリア //衝突時の強制停止 if(Start_sw==0) { crash=1; wait(2000); } } //LED点滅　右(P50) 0:off 1:on etc:点滅 ---------------------- void LED_right(int p) { switch(p) { case 0: // off P5.DR.BIT.B0 = 0; break; case 1: // on P5.DR.BIT.B0 = 1; break; default: // 点滅 while(p--) { P5.DR.BIT.B0 = 1; wait_ms(100); P5.DR.BIT.B0 = 0; wait_ms(100); } } } //LED点滅　中央(P51) 0:off 1:on etc:点滅 --------------------- void LED_center(int p) { switch(p) { case 0: // off P5.DR.BIT.B1 = 0; break; case 1: // on P5.DR.BIT.B1 = 1; break; default: // 点滅 while(p--) { P5.DR.BIT.B1 = 1; wait_ms(100); P5.DR.BIT.B1 = 0; wait_ms(100); } } } //LED点滅　中央(P52) 0:off 1:on etc:点滅 ------------- void LED_left(int p) { switch(p) { case 0: // off P5.DR.BIT.B2 = 0; break; case 1: // on P5.DR.BIT.B2 = 1; break; default: // 点滅 while(p--) { P5.DR.BIT.B2 = 1; wait_ms(100); P5.DR.BIT.B2 = 0; wait_ms(100); } } } //スピーカー P25 --------------- void speaker(int p) { int n; while(p--) { for(n=1;n<200;n++) { P2.DR.BIT.B5 = 1; wait(120); P2.DR.BIT.B5 = 0; wait(120); } wait(50); } } //光センサー値 >> PC ---------------------- void sens_PC() { char s[5]; speaker(1); SCI1_out_String ("------ \n\r"); SCI1_out_String ("AD SR "); SCI1_out_String (IntToDec(adSR,4,s)); SCI1_out_String ("\n\r"); SCI1_out_String ("AD SL "); SCI1_out_String (IntToDec(adSL,4,s)); SCI1_out_String ("\n\r"); SCI1_out_String ("AD FL "); SCI1_out_String (IntToDec(adFL,4,s)); SCI1_out_String ("\n\r"); SCI1_out_String ("AD FR "); SCI1_out_String (IntToDec(adFR,4,s)); SCI1_out_String ("\n\r"); } //センサー・モーターのカウント 0:OFF 1:ON ------------------- void TCNT_SW(unsigned char s) { //壁センサー ITU.TSTR.BIT.STR0=s; ITU0.TCNT = 0; // 壁センサー・カウンタークリアー //モーター ITU.TSTR.BIT.STR1=s; ITU.TSTR.BIT.STR2=s; ITU1.TCNT = 0; // モーター・カウンタークリアー ITU2.TCNT = 0; } //スタートまでのカウントダウン ------------------ void count_down1() { speaker(1); LED_right(1); wait_ms(1000); speaker(1); LED_center(1); wait_ms(1000); speaker(1); LED_left(1); wait_ms(1000); speaker(1); wait_ms(200); LED_right(0); LED_center(0); LED_left(0); } // 停止 （ロック状態）------------------------ void stop_Grip(unsigned int t) { McwR=McwL=0; // モーター 停止 MpcntR=MpcntL=0; // モーター パルスカウントクリアー Motor=MpatL[MsL] | MpatR[MsR]; wait_ms(t); // 停止 tミリ秒 ITU1.GRA=Mclock0; // 右モーター 割り込み時間 reset ITU2.GRA=Mclock0; // 左モーター 割り込み時間 reset Mclock=Mclock0; // スタート時モータークロック reset } // 停止 （ロック状態--->ロック解除）------------------------ void stop_Free(int t) { McwR=McwL=0; // モーター 停止 MpcntR=MpcntL=0; // モーター パルスカウントクリアー Motor=MpatL[MsL] | MpatR[MsR]; wait_ms(t); // 停止 tミリ秒 Motor = 0x00; // ロック解除 ITU1.GRA=Mclock0; // 右モーター 割り込み時間 reset ITU2.GRA=Mclock0; // 左モーター 割り込み時間 reset Mclock=Mclock0; // スタート時モータークロック reset } // 局地(左90°)回転 ---------------------------------- void turn90L() { if (Run_Level==0) { stop_Grip(100);} else { stop_Grip(25);} ITU1.GRA=Mclock1; // 右モーター 割り込み時間 set ITU2.GRA=Mclock1; // 左モーター 割り込み時間 set McwR=1; // 正回転 McwL=2; // 逆回転 while(MpcntL