//---------------------------------------------------------------------- // MUKUV CPU H8/3052 25MHz // // include　file // // Copyright (C) 2003 Mizuno Kazuo //----------------------------------------------------------------------- #define Start_sw P4.DR.BIT.B4 // start sw P44 #define Mode_sw P4.DR.BIT.B5 // mode sw P45 #define MotorP PB.DR.BYTE // モーター ポート名 // 走行パラメータ ************************* #define pR90 165 // 右90 °回転パルス数 #define pL90 160 // 左90 °回転パルス数 #define pL180 325 // 左180°回転パルス数 #define pBLK1 490 // １区画直進走行パルス数 (0.368mm/p) #define Mclock0 6250 // モーター基準クロック (1ms=6250) #define comp_wall 500 // 前壁 距離補正センサー値 #define down_wall 250 // 前壁 減速限界センサー値 #define cont_c 420 // 前壁 距離補正パルス数 #define cont_s 200 // 姿勢制御 横壁センサー値 #define cont_g0 4 // 姿勢制御 ゲイン #define Sopen2 70 // １/２区画前の横壁有無 センサー値 #define Fopen2 50 // １/２区画前の前壁有無 センサー値 #define SR_bas 370 // 右横壁センサー基準値 #define SL_bas 310 // 左横壁センサー基準値 // モーター ************************* unsigned int MpatR[9] = {0x00,0x01,0x03,0x02,0x06,0x04,0x0c,0x08,0x09}; // 右モーター１・２相励磁パターン unsigned int MpatL[9] = {0x00,0x90,0x80,0xc0,0x40,0x60,0x20,0x30,0x10}; // 左モーター１・２相励磁パターン unsigned char MsL=0,MsR=0; // モーター励磁パターン No unsigned char McwL=0,McwR=0; // モーター回転方向　０：停止　１：正　２：逆 int MpcntR,MpcntL; // モーター パルスカウント int clockR,clockL; // モーター(next)クロック unsigned char adv; // 発進加速　０：停止から直進　１：直進中 unsigned char adv_clock; // 発進加速度クロック unsigned int Mclock; // モータークロック unsigned int cont_g; // 姿勢制御 ゲイン // 壁センサー************************* unsigned int adFR,adFL,adSR,adSL; // 壁センサー　AD変換値 unsigned char sens_no; // センサー発光順　NO. // その他***************************** unsigned char crash; //衝突時の強制停止 0:走行 1:強制停止 unsigned char Run_Level; //完走レベル 0:未完走 1:検索 2:最短走行 3:スラローム //***** I/O 初期設定 ******* void IO_init(void) { PB.DDR = 0xff; // Motorポート(B) PB.DR.BYTE= 0x00; P5.DDR = 0xff; // センサーLEDポート(5) P6.DDR = 0xff; // LEDポート(6) P4.DDR = 0x00; P4.PCR.BIT.B4 = 1; // StartSW(P44) プルアップ P4.PCR.BIT.B5 = 1; // ModeSW(P45) プルアップ //---- 割り込み　壁センサー ITU0 --- ITU0.TCR.BIT.CCLR=1; // ＧＲＡのコンペアマッチで TCNT クリア ITU0.TCR.BIT.TPSC=1; // コンペアマッチ クロック 1:φ/2 2:φ/4 ITU0.TIER.BIT.IMIEA=1; // コンペアマッチ割り込み許可 //---- 割り込み　右モーター 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; // コンペアマッチ割り込み許可 } // 走行パラメータの初期設定---------------------------- void runData_init() { //壁センサー ITU0.GRA=3000; // 割り込み時間 (0.24ms) // ITU.TSTR.BIT.STR0=1; // カウント開始 cont_g=cont_g0; // 姿勢制御 ゲイン //モーター Mclock=Mclock0; adv_clock=Mclock0*3/150; // 発進加速度クロック ITU1.GRA=Mclock0; // 右モーター 割り込み時間 ITU2.GRA=Mclock0; // 左モーター 割り込み時間 McwR=0; // モーター 停止 McwL=0; MotorP = 0x00; // ロック解除 MpcntR=0; // モーター パルスカウントクリアー MpcntL=0; // ITU1.TCNT = 0; // カウンタークリアー // ITU2.TCNT = 0; // ITU.TSTR.BIT.STR1=1; // カウント開始 // ITU.TSTR.BIT.STR2=1; Run_Level=0; //完走レベル } //***** センサー・モーターのカウント 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; } //****** タイマ ITU0　割り込み関数 （センサー） ***** void int_imia0(void) { sens_no++; if(sens_no>3) sens_no=0; switch(sens_no) { case 0: // 右横 LED AD.ADCSR.BIT.CH = 7; // チャンネル AN7 P5.DR.BIT.B0=1; // on wait(12); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P5.DR.BIT.B0=0; // off adSR=(AD.ADDRD>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; case 1: // 左横 LED AD.ADCSR.BIT.CH = 4; // チャンネル AN4 P5.DR.BIT.B1=1; // on wait(12); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P5.DR.BIT.B1=0; // off adSL=(AD.ADDRA>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; case 2: // 左前 LED AD.ADCSR.BIT.CH = 6; // チャンネル AN6 P5.DR.BIT.B2=1; // on wait(12); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P5.DR.BIT.B2=0; // off adFL=(AD.ADDRC>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; case 3: // 右前 LED AD.ADCSR.BIT.CH = 5; // チャンネル AN5 P5.DR.BIT.B3=1; // on wait(12); AD.ADCSR.BIT.ADST = 1; // 変換開始 while(AD.ADCSR.BIT.ADF==0) {} // 変換終了待ち P5.DR.BIT.B3=0; // off adFR=(AD.ADDRB>>6)&0x3ff; // 数値読み込み AD.ADCSR.BIT.ADF=0; // 変換終了フラグクリア break; default:break; } ITU0.TSR.BIT.IMFA=0; // 割り込みフラグクリア //衝突時の強制停止 if((Start_sw==0) || (Mode_sw==0)) { crash=1; wait(2000); } } //***** 直進姿勢制御 ***** void pos_cont() { int cont_p; // 姿勢制御量 cont_p=0; if(adSL>cont_s && adSR>cont_s) { //両壁で制御 cont_p = (adSL - SL_bas) - (adSR - SR_bas); }else if(adSL>cont_s) { //左壁で制御 cont_p = (adSL - SL_bas) *2; }else if(adSR>cont_s) { //右壁で制御 cont_p = (SR_bas - adSR) *2; } cont_p=cont_p*cont_g; if ((MpcntR<150) && (adv=0)) { // 停止からの発進加速 clockR=(Mclock + cont_p)*4 - adv_clock*MpcntR; clockL=(Mclock - cont_p)*4 - adv_clock*MpcntL; } else { clockR=Mclock + cont_p; clockL=Mclock - cont_p; } } //****** タイマ ITU1　割り込み関数 （右モーター） ***** void int_imia1(void) { switch (McwR) { case 0: // 停止 // MsR=0; // MotorP = 0x00; break; case 1: // 正回転 MsR++; if(MsR>8) MsR=1; MotorP = MpatR[MsR] | MpatL[MsL]; MpcntR++; if (McwL==1) { // 直進姿勢制御 pos_cont(); ITU1.GRA=clockR; } break; case 2: // 逆回転 MsR--; if(MsR<1) MsR=8; MotorP = MpatR[MsR] | MpatL[MsL]; MpcntR++; break; case 3: // スラローム（正回転） MsR++; if(MsR>8) MsR=1; MotorP = MpatR[MsR] | MpatL[MsL]; MpcntR++; break; default:break; } ITU1.TSR.BIT.IMFA=0; // 割り込みフラグクリア } //****** タイマ ITU2　割り込み関数 （左モーター） ***** void int_imia2(void) { switch (McwL) { case 0: // 停止 // MsL=0; // MotorP = 0x00; break; case 1: // 正回転 MsL++; if(MsL>8) MsL=1; MotorP = MpatR[MsR] | MpatL[MsL]; MpcntL++; if (McwR==1) { // 直進姿勢制御 ITU2.GRA=clockL; } break; case 2: // 逆回転 MsL--; if(MsL<1) MsL=8; MotorP = MpatR[MsR] | MpatL[MsL]; MpcntL++; break; case 3: // スラローム（正回転） MsL++; if(MsL>8) MsL=1; MotorP = MpatR[MsR] | MpatL[MsL]; MpcntL++; break; default:break; } ITU2.TSR.BIT.IMFA=0; // 割り込みフラグクリア } //***** LED点滅　右横(P63) 0:off 1:on etc:点滅 ***** void LED_right_side(int p) { switch(p) { case 0: // off P6.DR.BIT.B3 = 0; break; case 1: // on P6.DR.BIT.B3 = 1; break; default: // 点滅 while(p--) { P6.DR.BIT.B3 = 1; wait_ms(100); P6.DR.BIT.B3 = 0; wait_ms(100); } } } //***** LED点滅　右前(P64) 0:off 1:on etc:点滅 ***** void LED_right_front(int p) { switch(p) { case 0: // off P6.DR.BIT.B4 = 0; break; case 1: // on P6.DR.BIT.B4 = 1; break; default: // 点滅 while(p--) { P6.DR.BIT.B4 = 1; wait_ms(100); P6.DR.BIT.B4 = 0; wait_ms(100); } } } //***** LED点滅　左前(P65) 0:off 1:on etc:点滅 ***** void LED_left_front(int p) { switch(p) { case 0: // off P6.DR.BIT.B5 = 0; break; case 1: // on P6.DR.BIT.B5 = 1; break; default: // 点滅 while(p--) { P6.DR.BIT.B5 = 1; wait_ms(100); P6.DR.BIT.B5 = 0; wait_ms(100); } } } //***** LED点滅　左横(P66) 0:off 1:on etc:点滅 ***** void LED_left_side(int p) { switch(p) { case 0: // off P6.DR.BIT.B6 = 0; break; case 1: // on P6.DR.BIT.B6 = 1; break; default: // 点滅 while(p--) { P6.DR.BIT.B6 = 1; wait_ms(100); P6.DR.BIT.B6 = 0; wait_ms(100); } } } //***** スピーカー P61 ***** void speaker(int p) { int n; while(p--) { for(n=1;n<200;n++) { P6.DR.BIT.B1 = 1; wait(120); P6.DR.BIT.B1 = 0; wait(120); } wait(50); } } // 停止 （ロック解除）----------------------- void stop_Free() { McwR=0; // モーター 停止 McwL=0; // ITU.TSTR.BIT.STR1=0; // カウント停止 // ITU.TSTR.BIT.STR2=0; // ITU1.TCNT = 0; // カウンタークリアー // ITU2.TCNT = 0; MotorP = 0x00; // ロック解除 adv=0; // 停止中 } // 停止 （ロック状態）------------------------ void stop_Grip(int t) { McwR=0; // モーター 停止 McwL=0; // ITU.TSTR.BIT.STR1=0; // カウント停止 // ITU.TSTR.BIT.STR2=0; // ITU1.TCNT = 0; // カウンタークリアー // ITU2.TCNT = 0; MotorP = MpatR[MsR] | MpatL[MsL]; // ロック継続 wait_ms(t); // 停止 tミリ秒 adv=0; // 停止中 } // 停止 （ロック状態--->ロック解除）------------------------ void stop_T(int t) { McwR=0; // モーター 停止 McwL=0; // ITU.TSTR.BIT.STR1=0; // カウント停止 // ITU.TSTR.BIT.STR2=0; // ITU1.TCNT = 0; // カウンタークリアー // ITU2.TCNT = 0; MotorP = MpatR[MsR] | MpatL[MsL]; // ロック継続 wait_ms(t); // 停止 tミリ秒 MotorP = 0x00; // ロック解除 adv=0; // 停止中 } // 光センサー値--->RS232C---->PC void sens_PC() { char s[5]; speaker(2); 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"); } //***** スタートまでのカウントダウン ************* void count_down1() { // ITU.TSTR.BIT.STR0=0; // カウント停止 // ITU.TSTR.BIT.STR1=0; // ITU.TSTR.BIT.STR2=0; TCNT_SW(0); // カウント停止 speaker(1); LED_right_side(1); wait_ms(1000); speaker(1); LED_right_front(1); wait_ms(1000); speaker(1); LED_left_front(1); wait_ms(1000); speaker(1); LED_left_side(1); wait_ms(200); LED_right_side(0); LED_right_front(0); LED_left_front(0); LED_left_side(0); // TCNT_SW(1); // カウント開始 // ITU.TSTR.BIT.STR0=1; // カウント開始 // ITU.TSTR.BIT.STR1=1; // ITU.TSTR.BIT.STR2=1; // ITU1.TCNT = 0; // カウンタークリアー // ITU2.TCNT = 0; } //***** ゴール ********** void finish() { unsigned char n; // ITU.TSTR.BIT.STR0=0; // カウント停止 // ITU.TSTR.BIT.STR1=0; // ITU.TSTR.BIT.STR2=0; TCNT_SW(0); // カウント停止 speaker(1); for (n=0;n<3;n++) { LED_right_side(1); LED_right_front(1); LED_left_front(1); LED_left_side(1); wait_ms(250); LED_right_side(0); LED_right_front(0); LED_left_front(0); LED_left_side(0); wait_ms(250); } // TCNT_SW(1); // カウント開始 // ITU.TSTR.BIT.STR0=1; // カウント開始 // ITU.TSTR.BIT.STR1=1; // ITU.TSTR.BIT.STR2=1; // ITU1.TCNT = 0; // カウンタークリアー // ITU2.TCNT = 0; }