#include <3687.h> // H8 Tinyの内部I/O定義 // 送受信用バッファ char txb[20], rxb[20]; char txb_2[20], rxb_2[20]; /*----------------------------------------------------------------------------*/ /* */ /* */ /* その他関数 */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------------------*/ /* 引数x20msウェイト */ /* (CPUクロック20MHz、1/128クロックで1カウントモード、156倍で19968クロック=約1mS) */ /*----------------------------------------------------------------------------------------*/ void wait (unsigned char speed) { long spd; spd = speed * 20; //引数を20倍 while (spd){ /*1mSウェイト*/ TV.TCRV0.BYTE = 0x03; //下の行と合わせて1/128クロックで1カウントモード TV.TCRV1.BYTE = 0X01; while(TV.TCNTV < 156); TV.TCNTV = 0; //カウンタクリア spd--; } } /*----------------------------------------------------------------------------------------*/ /* 引数x10uSウェイト */ /* (CPUクロック20MHz、1/8クロックで1カウントモード、25カウントで=10uS) */ /* ※大きい引数の場合はオーバーヘッド時間の差で遅れます。小さい時間のウェイトに向いている */ /*----------------------------------------------------------------------------------------*/ void wait_10us (unsigned char speed) { while (speed){ /*10uSウェイト*/ TV.TCRV0.BYTE = 0x01; //下の行と合わせて1/8クロックで1カウントモード TV.TCRV1.BYTE = 0X01; while(TV.TCNTV < 25); //25カウントで10uS TV.TCNTV = 0; //カウンタクリア speed--; } } /*----------------------------------------------------------------------------*/ /* */ /* */ /* ステップデータ関連 */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*--------------------------------------------------*/ /* 歩行ステップ1(右移動) */ /*--------------------------------------------------*/ void RoboMotion_hokou_1( unsigned char spd) { unsigned char step1[24] = {0,153,1,157,2,100,3,123,4,139,5,127,6,133,7,153,8,157,9,146,10,149,11,137}; snd_step(step1,spd); } /*--------------------------------------------------*/ /* 左足上げ */ /*--------------------------------------------------*/ void RoboMotion_hokou_Lup( unsigned char spd) { unsigned char step2[24] = {0,143,1,157,2,160,3,63,4,139,5,127,6,133,7,143,8,137,9,156,10,154,11,127}; snd_step(step2,spd); } /*--------------------------------------------------*/ /* 歩行ステップ3(左足前) */ /*--------------------------------------------------*/ void RoboMotion_hokou_3( unsigned char spd) { unsigned char step3[24] = {0,133,1,157,2,110,3,93,4,139,5,127,6,133,7,153,8,127,9,156,10,149,11,117}; snd_step(step3,spd); } /*--------------------------------------------------*/ /* 歩行ステップ4(左移動) */ /*--------------------------------------------------*/ void RoboMotion_hokou_4( unsigned char spd) { unsigned char step4[24] = {0,133,1,117,2,110,3,93,4,99,5,127,6,133,7,113,8,127,9,156,10,109,11,117}; snd_step(step4,spd); } /*--------------------------------------------------*/ /* 右足上げ */ /*--------------------------------------------------*/ void RoboMotion_hokou_Rup( unsigned char spd) { unsigned char step5[24] = {0,143,1,107,2,100,3,113,4,109,5,127,6,133,7,113,8,187,9,96,10,109,11,117}; snd_step(step5,spd); } /*--------------------------------------------------*/ /* 歩行ステップ6(右足前) */ /*--------------------------------------------------*/ void RoboMotion_hokou_6( unsigned char spd) { unsigned char step6[24] = {0,153,1,117,2,100,3,123,4,99,5,127,6,133,7,113,8,157,9,146,10,109,11,137}; snd_step(step6,spd); } /*--------------------------------------------------*/ /* 左体重移動 */ /*--------------------------------------------------*/ void RoboMotion_hokou_Lshift( unsigned char spd) { unsigned char step[24] = {0,143,1,117,2,100,3,113,4,99,5,127,6,133,7,113,8,137,9,156,10,109,11,127}; snd_step(step,spd); } /*--------------------------------------------------*/ /* 右体重移動 */ /*--------------------------------------------------*/ void RoboMotion_hokou_Rshift( unsigned char spd) { unsigned char step[24] = {0,143,1,157,2,100,3,113,4,139,5,127,6,133,7,153,8,137,9,156,10,149,11,127}; snd_step(step,spd); } /*--------------------------------------------------*/ /* 股ひらき */ /*--------------------------------------------------*/ void RoboMotion_hokou_LegOpen( unsigned char spd) { unsigned char step[24] = {0,143,1,157,2,100,3,113,4,139,5,127,6,133,7,113,8,137,9,156,10,109,11,127}; snd_step(step,spd); } /*--------------------------------------------------*/ /* ハッスル1 */ /*--------------------------------------------------*/ void RoboMotion_hustle1( unsigned char spd) { unsigned char step[24] = {0,143,1,137,2,100,3,133,4,119,5,127,6,133,7,133,8,117,9,156,10,129,11,127}; snd_step(step,spd); } /*--------------------------------------------------*/ /* ハッスル2 */ /*--------------------------------------------------*/ void RoboMotion_hustle2( unsigned char spd) { unsigned char step[24] = {0,173,1,137,2,140,3,153,4,119,5,127,6,133,7,133,8,97,9,116,10,129,11,97}; snd_step(step,spd); } /*--------------------------------------------------*/ /* 屈伸1 */ /*--------------------------------------------------*/ void RoboMotion_kussin1( unsigned char spd) { unsigned char step[24] = {0,163,1,137,2,140,3,93,4,119,5,127,6,133,7,133,8,157,9,116,10,129,11,107}; snd_step(step,spd); } /*--------------------------------------------------*/ /* ホームポジション */ /*--------------------------------------------------*/ void RoboMotion_home_P (unsigned char spd) { unsigned char step1[24] = {0,143,1,137,2,100,3,113,4,119,5,127,6,133,7,133,8,137,9,156,10,129,11,127}; snd_step(step1,spd); } /*--------------------------------------------------*/ /* ステップデータ送信 */ /*--------------------------------------------------*/ int snd_step ( unsigned char * snd, unsigned char snd_speed ) { unsigned char cnt; for (cnt = 0; cnt < 24; cnt=cnt+2){ SCI3_OUT_DATA(255); SCI3_OUT_DATA(snd[cnt]); SCI3_OUT_DATA(snd[cnt+1]); SCI3_OUT_DATA(snd_speed); } wait(snd_speed); return 0; } /*----------------------------------------------------------------------------*/ /* */ /* */ /* モーションデータ関連 */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*--------------------------------------------------*/ /* 前進歩行 引数はスピード */ /*--------------------------------------------------*/ void RoboMotion_hokou_FW (void) { RoboMotion_hokou_1(20); //STEP1右移動 RoboMotion_hokou_Lup(15); //STEP2左足上げ(他のモーションでも使用) RoboMotion_hokou_3(10); //STEP3左足前 RoboMotion_hokou_4(20); //STEP4左移動 RoboMotion_hokou_Rup(15); //STEP5右足上げ(他のモーションでも使用) RoboMotion_hokou_6(10); //STEP6右足前 } /*--------------------------------------------------*/ /* ゆっくり前進歩行 引数はスピード */ /*--------------------------------------------------*/ void RoboMotion_hokou_FW_SLOW (void) { RoboMotion_hokou_1(30); //STEP1右移動 RoboMotion_hokou_Lup(20); //STEP2左足上げ(他のモーションでも使用) RoboMotion_hokou_3(20); //STEP3左足前 RoboMotion_hokou_4(30); //STEP4左移動 RoboMotion_hokou_Rup(20); //STEP5右足上げ(他のモーションでも使用) RoboMotion_hokou_6(20); //STEP6右足前 } /*--------------------------------------------------*/ /* 後進(バック)歩行 */ /*--------------------------------------------------*/ void RoboMotion_hokou_BACK (void) { RoboMotion_hokou_6(30); //STEP6右足前 RoboMotion_hokou_Rup(20); //STEP5右足上げ(他のモーションでも使用) RoboMotion_hokou_4(20); //STEP4左移動 RoboMotion_hokou_3(30); //STEP3左足前 RoboMotion_hokou_Lup(20); //STEP2左足上げ(他のモーションでも使用) RoboMotion_hokou_1(20); //STEP1右移動 } /*--------------------------------------------------*/ /* 前に起き上がり */ /*--------------------------------------------------*/ void RoboMotion_okiagari_FW (void) { RoboMotion_home_P(60); //STEP1ホーム unsigned char step2[24] = {0,143,1,137,2,190,3,163,4,119,5,127,6,133,7,133,8,87,9,66,10,129,11,127}; snd_step(step2,35); //STEP2起き上がり1 // wait(10); unsigned char step3[24] = {0,193,1,137,2,170,3,133,4,119,5,127,6,133,7,133,8,117,9,86,10,129,11,77}; snd_step(step3,30); //STEP3起き上がり2 wait(20); RoboMotion_home_P(40); } /*--------------------------------------------------*/ /* 後ろに起き上がり */ /*--------------------------------------------------*/ void RoboMotion_okiagari_BW (void) { RoboMotion_home_P(60); //STEP1ホーム unsigned char step2[24] = {0,143,1,137,2,100,3,113,4,119,5,127,6,133,7,133,8,137,9,66,10,129,11,127}; snd_step(step2,20); //STEP2起き1 unsigned char step3[24] = {0,113,1,137,2,180,3,93,4,119,5,207,6,173,7,113,8,237,9,66,10,129,11,127}; snd_step(step3,30); //STEP3起き2 unsigned char step4[24] = {0,113,1,137,2,150,3,93,4,119,5,158,6,173,7,113,8,88,9,66,10,129,11,127}; snd_step(step4,30); //STEP4起き3 unsigned char step5[24] = {0,113,1,137,2,150,3,53,4,119,5,98,6,102,7,113,8,68,9,145,10,129,11,127}; snd_step(step5,30); //STEP5起き4 unsigned char step6[24] = {0,113,1,137,2,59,3,53,4,179,5,149,6,133,7,66,8,93,9,104,10,129,11,180}; snd_step(step6,30); //STEP6起き5 unsigned char step7[24] = {0,163,1,137,2,188,3,123,4,199,5,149,6,133,7,66,8,93,9,124,10,129,11,140}; snd_step(step7,60); //STEP7起き6 RoboMotion_home_P(40); //STEP8ホーム RoboMotion_okiagari_FW(); //前に起き上がり } /*--------------------------------------------------*/ /* 右倒れから起き上がり */ /*--------------------------------------------------*/ void RoboMotion_okiagari_R (void) { RoboMotion_home_P(60); //STEP1ホーム unsigned char step2[24] = {0,143,1,137,2,100,3,147,4,199,5,127,6,133,7,133,8,137,9,156,10,129,11,127}; snd_step(step2,30); //STEP2右起き1 // wait(20); unsigned char step3[24] = {0,143,1,137,2,100,3,147,4,199,5,127,6,81,7,133,8,107,9,86,10,129,11,147}; snd_step(step3,30); //STEP3右起き2 wait(30); //STEP4倒れるまで待ち RoboMotion_home_P(20); //STEP5ホーム RoboMotion_okiagari_FW(); //前に起き上がり } /*--------------------------------------------------*/ /* 左倒れから起き上がり */ /*--------------------------------------------------*/ void RoboMotion_okiagari_L (void) { RoboMotion_home_P(60); //STEP1ホーム unsigned char step2[24] = {0,143,1,137,2,100,3,113,4,119,5,177,6,133,7,56,8,102,9,141,10,129,11,127}; snd_step(step2,30); //STEP2左起き1 unsigned char step3[24] = {0,143,1,137,2,160,3,163,4,119,5,197,6,133,7,56,8,102,9,141,10,129,11,127}; snd_step(step3,20); //STEP3左起き2 wait(20); //STEP4倒れるまで待ち RoboMotion_home_P(20); //STEP5ホーム RoboMotion_okiagari_FW(); //前に起き上がり } /*--------------------------------------------------*/ /* 左ターン */ /*--------------------------------------------------*/ void RoboMotion_hokou_Lturn (void) { RoboMotion_home_P(20); //STEP1ホームポジション RoboMotion_hokou_Lshift(10); //STEP2左体重移動 RoboMotion_hokou_Rup(10); //STEP3右足上げ unsigned char step4[24] = {0,143,1,117,2,100,3,113,4,109,5,187,6,133,7,113,8,157,9,136,10,109,11,117}; snd_step(step4,20); //STEP4左旋回1 unsigned char step5[24] = {0,143,1,137,2,100,3,113,4,119,5,177,6,133,7,133,8,157,9,136,10,129,11,117}; snd_step(step5,10); //STEP5左旋回2 RoboMotion_home_P(20); //STEP6ホームポジション } /*--------------------------------------------------*/ /* 右ターン */ /*--------------------------------------------------*/ void RoboMotion_hokou_Rturn (void) { RoboMotion_home_P(20); //STEP1ホームポジション RoboMotion_hokou_Rshift(10); //STEP2右体重移動 RoboMotion_hokou_Lup(10); //STEP3左足上げ unsigned char step4[24] = {0,113,1,157,2,80,3,93,4,139,5,127,6,73,7,143,8,137,9,156,10,149,11,127}; snd_step(step4,20); //STEP4右旋回1 unsigned char step5[24] = {0,123,1,137,2,80,3,93,4,119,5,127,6,103,7,133,8,137,9,156,10,129,11,117}; snd_step(step5,10); //STEP5右旋回2 RoboMotion_home_P(20); //STEP6ホームポジション } /*--------------------------------------------------*/ /* 右ステップ */ /*--------------------------------------------------*/ void RoboMotion_hokou_Rstep (void) { RoboMotion_home_P(20); //STEP1ホームポジション RoboMotion_hokou_Lshift(10); //STEP2左体重移動 RoboMotion_hokou_Rup(10); //STEP3右足上げ RoboMotion_hokou_LegOpen(20); //STEP4股ひらき unsigned char step5[24] = {0,143,1,177,2,100,3,113,4,139,5,127,6,133,7,153,8,137,9,156,10,139,11,127}; snd_step(step5,20); //STEP5左足寄せ unsigned char step6[24] = {0,143,1,157,2,160,3,63,4,139,5,127,6,133,7,143,8,137,9,156,10,154,11,127}; snd_step(step6,10); //STEP6左足上げ RoboMotion_home_P(20); //STEP7ホームポジション } /*--------------------------------------------------*/ /* 左ステップ */ /*--------------------------------------------------*/ void RoboMotion_hokou_Lstep (void) { RoboMotion_home_P(20); //STEP1ホームポジション RoboMotion_hokou_Rshift(10); //STEP2右体重移動 RoboMotion_hokou_Lup(10); //STEP3左足上げ RoboMotion_hokou_LegOpen(20); //STEP4股ひらき unsigned char step5[24] = {0,143,1,127,2,100,3,113,4,99,5,127,6,133,7,133,8,137,9,156,10,79,11,127}; snd_step(step5,20); //STEP5右足寄せ unsigned char step6[24] = {0,143,1,107,2,100,3,113,4,109,5,127,6,133,7,113,8,157,9,116,10,109,11,107}; snd_step(step6,10); //STEP6右足上げ RoboMotion_home_P(20); //STEP7ホームポジション } /*--------------------------------------------------*/ /* 屈伸 */ /*--------------------------------------------------*/ void RoboMotion_kussin (void) { RoboMotion_home_P(20); //STEP1ホームポジション unsigned char step2[24] = {0,203,1,137,2,220,3,53,4,119,5,127,6,133,7,133,8,197,9,36,10,129,11,67}; snd_step(step2,60); //STEP2屈伸3 RoboMotion_home_P(60); //STEP3ホームポジション } /*--------------------------------------------------*/ /* 3,2,1、ハッスルハッスル! */ /*--------------------------------------------------*/ void RoboMotion_hustle (void) { int i,j; RoboMotion_home_P(20); //STEP1ホームポジション for(i=0; i < 3; i++){ RoboMotion_hustle1(0); //STEP2ハッスルカウントx3 wait(30); RoboMotion_home_P(20); } for(j=0; j < 2; j++){ RoboMotion_hustle2(20); //STEPハッスルx2 RoboMotion_kussin1(20); } wait(100); RoboMotion_home_P(20); //STEP1ホームポジション } /*--------------------------------------------------*/ /* かかと落とし */ /*--------------------------------------------------*/ void RoboMotion_kakato (void) { RoboMotion_home_P(20); //STEP1ホームポジション unsigned char step2[24] = {0,143,1,112,2,100,3,113,4,94,5,127,6,133,7,108,8,137,9,156,10,104,11,127}; snd_step(step2,30); //STEP2左体重移動(かかと用) unsigned char step3[24] = {0,153,1,107,2,120,3,103,4,139,5,127,6,133,7,133,8,237,9,16,10,109,11,87}; snd_step(step3,30); //STEP3かかと1 unsigned char step4[24] = {0,153,1,107,2,140,3,103,4,134,5,137,6,133,7,133,8,237,9,166,10,109,11,147}; snd_step(step4,0); //STEP4かかと3 wait(30); //STEP5ため unsigned char step6[24] = {0,153,1,107,2,140,3,83,4,134,5,137,6,133,7,133,8,237,9,86,10,109,11,127}; snd_step(step6,0); //STEP6かかと4 wait(40); //STEP7ため unsigned char step8[24] = {0,143,1,107,2,100,3,113,4,134,5,127,6,133,7,133,8,147,9,156,10,109,11,127}; snd_step(step8,40); //STEP8かかと5 RoboMotion_home_P(30); //STEP9ホームポジション } /*--------------------------------------------------*/ /* 本昇り降り */ /*--------------------------------------------------*/ void RoboMotion_book (void) { RoboMotion_home_P(20); //STEP1ホームポジション RoboMotion_hokou_Rshift(10); //STEP2右体重移動 unsigned char step3[24] = {0,173,1,157,2,200,3,43,4,139,5,127,6,133,7,143,8,137,9,156,10,154,11,127}; snd_step(step3,30); //STEP3左足大きく上げ unsigned char step4[24] = {0,78,1,157,2,90,3,63,4,139,5,127,6,133,7,143,8,117,9,156,10,154,11,127}; snd_step(step4,30); //STEP4左足大きく上げ2 unsigned char step5[24] = {0,103,1,147,2,120,3,63,4,129,5,127,6,133,7,143,8,97,9,156,10,139,11,107}; snd_step(step5,30); //STEP5本2 unsigned char step6[24] = {0,88,1,127,2,100,3,13,4,119,5,127,6,133,7,133,8,147,9,156,10,109,11,107}; snd_step(step6,30); //STEP6本4 unsigned char step7[24] = {0,118,1,107,2,100,3,13,4,119,5,127,6,133,7,133,8,147,9,156,10,109,11,57}; snd_step(step7,20); //STEP7本5 wait(10); unsigned char step8[24] = {0,133,1,107,2,100,3,73,4,119,5,127,6,133,7,133,8,147,9,156,10,109,11,127}; snd_step(step8,30); //STEP8本6 unsigned char step9[24] = {0,143,1,107,2,100,3,113,4,109,5,127,6,133,7,113,8,167,9,96,10,109,11,97}; snd_step(step9,20); //STEP9右足ちょい上げ RoboMotion_home_P(20); //STEP10ホームポジション wait(50); //−−−以下降りる−−− unsigned char step12[24] = {0,143,1,157,2,100,3,113,4,129,5,127,6,133,7,133,8,137,9,156,10,159,11,127}; snd_step(step12,30); //STEP12一歩1 wait(10); unsigned char step13[24] = {0,123,1,147,2,100,3,83,4,129,5,127,6,133,7,133,8,137,9,156,10,139,11,122}; snd_step(step13,30); //STEP13一歩2 wait(10); unsigned char step14[24] = {0,133,1,107,2,100,3,83,4,119,5,127,6,133,7,113,8,177,9,116,10,119,11,122}; snd_step(step14,30); //STEP14一歩3 wait(10); RoboMotion_home_P(30); //STEP15ホームポジション wait(30); unsigned char step16[24] = {0,143,1,157,2,100,3,113,4,139,5,127,6,133,7,153,8,137,9,156,10,159,11,127}; snd_step(step16,30); //STEP16体重移動 wait(10); unsigned char step17[24] = {0,123,1,137,2,200,3,63,4,139,5,127,6,133,7,133,8,137,9,156,10,159,11,127}; snd_step(step17,50); //STEP17降りる1 unsigned char step18[24] = {0,73,1,147,2,100,3,23,4,109,5,127,6,133,7,123,8,197,9,36,10,159,11,67}; snd_step(step18,50); //STEP18降りる2 unsigned char step19[24] = {0,113,1,147,2,100,3,23,4,109,5,127,6,133,7,123,8,197,9,36,10,159,11,67}; snd_step(step19,50); //STEP19降りる3 unsigned char step20[24] = {0,68,1,137,2,80,3,23,4,119,5,127,6,133,7,133,8,197,9,36,10,129,11,37}; snd_step(step20,50); //STEP20降りる5 unsigned char step21[24] = {0,88,1,107,2,80,3,3,4,119,5,127,6,133,7,133,8,217,9,56,10,109,11,37}; snd_step(step21,50); //STEP21降りる6 unsigned char step22[24] = {0,108,1,107,2,80,3,33,4,129,5,127,6,133,7,133,8,197,9,86,10,129,11,37}; snd_step(step22,50); //STEP22降りる7 unsigned char step23[24] = {0,128,1,107,2,100,3,73,4,129,5,127,6,133,7,133,8,217,9,76,10,129,11,97}; snd_step(step23,50); //STEP23降りる8 RoboMotion_home_P(50); //STEP24ホームポジション } /*--------------------------------------------------*/ /* 審査モーション */ /*--------------------------------------------------*/ void RoboMotion_shinsa (void) { int i; RoboMotion_okiagari_FW(); wait(50); RoboMotion_kussin(); wait(50); RoboMotion_hokou_Rstep(); wait(50); RoboMotion_hokou_Lstep(); wait(50); for (i=0; i<4; i++){ RoboMotion_hokou_FW_SLOW(); } wait(50); RoboMotion_home_P(50); } // RoboMotion_hokou_Lturn(); // RoboMotion_hokou_Rturn(); // RoboMotion_hokou_FW(); // RoboMotion_hokou_BACK(); // RoboMotion_okiagari_FW(); // RoboMotion_okiagari_BW(); // RoboMotion_kakato(); // RoboMotion_book(); // RoboMotion_hokou_Rstep(); // RoboMotion_hokou_Lstep(); // RoboMotion_kussin(); // RoboMotion_hustle(); /*----------------------------------------------------------------------------*/ /* */ /* */ /* 2軸加速度センサー */ /* P12をX出力へ */ /* P11をY出力へ接続 */ /*----------------------------------------------------------------------------*/ /*------------------------------------------------------*/ /* X軸計測関数(P12からパルス入力) */ /*------------------------------------------------------*/ unsigned int Acc_X_OUT(void) { unsigned int tilt = 0; while (IO.PDR1.BIT.B2); //パルスの立下りまで待つ(P12) while (!IO.PDR1.BIT.B2); //パルスの立上がりまで待つ TB1.TMB1.BYTE = 0xFA; //タイマーB1、クロックの1/512、オートリロード TB1.TLB1 = 0x00; //カウンタスタート(オーリロード) while (IO.PDR1.BIT.B2); //パルスの立下りまで待つ(計測終了) tilt = TB1.TCB1; //タイマーの値を変数へ格納 return tilt; //結果を返す } /*------------------------------------------------------*/ /* Y軸計測関数(P11からパルス入力) */ /*------------------------------------------------------*/ unsigned int Acc_Y_OUT(void) { unsigned int tilt = 0; while (IO.PDR1.BIT.B1); //パルスの立下りまで待つ(P11) while (!(IO.PDR1.BIT.B1)); //パルスの立上りまで待つ TB1.TMB1.BYTE = 0xFA; //タイマーB1、クロックの1/512、オートリロード TB1.TLB1 = 0x00; //カウンタスタート(オーリロード) while (IO.PDR1.BIT.B1); //パルスの立下りまで待つ(計測終了) tilt = TB1.TCB1; //タイマーの値を変数へ格納 return tilt; //結果を返す } /*----------------------------------------------------------------------------*/ /* */ /* */ /* 超音波測距センサー */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------------------*/ /* 超音波距離センサーの距離測定(cmで返す) */ /* P34:トリガー入力 */ /* P35:結果出力 */ /*----------------------------------------------------------------------------------------*/ unsigned int US_OUT (void) { IO.PCR3 = 0x10; //P34を出力ポート,P35を入力ポートへ設定 unsigned int length = 0; //距離格納変数 IO.PDR3.BIT.B4 = 1; //P11(トリガ)をHigh wait_10us(2); //20uSウェイト IO.PDR3.BIT.B4 = 0; //P11(トリガ)をLow(超音波センサー測定開始) while (!(IO.PDR3.BIT.B5)); //センサーの出力が立ち上がるまで待つ TB1.TMB1.BYTE = 0xFA; //タイマーB1、クロックの1/512、オートリロード TB1.TLB1 = 0x00; //カウンタスタート(オーリロード) while (IO.PDR3.BIT.B5); //センサーの出力が立下りまで待つ(計測終了) length = TB1.TCB1 / 2.4; //タイマーの値を1cm単位に変換 return length; } /*----------------------------------------------------------------------------*/ /* */ /* */ /* 色センサー */ /* ※通信はSCI3_2を使用 */ /* */ /*----------------------------------------------------------------------------*/ /*--------------------------------------------------*/ /* 色センサー初期化 */ /*--------------------------------------------------*/ void iro_S_INIT( void ) { /*命令の最後にCR(\r)をつける。ASCIIコードだと0x0D。*/ wait(50); SCI3_2_OUT_STRING("RS\r"); //色センサーリセット while(SCI3_2_IN_DATA() == ':'); wait(5); SCI3_2_OUT_STRING("L1 1\r"); //緑LEDをON while(SCI3_2_IN_DATA() == ':'); wait(5); SCI3_2_OUT_STRING("CR 18 44\r"); //オートゲイン、ホワイトバランスON while(SCI3_2_IN_DATA() == ':'); wait(250); //ホワイトバランスを取るまで5秒待つ SCI3_2_OUT_STRING("CR 18 44 19 32\r"); //オートゲイン、ホワイトバランス設定終了 while(SCI3_2_IN_DATA() == ':'); wait(5); SCI3_2_OUT_STRING("L1 2\r"); //緑LEDをモード2へ while(SCI3_2_IN_DATA() == ':'); wait(5); SCI3_2_OUT_STRING("PM 1\r"); //Pollモードへ設定 while(SCI3_2_IN_DATA() == ':'); wait(5); SCI3_2_OUT_STRING("RM 3\r"); //バイナリデータ出力モード while(SCI3_2_IN_DATA() == ':'); wait(5); SCI3_2_OUT_STRING("L1 0\r"); wait(50); SCI3_2_OUT_STRING("L1 1\r"); wait(5); //色センサーに命令後は少し待たないとNG } /*--------------------------------------------------*/ /* 色読み込み&PCへ出力(通信ソフトを使用) */ /*--------------------------------------------------*/ /* void iro_S_COLOR_OUT( void ) { int i; SCI3_2_IN_DATA_CLEAR (); unsigned char rsv[8]; SCI3_2_OUT_STRING("GM\r"); //色読み込み for (i=0; i < 8; i++){ rsv[i] = SCI3_2_IN_DATA(); } SCI3_PRINTF("0:=%d\n",rsv[0]); SCI3_PRINTF("1:=%c\n",rsv[1]); SCI3_PRINTF("2:R=%d\n",rsv[2]); SCI3_PRINTF("3:G=%d\n",rsv[3]); SCI3_PRINTF("4:B=%d\n",rsv[4]); SCI3_PRINTF("5:R_Div=%d\n",rsv[5]); SCI3_PRINTF("6:G_Div=%d\n",rsv[6]); SCI3_PRINTF("7:B_Div=%d\n\n",rsv[7]); } */ /*--------------------------------------------------*/ /* 画面中心位置の色を記憶 */ /*--------------------------------------------------*/ void iro_S_COLOR_MEMORY( void ) { // SCI3_PRINTF("MEM-START!!\n"); int i; SCI3_IN_DATA_CLEAR (); SCI3_2_IN_DATA_CLEAR (); unsigned char rsv1[8]; SCI3_2_OUT_STRING("TW\r"); //TW、色の記憶命令 while(SCI3_2_IN_DATA() == ':'); for (i=0; i < 8; i++){ rsv1[i] = SCI3_2_IN_DATA(); //記憶後はSパケットを返す } wait(5); //色センサーに命令後は少し待たないとNG // SCI3_PRINTF("0:=%d\n",rsv1[0]); //以下、Sパケット表示 // SCI3_PRINTF("1:=%c\n",rsv1[1]); // SCI3_PRINTF("2:記憶R=%d\n",rsv1[2]); // SCI3_PRINTF("3:記憶G=%d\n",rsv1[3]); // SCI3_PRINTF("4:記憶B=%d\n",rsv1[4]); } /*--------------------------------------------------*/ /* 色の追跡 */ /*--------------------------------------------------*/ unsigned char iro_S_COLOR_TRACE( void ) { // SCI3_PRINTF("TRACE-START!!\n"); unsigned char DIR; //方向格納変数 int i; SCI3_2_IN_DATA_CLEAR (); unsigned char rsv[10]; SCI3_2_OUT_STRING("TC\r"); //TC、色追跡命令 // while(SCI3_2_IN_DATA() == ':'); for (i=0; i < 10; i++){ rsv[i] = SCI3_2_IN_DATA(); //Mパケットを返す } DIR = 0; if (rsv[2] > 15 && rsv[2] < 65){ if (rsv[3] > 80) DIR = 1; //前にある if (rsv[3] < 30) DIR = 2; //後ろにある } if (rsv[2] <= 15) DIR = 3; //右にある if (rsv[2] >= 65) DIR = 4; //左にある if (rsv[8] == 0) DIR = 0; // SCI3_PRINTF("0:=%d\n",rsv[0]); //以下、Mパケット表示 // SCI3_PRINTF("1:=%c\n",rsv[1]); // SCI3_PRINTF("2:中心X=%d\n",rsv[2]); // SCI3_PRINTF("3:中心Y=%d\n",rsv[3]); // SCI3_PRINTF("4:左上X=%d\n",rsv[4]); // SCI3_PRINTF("5:左上Y=%d\n",rsv[5]); // SCI3_PRINTF("6:右下X=%d\n",rsv[6]); // SCI3_PRINTF("7:右下Y=%d\n\n",rsv[7]); // SCI3_PRINTF("8:マッチしたピクセル数=%d\n",rsv[8]); // SCI3_PRINTF("9:マッチ度=%d\n\n",rsv[9]); return DIR; } /*--------------------------------------------------*/ /* 色の追跡&歩行 */ /*--------------------------------------------------*/ void iro_S_hokou(void) { unsigned char DIRECTION =0; DIRECTION = iro_S_COLOR_TRACE(); // SCI3_PRINTF("\nDIR=%d\n",DIRECTION); if (DIRECTION == 1) RoboMotion_hokou_FW(); if (DIRECTION == 2) RoboMotion_hokou_BACK(); if (DIRECTION == 3) RoboMotion_hokou_Rturn(); if (DIRECTION == 4) RoboMotion_hokou_Lturn(); wait(5); } /*-----------------------------------------------*/ /* バトルモード */ /*-----------------------------------------------*/ void BATTLE_MODE(void) { unsigned int TILT_X; unsigned int TILT_Y; /*色センサー初期化*/ iro_S_INIT(); iro_S_COLOR_MEMORY(); while(1){ if (US_OUT() < 3) RoboMotion_kakato(); TILT_X = Acc_X_OUT(); TILT_Y = Acc_Y_OUT(); if (TILT_X < 155) RoboMotion_okiagari_BW(); if (TILT_X > 245) RoboMotion_okiagari_FW(); if (TILT_Y > 245) RoboMotion_okiagari_R(); if (TILT_Y < 155) RoboMotion_okiagari_L(); iro_S_hokou(); wait(5); // SCI3_PRINTF("X_tilt=%d\n",Acc_X_OUT()); // SCI3_PRINTF("Y_tilt=%d\n",Acc_Y_OUT()); // SCI3_PRINTF("Length=%d\n\n",US_OUT()); // wait(50); } } /*----------------------------------------------------------------------------*/ /* */ /* */ /* メインルーチン */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*-----------------------------------------------*/ /* メイン */ /*-----------------------------------------------*/ void main(void) { /* 通信ポートを初期化 ボーレートは9600[bps]*/ SCI3_INIT (br9600, txb, sizeof(txb), rxb, sizeof(rxb)); SCI3_2_INIT (br9600, txb_2, sizeof(txb_2), rxb_2, sizeof(rxb_2)); EI; // 初期化後は割り込みを許可状態に unsigned int DISTANCE; int i; SCI3_PRINTF("START!!"); wait(5); RoboMotion_home_P(30); wait(150); /*起動時のモード切替*/ DISTANCE = US_OUT(); if (DISTANCE < 10) RoboMotion_shinsa(); //仰向けに倒れた状態からスタート時は審査モーションへ if (DISTANCE < 30){ //背後10〜30cmに手をかざすとバトルモードへ RoboMotion_kussin(); //バトルモード切替の合図で屈伸一回(この後、色ボールを見せる) BATTLE_MODE(); } /*-----予選-----*/ RoboMotion_book(); wait(150); RoboMotion_hustle(); wait(10); RoboMotion_kussin(); RoboMotion_hokou_Rstep(); RoboMotion_hokou_Lstep(); for (i=0; i<2; i++){ RoboMotion_hokou_FW_SLOW(); } for (i=0; i < 2; i++){ RoboMotion_hokou_BACK(); } RoboMotion_home_P(30); for (i=0; i < 2; i++){ RoboMotion_hokou_Lturn(); } /*-----バトルモード----*/ BATTLE_MODE(); /*関数もくじ*/ /*動き*/ // RoboMotion_hokou_Lturn(); // RoboMotion_hokou_Rturn(); // RoboMotion_hokou_FW(); // RoboMotion_hokou_BACK(); // RoboMotion_okiagari_FW(); // RoboMotion_okiagari_BW(); // RoboMotion_kakato(); // RoboMotion_book(); // RoboMotion_hokou_Rstep(); // RoboMotion_hokou_Lstep(); // RoboMotion_kussin(); // RoboMotion_hustle(); /*色による歩行*/ // iro_S_hokou(); /*傾きを返す*/ // Acc_X_OUT(); // Acc_Y_OUT(); /*距離を返す*/ // US_OUT(); }