/************************************************** ８ｃｈシーケンス制御プログラム RIKIYA 2001.07.09 rbtest2.c H8-3048/F ITU2によって約5msごとに割り込みを発生させPIOポート AとBからPWM波形を生成してモーター8ch分の制御を行なう 関節の位置検出はA/D変換グループ0(AN0〜AN3)および グループ1(AN4〜7)をスキャンする。 シーケンス制御のためのデータテーブルは、SCI1からCSV ファイルとして読み込む。 １シーケンスあたり、以下のコマンドで構成される。 C,MM,S,PP,---------- あとは繰り返し．．． C コマンド種別 S : ひとつ前のシーケンスが終了したら次のシーケンスを開始する。 E : ひとつ前のシーケンスでコマンドセットが終了した時点で 　　　次のシーケンスを開始する。 W : 単に時間稼ぎをする。ただし、タイマー割り込みとする。 MM 関節番号の指定 01 〜 12 の２桁 Wコマンドの場合は待機時間0.1S単位 S モーター速度の指定 0〜8 の１桁 PP 目標停止位置の指定 00〜31 の２桁 /**************************************************/ #include <3048f.h> #include #pragma interrupt(intimia1) /* ITU1の割り込み関数宣言 */ #pragma interrupt(intimia2) /* ITU2の割り込み関数宣言 */ extern int CNT; /*PWM用カウンタ*/ extern int WAITCNT; /*Wコマンド用カウンタ*/ /* シーケンスデータテーブル変数 */ extern char G_COMM; /* コマンド種別テーブル 100Byte分 */ extern int G_MOTOR; /* 関節指定テーブル 200Byte分 */ extern int G_SPEED; /* 速度指定テーブル 200Byte分 */ extern int G_POS; /* 停止位置テーブル 200Byte分 */ /* 関節制御パラメータテーブル */ extern int G_STATUS; /* 関節現在位置テーブル 24Byte分 */ extern int G_STPPOS; /* 停止目標位置テーブル 24Byte分 */ extern int G_SPMODE; /* 速度指定テーブル 24Byte分 */ extern char G_NRMODE; /* 正転/逆転テーブル　　 12Byte分 */ /* PIOビット割り振り */ #define P1_L0 PA.DR.BIT.B0 /* 左太もも制御ビット0 */ #define P1_L1 PA.DR.BIT.B1 /* 左太もも制御ビット1 */ #define P2_L0 PA.DR.BIT.B2 /* 左ヒザ制御ビット0 */ #define P2_L1 PA.DR.BIT.B3 /* 左ヒザ制御ビット1 */ #define P3_L0 PA.DR.BIT.B4 /* 左足首前後制御ビット0 */ #define P3_L1 PA.DR.BIT.B5 /* 左足首前後制御ビット1 */ #define P4_L0 PA.DR.BIT.B6 /* 左股関節左右制御ビット0 */ #define P4_L1 PA.DR.BIT.B7 /* 左股関節左右制御ビット1 */ #define P1_R0 PB.DR.BIT.B0 /* 右太もも制御ビット0 */ #define P1_R1 PB.DR.BIT.B1 /* 右太もも制御ビット1 */ #define P2_R0 PB.DR.BIT.B2 /* 右ヒザ制御ビット0 */ #define P2_R1 PB.DR.BIT.B3 /* 右ヒザ制御ビット1 */ #define P3_R0 PB.DR.BIT.B4 /* 右足首前後制御ビット0 */ #define P3_R1 PB.DR.BIT.B5 /* 右足首前後制御ビット1 */ #define P4_R0 PB.DR.BIT.B6 /* 右股関節左右制御ビット0 */ #define P4_R1 PB.DR.BIT.B7 /* 右股関節左右制御ビット1 */ /********************************************************* メイン関数 **********************************************************/ void main(void){ int i; int *status; int *spmode; int *stppos; char *nrmode; char rx_data; /* ハードウエア環境の初期化 */ timer_init(); /* 時間稼ぎタイマー初期化 itu0 */ lcd_init(); /* 液晶表示器初期化 */ sci1_init(); /* シリアルポート1初期化 */ PA.DDR = 0xff; /* portA出力モード　左足用 */ PB.DDR = 0xff; /* portB出力モード　右足用 */ P5.DDR = 0xff; /* port5出力モード割込み表示用 */ ITU1.TCR.BYTE = 0x23; /* GRAコンペアマッチ clock 1/8 */ ITU1.GRA = 0x4E20; /* GRAを4E20に設定 約10ms*/ ITU1.TIER.BYTE = 0xF9; /* ITU1のGRAによるコンペアマッチ割込みを許可*/ ITU.TSTR.BIT.STR1 = 0; /* カウント停止状態 */ ITU2.TCR.BYTE = 0x23; /* GRAコンペアマッチ clock 1/8 */ ITU2.GRA = 0x2710; /* GRAを2710に設定 約5ms*/ ITU2.TIER.BYTE = 0xF9; /* ITU2のGRAによるコンペアマッチ割込みを許可*/ ITU.TSTR.BIT.STR2 = 0; /* カウント停止状態 */ /* 変数の初期化 */ CNT = 0; WAITCNT = 0; status = &G_STATUS; spmode = &G_SPMODE; stppos = &G_STPPOS; nrmode = &G_NRMODE; for(i=0;i<12;i++){ *status = 0; *spmode = 0; *stppos = 0; *nrmode = '-'; status++; spmode++; stppos++; nrmode++; } /* メッセージ表示 */ sci1_strtx("TekuRobo rbtest2.c RUN"); sci1_tx('\r'); sci1_tx('\n'); lcd_write4(0x01,0); /* 画面クリア */ lcd_locate(0,1); lcd_print("LET'S GO!"); P5.DR.BYTE = 0x01; /*通常処理中を示すLEDを点灯*/ /* メイン処理 */ do{ sci1_strtx("PLEASE COMMAND INPUT"); sci1_tx('\r'); sci1_tx('\n'); rx_data = sci1_rx(); /* 1文字受信 */ switch(rx_data){ case 'S': /* CSV DATA受信モード */ csv_conv(); break; case 'P': /* 受信データテーブル表示モード */ table_list(); break; case 'L': /* 関節位置一覧表示 */ pos_list(); break; case 'G': /* シーケンシャル動作 */ rb_move(); break; } }while(1); } /*************************************************** シーケンシャル動作関数 ***************************************************/ rb_move(){ int i = 1; int end_flag = 0; /* 動作終了フラグ 0:終了、1:動作中 */ int *p_motor; /* G_MOTOR用ポインタ変数 */ int *p_speed; /* G_SPEED用ポインタ変数 */ int *p_pos; /* G_POS用ポインタ変数 */ char *p_comm; /* G_COMM用ポインタ変数 */ /* ポインタ変数初期化 */ p_comm = &G_COMM; p_motor = &G_MOTOR; p_speed = &G_SPEED; p_pos = &G_POS; ITU.TSTR.BIT.STR2 = 1; /* ITU2 TCNTカウント開始 */ do{ /* 関節角度読み込み */ pos_scan(); switch(*p_comm){ case 'S': /* ストップスタート動作 */ if(end_flag == 0){ /* コマンドセット */ comm_set(*p_motor,*p_speed,*p_pos); /* 動作状態表示 */ run_disp(i,*p_comm,*p_motor,*p_speed,*p_pos); i++; /* 次のテーブルに移動 */ p_comm++; p_motor++; p_speed++; p_pos++; end_flag = 0; } break; case 'E': /* スタートスタート動作 */ /* コマンドセット */ comm_set(*p_motor,*p_speed,*p_pos); /* 動作状態表示 */ run_disp(i,*p_comm,*p_motor,*p_speed,*p_pos); i++; /* 次のテーブルに移動 */ p_comm++; p_motor++; p_speed++; p_pos++; end_flag = 0; break; case 'W': /* ストップスタート動作 */ if(end_flag == 0){ ITU.TSTR.BIT.STR1 = 1; /* カウント開始 */ if((*p_motor * 10) <= WAITCNT){ ITU.TSTR.BIT.STR1 = 0; /* カウント停止 */ /* 動作状態表示 */ run_disp(i,*p_comm,*p_motor,*p_speed,*p_pos); i++; /* 次のテーブルに移動 */ p_comm++; p_motor++; p_speed++; p_pos++; end_flag = 0; WAITCNT = 0; } } break; case 'Q': /* シーケンス終了 */ break; } /* 関節監視制御関数 */ end_flag = move_ctrl(); }while((*p_comm != 'Q')||(end_flag != 0)); /*処理終了条件 */ sci1_strtx("MOVING FINISHED"); sci1_tx('\r'); sci1_tx('\n'); ITU.TSTR.BIT.STR2 = 0; /* ITU2 TCNTカウント停止 */ return; } /**************************************************** コマンドセット関数 *****************************************************/ comm_set(int motor,int speed,int pos){ int *status; int *stppos; int *spmode; char *nrmode; /* 指定の関節のパラメータを選択 */ status = &G_STATUS + (motor - 1); stppos = &G_STPPOS + (motor - 1); spmode = &G_SPMODE + (motor - 1); nrmode = &G_NRMODE + (motor - 1); /* 速度、停止位置のパラメータをセット */ *spmode = speed; *stppos = pos; if(*stppos >= *status){ *nrmode = '+'; } else if(*stppos < *status){ *nrmode = '-'; } return; } /**************************************************** 動作状況の経過を表示する *****************************************************/ void run_disp(int i,char p_comm,int p_motor,int p_speed,int p_pos){ val_str_tx(i,0); sci1_strtx(" "); sci1_tx(p_comm); sci1_strtx(" "); val_str_tx(p_motor,0); sci1_strtx(" "); val_str_tx(p_speed,0); sci1_strtx(" "); val_str_tx(p_pos,1); return; } /**************************************************** 関節監視制御関数 ひとつでも動いている関節があれば動いている関節数を返す。 全然動いていなければ0を返す。 ****************************************************/ int move_ctrl(int motor){ int i; int end_flag = 0; int *status; int *stppos; int *spmode; char *nrmode; /* 関節パラメータのポインタ設定 */ status = &G_STATUS; stppos = &G_STPPOS; spmode = &G_SPMODE; nrmode = &G_NRMODE; /* 各関節の監視処理 */ for(i=1;i<9;i++){ /* 目標位置に達したら停止 */ if(*status == *stppos){ *spmode = 0; } /* +方向に行き過ぎた場合の補正 */ if((*status < *stppos)&&(*nrmode == '-')){ *nrmode = '+'; *spmode = 1; } /* -方向に行き過ぎた場合の補正 */ else if((*status > *stppos)&&(*nrmode == '+')){ *nrmode = '-'; *spmode = 1; } /* 動作中ならend_flagに1を足す */ if(*spmode != 0){ end_flag++; } /* デバッグ用のパラメータ表示処理 val_str_tx(i,0); sci1_strtx(" "); val_str_tx(*status,0); sci1_strtx(" "); val_str_tx(*stppos,0); sci1_strtx(" "); val_str_tx(*spmode,0); sci1_strtx(" "); val_str_tx(end_flag,0); sci1_strtx(" "); sci1_tx(*nrmode); sci1_tx('\r'); sci1_tx('\n'); /* 次の関節のパラメータに移動 */ status++; stppos++; spmode++; nrmode++; } return(end_flag); /* 動作中の関節数を返す */ } /********************************************************** 関節の現在位置読み込み **********************************************************/ pos_scan(void){ int *status; /* 各関節位置のポインタ設定 */ status = &G_STATUS; /*A/D変換設定 SCANMODE ch0-3 */ AD.CSR.BYTE = 0x33; while(AD.CSR.BIT.ADF == 0){} /*スキャン停止*/ AD.CSR.BIT.ADST = 0; /*現在位置取り込み*/ *status = AD.DRA>>11; status++; *status = AD.DRB>>11; status++; *status = AD.DRC>>11; status++; *status = AD.DRD>>11; /*A/D変換設定 SCANMODE ch4-7 */ AD.CSR.BYTE = 0x37; while(AD.CSR.BIT.ADF == 0){} /*スキャン停止*/ AD.CSR.BIT.ADST = 0; /*現在位置取り込み*/ status++; *status = AD.DRA>>11; status++; *status = AD.DRB>>11; status++; *status = AD.DRC>>11; status++; *status = AD.DRD>>11; return; } /*************************************************** 関節位置一覧表示 ****************************************************/ pos_list(){ int *status; /* データ取り込み関数実行 */ pos_scan(); /* 各関節位置のポインタ設定 */ status = &G_STATUS; /* 各関節位置の表示 */ sci1_strtx("1L "); val_str_tx(*status,1); status++;/* 次の関節に移動 */ sci1_strtx("2L "); val_str_tx(*status,1); status++; sci1_strtx("3L "); val_str_tx(*status,1); status++; sci1_strtx("4L "); val_str_tx(*status,1); status++; sci1_strtx("1R "); val_str_tx(*status,1); status++; sci1_strtx("2R "); val_str_tx(*status,1); status++; sci1_strtx("3R "); val_str_tx(*status,1); status++; sci1_strtx("4R "); val_str_tx(*status,1); status++; sci1_tx('\r'); sci1_tx('\n'); return; } /*************************************************** CSVデータ読み込み、テーブル作成関数 ***************************************************/ csv_conv(){ int i,j; int *p_motor; /* G_MOTOR用ポインタ変数 */ int *p_speed; /* G_SPEED用ポインタ変数 */ int *p_pos; /* G_POS用ポインタ変数 */ char *p_comm; /* G_COMM用ポインタ変数 */ char rx_data; /* データ受信変数 */ char rx_comm[1000]; /* CSVデータ受信バッファ */ /* ポインタ変数初期化 */ p_comm = &G_COMM; p_motor = &G_MOTOR; p_speed = &G_SPEED; p_pos = &G_POS; /* 受信バッファ初期化 */ for(i=0;i<1000;i++){ rx_comm[i] = '\0'; } /* メッセージ表示 */ sci1_strtx("PLEASE SEND THE CSV FILE!"); sci1_tx('\r'); sci1_tx('\n'); lcd_write4(0x01,0); /* 画面クリア */ lcd_locate(0,1); lcd_print("PLEASE SEND CSV!"); /* CSVデータファイル受信 */ i = 0; do{ rx_data = sci1_rx(); /* 1文字受信 */ rx_comm[i] = rx_data; /* 1文字を文字列に格納 */ i++; }while(rx_data != 'Q'); /* データ終了検出 */ rx_comm[i] = '\0'; /* メッセージ表示 */ sci1_strtx("DATA CONVERTING..."); sci1_tx('\r'); sci1_tx('\n'); lcd_write4(0x01,0); /* 画面クリア */ lcd_locate(0,1); lcd_print("CONVERTING..."); /* 各データテーブルへの割り振り */ i = 0; /* 0〜1000までのrx_comm[]操作用 */ j = 0; /* 0〜9までの１シーケンス分操作用 */ do{ switch(j){ case 0: /* コマンド種別格納 */ *p_comm = rx_comm[i]; p_comm++; break; case 2: /* 関節指定を２桁数字に変換して格納 */ *p_motor = ((rx_comm[i] & 0x0f)*10) + (rx_comm[i+1] & 0x0f); p_motor++; break; case 3: break; case 5: /* 回転速度指定を１桁数字に変換して格納 */ *p_speed = rx_comm[i] & 0x0f; p_speed++; break; case 7: /* 停止位置を２桁数字に変換して格納 */ *p_pos = ((rx_comm[i] & 0x0f)*10) + (rx_comm[i+1] & 0x0f); p_pos++; break; case 8: break; case 1: /* カンマ文字の部分は無視する */ case 4: case 6: case 9: break; } i++; j++; if(j >= 10){ j = 0; } /* j 変数の初期化 0〜9の繰り返しにする */ }while(rx_comm[i] != 'Q'); *p_comm = 'Q'; /* メッセージ表示 */ sci1_strtx("DATA CONVERT FINISHED!"); sci1_tx('\r'); sci1_tx('\n'); lcd_write4(0x01,0); /* 画面クリア */ lcd_locate(0,1); lcd_print("CONVERT FINISH"); return; } /************************************************* 受信データテーブル表示関数 *************************************************/ table_list(){ int count = 1; int *p_motor; /* G_MOTOR用ポインタ変数 */ int *p_speed; /* G_SPEED用ポインタ変数 */ int *p_pos; /* G_POS用ポインタ変数 */ char *p_comm; /* G_COMM用ポインタ変数 */ /* ポインタ変数初期化 */ p_comm = &G_COMM; p_motor = &G_MOTOR; p_speed = &G_SPEED; p_pos = &G_POS; sci1_tx('\r'); sci1_tx('\n'); sci1_strtx("DATA TABLE LIST"); sci1_tx('\r'); sci1_tx('\n'); do{ val_str_tx(count,0); /* 連番表示 */ sci1_strtx(" "); /* 4文字スペース */ sci1_tx(*p_comm); /* コマンド種別表示 */ sci1_strtx(" "); /* 4文字スペース */ val_str_tx(*p_motor,0); /* 関節番号表示 */ sci1_strtx(" "); /* 4文字スペース */ val_str_tx(*p_speed,0); /* 回転速度番号表示 */ sci1_strtx(" "); /* 4文字スペース */ val_str_tx(*p_pos,1); /* 回転速度番号表示 */ count++; /* カウンタ更新 */ p_comm++; /* 各データテーブル更新 */ p_motor++; p_speed++; p_pos++; }while(*p_comm != 'Q'); sci1_tx('\r'); sci1_tx('\n'); sci1_strtx("LIST END"); sci1_tx('\r'); sci1_tx('\n'); return; } /**************************************************** ２桁の数字を２桁の文字列に変換してＰＣに送信する。 mode = 0 は、改行コードの送信をしない。 mode = 1 は、改行コードの送信をする。 ****************************************************/ val_str_tx(int status , int mode){ char pos[3]; pos[0] = status/10 | 0x30; /*10のケタ*/ pos[1] = status%10 | 0x30; /*1のケタ*/ pos[2] = '\0'; /*ヌル文字で締めくくる*/ sci1_strtx(pos); /*sci1に送信*/ /* mode = 1なら、改行コードを送信する */ if(mode == 1){ sci1_tx('\r'); sci1_tx('\n'); } return; } /*********************************************************** Wコマンド用割り込み処理 ************************************************************/ void intimia1(void){ WAITCNT++; /* カウンタを+1する */ ITU1.TSR.BIT.IMFA = 0; /* 検知フラグを戻して再開 */ } /********************************************************** 割り込み処理 5ms毎に発生する割り込み処理で、モーターをPWM制御する。 ***********************************************************/ void intimia2(void){ int i; int *status; int *stppos; int *spmode; char *nrmode; P5.DR.BYTE = 0x02; /*割込み期間中を示すLEDを点灯*/ CNT++; /*割込み回数をカウントする*/ if(CNT == 11){CNT = 1;} /*カウントは1〜10を繰り返す*/ /*********************** 停止処理 ************************ 各関節の移動範囲制限と、*spmode = 0の場合のモーター停止処理 */ status = &G_STATUS; stppos = &G_STPPOS; spmode = &G_SPMODE; nrmode = &G_NRMODE; for(i=1;i<9;i++){ switch(i){ case 1:/* 左太もも */ if((*status <= 1)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 30)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P1_L0 = 1; P1_L1 = 1; } break; case 2:/* 左ヒザ */ if((*status <= 5)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 31)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P2_L0 = 1; P2_L1 = 1; } break; case 3:/* 左足首 */ if((*status <= 6)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 27)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P3_L0 = 1; P3_L1 = 1; } break; case 4:/* 左ガニマタ */ if((*status <= 6)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 26)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P4_L0 = 1; P4_L1 = 1; } break; case 5:/* 右太もも */ if((*status <= 1)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 30)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P1_R0 = 1; P1_R1 = 1; } break; case 6:/* 右ヒザ */ if((*status <= 5)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 31)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P2_R0 = 1; P2_R1 = 1; } break; case 7:/* 右足首 */ if((*status <= 6)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 27)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P3_R0 = 1; P3_R1 = 1; } break; case 8:/* 右ガニマタ */ if((*status <= 6)&&(*status > *stppos)){ *spmode = 0; } else if((*status >= 26)&&(*status < *stppos)){ *spmode = 0; } if(*spmode == 0){ P4_R0 = 1; P4_R1 = 1; } break; } /* 次の関節パラメータに移動 */ status++; stppos++; spmode++; nrmode++; } /* 左太もも PWM制御 *****************/ status = &G_STATUS; stppos = &G_STPPOS; spmode = &G_SPMODE; nrmode = &G_NRMODE; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P1_L0 = 1; P1_L1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P1_L0 = 0; P1_L1 = 1; } break; case 2: if(*spmode == 1){ P1_L0 = 0; P1_L1 = 0; } break; case 3: if(*spmode == 2){ P1_L0 = 0; P1_L1 = 0; } break; case 4: if(*spmode == 3){ P1_L0 = 0; P1_L1 = 0; } break; case 5: if(*spmode == 4){ P1_L0 = 0; P1_L1 = 0; } break; case 6: if(*spmode == 5){ P1_L0 = 0; P1_L1 = 0; } break; case 7: if(*spmode == 6){ P1_L0 = 0; P1_L1 = 0; } break; case 8: if(*spmode == 7){ P1_L0 = 0; P1_L1 = 0; } break; case 9: if(*spmode == 8){ P1_L0 = 0; P1_L1 = 0; } break; } /* 左ヒザ PWM制御 *****************/ status++; stppos++; spmode++; nrmode++; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P2_L0 = 1; P2_L1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P2_L0 = 0; P2_L1 = 1; } break; case 2: if(*spmode == 1){ P2_L0 = 0; P2_L1 = 0; } break; case 3: if(*spmode == 2){ P2_L0 = 0; P2_L1 = 0; } break; case 4: if(*spmode == 3){ P2_L0 = 0; P2_L1 = 0; } break; case 5: if(*spmode == 4){ P2_L0 = 0; P2_L1 = 0; } break; case 6: if(*spmode == 5){ P2_L0 = 0; P2_L1 = 0; } break; case 7: if(*spmode == 6){ P2_L0 = 0; P2_L1 = 0; } break; case 8: if(*spmode == 7){ P2_L0 = 0; P2_L1 = 0; } break; case 9: if(*spmode == 8){ P2_L0 = 0; P2_L1 = 0; } break; } /* 左足首 PWM制御 *****************/ status++; stppos++; spmode++; nrmode++; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P3_L0 = 1; P3_L1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P3_L0 = 0; P3_L1 = 1; } break; case 2: if(*spmode == 1){ P3_L0 = 0; P3_L1 = 0; } break; case 3: if(*spmode == 2){ P3_L0 = 0; P3_L1 = 0; } break; case 4: if(*spmode == 3){ P3_L0 = 0; P3_L1 = 0; } break; case 5: if(*spmode == 4){ P3_L0 = 0; P3_L1 = 0; } break; case 6: if(*spmode == 5){ P3_L0 = 0; P3_L1 = 0; } break; case 7: if(*spmode == 6){ P3_L0 = 0; P3_L1 = 0; } break; case 8: if(*spmode == 7){ P3_L0 = 0; P3_L1 = 0; } break; case 9: if(*spmode == 8){ P3_L0 = 0; P3_L1 = 0; } break; } /* 左ガニマタ PWM制御 *****************/ status++; stppos++; spmode++; nrmode++; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P4_L0 = 1; P4_L1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P4_L0 = 0; P4_L1 = 1; } break; case 2: if(*spmode == 1){ P4_L0 = 0; P4_L1 = 0; } break; case 3: if(*spmode == 2){ P4_L0 = 0; P4_L1 = 0; } break; case 4: if(*spmode == 3){ P4_L0 = 0; P4_L1 = 0; } break; case 5: if(*spmode == 4){ P4_L0 = 0; P4_L1 = 0; } break; case 6: if(*spmode == 5){ P4_L0 = 0; P4_L1 = 0; } break; case 7: if(*spmode == 6){ P4_L0 = 0; P4_L1 = 0; } break; case 8: if(*spmode == 7){ P4_L0 = 0; P4_L1 = 0; } break; case 9: if(*spmode == 8){ P4_L0 = 0; P4_L1 = 0; } break; } /* 右太もも PWM制御 *****************/ status++; stppos++; spmode++; nrmode++; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P1_R0 = 1; P1_R1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P1_R0 = 0; P1_R1 = 1; } break; case 2: if(*spmode == 1){ P1_R0 = 0; P1_R1 = 0; } break; case 3: if(*spmode == 2){ P1_R0 = 0; P1_R1 = 0; } break; case 4: if(*spmode == 3){ P1_R0 = 0; P1_R1 = 0; } break; case 5: if(*spmode == 4){ P1_R0 = 0; P1_R1 = 0; } break; case 6: if(*spmode == 5){ P1_R0 = 0; P1_R1 = 0; } break; case 7: if(*spmode == 6){ P1_R0 = 0; P1_R1 = 0; } break; case 8: if(*spmode == 7){ P1_R0 = 0; P1_R1 = 0; } break; case 9: if(*spmode == 8){ P1_R0 = 0; P1_R1 = 0; } break; } /* 右ヒザ PWM制御 *****************/ status++; stppos++; spmode++; nrmode++; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P2_R0 = 1; P2_R1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P2_R0 = 0; P2_R1 = 1; } break; case 2: if(*spmode == 1){ P2_R0 = 0; P2_R1 = 0; } break; case 3: if(*spmode == 2){ P2_R0 = 0; P2_R1 = 0; } break; case 4: if(*spmode == 3){ P2_R0 = 0; P2_R1 = 0; } break; case 5: if(*spmode == 4){ P2_R0 = 0; P2_R1 = 0; } break; case 6: if(*spmode == 5){ P2_R0 = 0; P2_R1 = 0; } break; case 7: if(*spmode == 6){ P2_R0 = 0; P2_R1 = 0; } break; case 8: if(*spmode == 7){ P2_R0 = 0; P2_R1 = 0; } break; case 9: if(*spmode == 8){ P2_R0 = 0; P2_R1 = 0; } break; } /* 右足首 PWM制御 *****************/ status++; stppos++; spmode++; nrmode++; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P3_R0 = 1; P3_R1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P3_R0 = 0; P3_R1 = 1; } break; case 2: if(*spmode == 1){ P3_R0 = 0; P3_R1 = 0; } break; case 3: if(*spmode == 2){ P3_R0 = 0; P3_R1 = 0; } break; case 4: if(*spmode == 3){ P3_R0 = 0; P3_R1 = 0; } break; case 5: if(*spmode == 4){ P3_R0 = 0; P3_R1 = 0; } break; case 6: if(*spmode == 5){ P3_R0 = 0; P3_R1 = 0; } break; case 7: if(*spmode == 6){ P3_R0 = 0; P3_R1 = 0; } break; case 8: if(*spmode == 7){ P3_R0 = 0; P3_R1 = 0; } break; case 9: if(*spmode == 8){ P3_R0 = 0; P3_R1 = 0; } break; } /* 右ガニマタ PWM制御 *****************/ status++; stppos++; spmode++; nrmode++; switch(CNT){ case 1:if(*spmode == 0){break;} if(*nrmode == '+'){ /*正転*/ P4_R0 = 1; P4_R1 = 0; } else if(*nrmode == '-'){ /*逆転*/ P4_R0 = 0; P4_R1 = 1; } break; case 2: if(*spmode == 1){ P4_R0 = 0; P4_R1 = 0; } break; case 3: if(*spmode == 2){ P4_R0 = 0; P4_R1 = 0; } break; case 4: if(*spmode == 3){ P4_R0 = 0; P4_R1 = 0; } break; case 5: if(*spmode == 4){ P4_R0 = 0; P4_R1 = 0; } break; case 6: if(*spmode == 5){ P4_R0 = 0; P4_R1 = 0; } break; case 7: if(*spmode == 6){ P4_R0 = 0; P4_R1 = 0; } break; case 8: if(*spmode == 7){ P4_R0 = 0; P4_R1 = 0; } break; case 9: if(*spmode == 8){ P4_R0 = 0; P4_R1 = 0; } break; } ITU2.TSR.BIT.IMFA = 0; /* 検知フラグを戻して再開 */ }