#include <3694.h> // H8 Tinyの内部I/O定義 // 送受信用バッファ char txb[20], rxb[20]; /*----------------------------------------------------------------------------*/ /* */ /* */ /* その他関数 */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------------------*/ /* 引数ｘ20msウェイト */ /* (CPUクロック２０MHｚ、1/128クロックで１カウントモード、156倍で19968クロック＝約１ｍＳ) */ /*----------------------------------------------------------------------------------------*/ void wait (unsigned char speed) { long spd; spd = speed * 20; //引数を２０倍 while (spd){ /*１ｍＳウェイト*/ TV.TCRV0.BYTE = 0x03; //下の行と合わせて1/128クロックで1カウントモード TV.TCRV1.BYTE = 0X01; while(TV.TCNTV < 156); TV.TCNTV = 0; //カウンタクリア spd--; } } /*----------------------------------------------------------------------------------------*/ /* 引数ｘ10uSウェイト */ /* (CPUクロック２０MHｚ、1/8クロックで１カウントモード、25カウントで＝１0uS) */ /* ※大きい引数の場合はオーバーヘッド時間の差で遅れます。小さい時間のウェイトに向いている */ /*----------------------------------------------------------------------------------------*/ void wait_10us (unsigned char speed) { while (speed){ /*１0uＳウェイト*/ TV.TCRV0.BYTE = 0x01; //下の行と合わせて1/8クロックで1カウントモード TV.TCRV1.BYTE = 0X01; while(TV.TCNTV < 25); //25カウントで１0ｕＳ TV.TCNTV = 0; //カウンタクリア speed--; } } /*----------------------------------------------------------------------------*/ /* */ /* */ /* ステップデータ関連 */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*--------------------------------------------------*/ /* 歩行ステップ１（右移動） */ /*--------------------------------------------------*/ 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); } /*--------------------------------------------------*/ /* 歩行ステップ３（左足前） */ /*--------------------------------------------------*/ 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); } /*--------------------------------------------------*/ /* 歩行ステップ４（左移動） */ /*--------------------------------------------------*/ 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); } /*--------------------------------------------------*/ /* 歩行ステップ６（右足前） */ /*--------------------------------------------------*/ 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_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_BACK (void) { RoboMotion_hokou_6(20); //STEP6右足前 RoboMotion_hokou_Rup(15); //STEP5右足上げ（他のモーションでも使用） RoboMotion_hokou_4(10); //STEP4左移動 RoboMotion_hokou_3(20); //STEP3左足前 RoboMotion_hokou_Lup(15); //STEP2左足上げ（他のモーションでも使用） RoboMotion_hokou_1(10); //STEP1右移動 } /*----------------------------------------------------------------------------*/ /* */ /* */ /* 超音波センサー関連 */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------------------*/ /* 超音波距離センサーの距離測定（ｃｍで返す） */ /* Ｐ１１：トリガー入力 */ /* Ｐ１２：結果出力 */ /*----------------------------------------------------------------------------------------*/ unsigned int US_OUT (void) { IO.PCR1 = 0x02; //P11を出力ポート,P12を入力ポートへ設定 unsigned int length = 0; //距離格納変数 IO.PDR1.BIT.B1 = 1; //P11（トリガ）をHigh wait_10us(2); //20uSウェイト IO.PDR1.BIT.B1 = 0; //P11（トリガ）をLow（超音波センサー測定開始） while (!(IO.PDR1.BIT.B2)); //センサーの出力が立ち上がるまで待つ TA.TMA.BYTE = 0x13; //タイマーＡ、クロックの１／５１２カウントモード while (IO.PDR1.BIT.B2); //センサーの出力が立下りまで待つ（計測終了） length = TA.TCA / 2.4; //タイマーの値を１ｃｍ単位に変換 TA.TMA.BYTE = 0x1F; //タイマーＡリセット return length; } /*----------------------------------------------------------------------------*/ /* */ /* */ /* メインルーチン */ /* */ /* */ /*----------------------------------------------------------------------------*/ /*-----------------------------------------------*/ /* メイン */ /*-----------------------------------------------*/ void main(void) { // 通信ポートを初期化 ボーレートは9600[bps] SCI3_INIT (br9600, txb, sizeof(txb), rxb, sizeof(rxb)); EI; // 初期化後は割り込みを許可状態に RoboMotion_home_P(20); //雷電を直立位置へ wait(50); //一秒待つ unsigned int DIST; //距離を入れる変数 while(1){ //無限ループ DIST = US_OUT(); //距離を測定 if (US_OUT() > 15) RoboMotion_hokou_FW(); //距離が１５ｃｍ以上なら前進 if (US_OUT() < 10) RoboMotion_hokou_BACK(); //距離が１０ｃｍ以下ならバック } }