/**************************************************
 ８ｃｈ手動関節制御プログラム    RIKIYA 2001.06.24 
                                         rbtest1.c 
                                         H8-3048/F 
ITU2によって約5msごとに割り込みを発生させPIOポート
AとBからPWM波形を生成してモーター8ch分の制御を行なう

関節の位置検出はA/D変換グループ0(AN0〜AN3)および
グループ1(AN4〜7)をスキャンする。

リモート操作はSCI1チャンネルでパソコンからデータ
を受信して行なう。SCI1から以下の文字列を受信する

1文字目：動作させるモーターのグループ指定 LL:左脚 RR:右脚
2文字目：動作させるモーターのグループ指定 LR:左右同時
3文字目：動作させる関節　1:太もも 2:ヒザ 3:足首 4:股関節
4文字目：モーターの回転速度 0(停止)〜8(最速)
5文字目：モーター停止位置設定モード S:相対位置 Z:絶対位置
6文字目：モーター回転方向 +:正回転 -:逆回転 (Sの時のみ有効)
7文字目：モーター停止位置00〜31の10の桁(実際は範囲制限あり）
8文字目：モーター停止位置00〜31の1の桁
9文字目：リターン '\r'

例１）
左脚のヒザを５の速さで現在位置から正方向に３ポイント回転させる
LL25S+03  と入力してリターンを押す。

例２）
左右同時に太ももを３の早さで絶対値16の位置まで回転させる
LR13Z+16　と入力してリターンを押す。

Zの場合は正逆パラメータを無視するため-（マイナス）でもＯＫだが、
省いて詰めたりしたらＮＧ。

/**************************************************/

#include <3048f.h>
#include <myfunc.h>
#pragma interrupt(intimia2)

extern	int CNT;		/*PWM用カウンタ*/

/* 左脚太ももパラメータ */
extern  int STATUS1L;		/*モーター位置検出 0-31*/
extern	int SPMODE1L;		/*モーター速度設定 0-8*/
extern	int STPPOS1L;		/*モーター停止位置設定 0-31*/

/* 左脚ヒザパラメータ */
extern  int STATUS2L;		/*モーター位置検出 0-31*/
extern	int SPMODE2L;		/*モーター速度設定 0-8*/
extern	int STPPOS2L;		/*モーター停止位置設定 0-31*/

/* 左脚足首前後パラメータ */
extern  int STATUS3L;		/*モーター位置検出 0-31*/
extern	int SPMODE3L;		/*モーター速度設定 0-8*/
extern	int STPPOS3L;		/*モーター停止位置設定 0-31*/

/* 左脚股関節左右パラメータ */
extern  int STATUS4L;		/*モーター位置検出 0-31*/
extern	int SPMODE4L;		/*モーター速度設定 0-8*/
extern	int STPPOS4L;		/*モーター停止位置設定 0-31*/

/* 右脚太ももパラメータ */
extern  int STATUS1R;		/*モーター位置検出 0-31*/
extern	int SPMODE1R;		/*モーター速度設定 0-8*/
extern	int STPPOS1R;		/*モーター停止位置設定 0-31*/

/* 右脚ヒザパラメータ */
extern  int STATUS2R;		/*モーター位置検出 0-31*/
extern	int SPMODE2R;		/*モーター速度設定 0-8*/
extern	int STPPOS2R;		/*モーター停止位置設定 0-31*/

/* 右脚足首前後パラメータ */
extern  int STATUS3R;		/*モーター位置検出 0-31*/
extern	int SPMODE3R;		/*モーター速度設定 0-8*/
extern	int STPPOS3R;		/*モーター停止位置設定 0-31*/

/* 右脚股関節左右パラメータ */
extern  int STATUS4R;		/*モーター位置検出 0-31*/
extern	int SPMODE4R;		/*モーター速度設定 0-8*/
extern	int STPPOS4R;		/*モーター停止位置設定 0-31*/

/* 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;
char command[10];          /* 受信コマンド */

/*ハードウエア環境の初期化 */
   timer_init();           /* 時間稼ぎタイマー初期化 itu0 */
   lcd_init();             /* 液晶表示器初期化 */
   sci1_init();            /* シリアルポート1初期化 */
   PA.DDR = 0xff;          /* portA出力モード　左足用 */
   PB.DDR = 0xff;          /* portB出力モード　右足用 */
   P5.DDR = 0xff;          /* port5出力モード割込み表示用 */

   ITU2.TCR.BYTE = 0x23;   /* GRAコンペアマッチ clock 1/8 */
   ITU2.GRA = 0x2710;      /* GRAを2710に設定 約5ms*/
   ITU2.TIER.BYTE = 0xF9;  /* ITU1のGRAによるコンペアマッチ割込みを許可*/
   ITU.TSTR.BIT.STR2 = 0;  /* カウント停止状態 */
   ITU.TSTR.BIT.STR2 = 1;  /* ITU2 TCNTカウント開始 */
/*変数の初期化 */
   CNT = 0;
   for(i=0;i<10;i++){command[i] = '\0';}

   SPMODE1L = 0;
   SPMODE2L = 0;
   SPMODE3L = 0;
   SPMODE4L = 0;
   SPMODE1R = 0;
   SPMODE2R = 0;
   SPMODE3R = 0;
   SPMODE4R = 0;

/*パソコンと液晶画面表示*/
   menu_write();

/*ロボット駆動制御 */
   while(1){
       P5.DR.BYTE = 0x01;          /* 通常処理期間のLED表示*/
       comm_in(command);           /* コマンド文字列の受信 */
       pos_scan();                 /* 関節位置読み込み */
       comm_set(command);          /* コマンド文字列でパラメータを設定する */
       mo_ctrl(command);           /*モーター状態監視制御*/

   }/*while*/      
}

/************************************************************
パソコンメニュー表示関数
*************************************************************/
menu_write(){
/*ハイパーターミナル表示*/
   sci1_strtx("MANUAL REMOTE TEST rbtest1.c");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("1,2 LL:left , RR:right , LR:left&right");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("3   1:MOMO , 2:HIZA , 3:ASHIKUBI , 4:GANIMATA");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("4   MOTOR SPEED 0:STOP --- 8:FAST");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("5   POSITION MODE S:SOUTAI , Z:ZETTAI");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("6   NOR/REV  +:NORMAL , -:REVERSE");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("7,8 STOP POSITION 00 --- 31");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("Enter Key : Finish!");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx("M   MOTOR POSITION LIST");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_tx('\r');
   sci1_tx('\n');
/*液晶表示 */
   lcd_locate(0,1);		/*液晶表示位置指定*/
   lcd_print("OK READY GO!"); 	/*液晶表示器の表示*/
}

/***********************************************************
コマンド文字列を受信する
************************************************************/
comm_in(char *command){
int i = 0;
char rx_data;
char rx_comm[10];

     lcd_locate(0,0);		/* 液晶表示位置指定1行目先頭*/

/* リターンが入力されるまでの文字列を受信 */
     do{
           rx_data = sci1_rx();    	/* 1文字受信 */
           if(rx_data != '\r'){         /* リターン以外を受信した時の処理*/
                sci1_tx(rx_data);	/* 1文字送信 */
                lcd_write4(rx_data,1);	/* 1文字液晶表示 */
                rx_comm[i] = rx_data;	/* 1文字を文字列に格納 */
                i++;
           }
           if((rx_data == 'Q')||(rx_data == 'q')){ /*入力し直し処理*/
                 i = 0;
                 sci1_tx('\r');
                 sci1_strtx("          ");      /* パソコン表示クリア*/
                 sci1_tx('\r');
                 lcd_write4(0x01,0);		/* 画面クリア */
                 lcd_locate(0,0);		/* 液晶表示位置指定1行目先頭*/
           }
           if(rx_data == 'M'){          /*各関節位置の一覧表示*/
                 pos_scan();            /*関節位置読み込み */
                 sci1_tx('\r');
                 sci1_tx('\n');
                 sci1_strtx("STATUS1L = "); val_str_tx(STATUS1L);
                 sci1_strtx("STATUS2L = "); val_str_tx(STATUS2L);
                 sci1_strtx("STATUS3L = "); val_str_tx(STATUS3L);
                 sci1_strtx("STATUS4L = "); val_str_tx(STATUS4L);
                 sci1_strtx("STATUS1R = "); val_str_tx(STATUS1R);
                 sci1_strtx("STATUS2R = "); val_str_tx(STATUS2R);
                 sci1_strtx("STATUS3R = "); val_str_tx(STATUS3R);
                 sci1_strtx("STATUS4R = "); val_str_tx(STATUS4R);
                 sci1_tx('\r');
                 sci1_tx('\n');
                 sci1_tx('\r');
                 sci1_tx('\n');
                 i = 0;
           }
     }while(rx_data != '\r');         	/* リターン入力を検出 */
     rx_comm[i] = '\0';

     /*入力したコマンド文字列を引数*commandに渡す*/
     for(i=0;i<10;i++){
           *command = rx_comm[i];
           command++;
     }

/*入力コマンド文字列の再表示*/
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_strtx(command);
   sci1_tx('\r');
   sci1_tx('\n');

/*入力コマンドを液晶に表示*/
   lcd_write4(0x01,0);		/* 画面クリア */
   lcd_locate(0,0);		/* 液晶表示位置指定1行目先頭*/

   return;
}
/*******************************************************
入力したコマンドをパラメータに渡す
*******************************************************/
comm_set(char *command){
int i=0;
char comm[10];

   /* 引数のコマンド文字列を配列変数comm[]にコピーする */
   for(i=0;i<10;i++){
       comm[i] = *command;
       command++;
   }
   /*左足制御パラメータの設定 */
   if(comm[0] == 'L'){     /*左足制御は確定*/
       switch(comm[2]){
           case '1':       /*左太もも*/
               SPMODE1L = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS1L = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS1L = STATUS1L + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                             break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS1L = STATUS1L - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                             break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS1L = "); val_str_tx(STATUS1L);
               sci1_strtx("STPPOS1L = "); val_str_tx(STPPOS1L);
               sci1_strtx("SPMODE1L = "); val_str_tx(SPMODE1L);
               break;
           case '2':       /*左ヒザ*/
               SPMODE2L = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS2L = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS2L = STATUS2L + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                              break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS2L = STATUS2L - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                              break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS2L = "); val_str_tx(STATUS2L);
               sci1_strtx("STPPOS2L = "); val_str_tx(STPPOS2L);
               sci1_strtx("SPMODE2L = "); val_str_tx(SPMODE2L);
               break;
           case '3':       /*左足首*/
               SPMODE3L = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS3L = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS3L = STATUS3L + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                             break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS3L = STATUS3L - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                             break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS3L = "); val_str_tx(STATUS3L);
               sci1_strtx("STPPOS3L = "); val_str_tx(STPPOS3L);
               sci1_strtx("SPMODE3L = "); val_str_tx(SPMODE3L);
               break;
           case '4':       /*左ガニマタ*/
               SPMODE4L = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS4L = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS4L = STATUS4L + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                             break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS4L = STATUS4L - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                             break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS4L = "); val_str_tx(STATUS4L);
               sci1_strtx("STPPOS4L = "); val_str_tx(STPPOS4L);
               sci1_strtx("SPMODE4L = "); val_str_tx(SPMODE4L);
               break;
           }
   }

   /*右足制御パラメータの設定 */
   if(comm[1] == 'R'){     /*右足制御は確定*/
       switch(comm[2]){
           case '1':       /*右太もも*/
               SPMODE1R = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS1R = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS1R = STATUS1R + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS1R = STATUS1R - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS1R = "); val_str_tx(STATUS1R);
               sci1_strtx("STPPOS1R = "); val_str_tx(STPPOS1R);
               sci1_strtx("SPMODE1R = "); val_str_tx(SPMODE1R);
               break;
           case '2':       /*右ヒザ*/
               SPMODE2R = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS2R = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS2R = STATUS2R + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS2R = STATUS2R - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS2R = "); val_str_tx(STATUS2R);
               sci1_strtx("STPPOS2R = "); val_str_tx(STPPOS2R);
               sci1_strtx("SPMODE2R = "); val_str_tx(SPMODE2R);
               break;
           case '3':       /*右足首*/
               SPMODE3R = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS3R = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS3R = STATUS3R + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS3R = STATUS3R - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS3R = "); val_str_tx(STATUS3R);
               sci1_strtx("STPPOS3R = "); val_str_tx(STPPOS3R);
               sci1_strtx("SPMODE3R = "); val_str_tx(SPMODE3R);
               break;
           case '4':       /*右ガニマタ*/
               SPMODE4R = comm[3] & 0x0f;      /*回転速度設定*/
               if(comm[4] == 'Z'){             /*絶対値指定の場合*/
                       STPPOS4R = ((comm[6] & 0x0F)*10)+(comm[7] & 0x0F);/*入力値そのまま*/
               }
               else if(comm[4] == 'S'){
                   switch(comm[5]){
                       case '+':STPPOS4R = STATUS4R + (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置に入力値を足す*/
                       case '-':STPPOS4R = STATUS4R - (((comm[6] & 0x0F)*10)+(comm[7] & 0x0F));
                                break;      /*現在位置から入力値を引く*/
                   }
               }
               sci1_strtx("STATUS4R = "); val_str_tx(STATUS4R);
               sci1_strtx("STPPOS4R = "); val_str_tx(STPPOS4R);
               sci1_strtx("SPMODE4R = "); val_str_tx(SPMODE4R);
               break;
           }
   }

return;
}

/********************************************************
関節動作の監視制御
********************************************************/
mo_ctrl(char *command){
int i=0;
int endflag1,endflag2;
char comm[10];

     endflag1 = 0;     /*左足用終了フラグ*/
     endflag2 = 0;     /*右足用終了フラグ*/

   /* 引数のコマンド文字列を配列変数comm[]にコピーする */
   for(i=0;i<10;i++){
       comm[i] = *command;
       command++;
   }
     
     do{
           pos_scan();             /* 関節位置読み込み */
           
           /* 左足状態監視 */
           if(comm[0] == 'L'){     /*左足制御は確定*/
                 endflag1 = 1;
                 switch(comm[2]){
                   case '1':       /*左太もも*/
                       if(STATUS1L == STPPOS1L){       /*目標位置到着*/
                             SPMODE1L = 0;             /*モーター停止*/
                             endflag1 = 0;             /*処理終了*/
                       }
                       break;
                   case '2':       /*左ヒザ*/
                       if(STATUS2L == STPPOS2L){       /*目標位置到着*/
                             SPMODE2L = 0;             /*モーター停止*/
                             endflag1 = 0;             /*処理終了*/
                       }
                       break;
                   case '3':       /*左足首*/
                       if(STATUS3L == STPPOS3L){       /*目標位置到着*/
                             SPMODE3L = 0;             /*モーター停止*/
                             endflag1 = 0;             /*処理終了*/
                       }
                       break;
                   case '4':       /*左ガニマタ*/
                       if(STATUS4L == STPPOS4L){       /*目標位置到着*/
                             SPMODE4L = 0;             /*モーター停止*/
                             endflag1 = 0;             /*処理終了*/
                       }
                       break;
                 }
           }

           /*右足状態監視*/
           if(comm[1] == 'R'){     /*右足制御は確定*/
                 endflag2 = 1;
                 switch(comm[2]){
                    case '1':       /*右太もも*/
                       if(STATUS1R == STPPOS1R){       /*目標位置到着*/
                             SPMODE1R = 0;               /*モーター停止*/
                             endflag2 = 0;               /*処理終了*/
                       }
                       break;
                    case '2':       /*右ヒザ*/
                       if(STATUS2R == STPPOS2R){       /*目標位置到着*/
                             SPMODE2R = 0;               /*モーター停止*/
                             endflag2 = 0;               /*処理終了*/
                       }
                       break;
                    case '3':       /*右足首*/
                       if(STATUS3R == STPPOS3R){       /*目標位置到着*/
                             SPMODE3R = 0;               /*モーター停止*/
                             endflag2 = 0;               /*処理終了*/
                       }
                       break;
                    case '4':       /*右ガニマタ*/
                       if(STATUS4R == STPPOS4R){       /*目標位置到着*/
                             SPMODE4R = 0;               /*モーター停止*/
                             endflag2 = 0;               /*処理終了*/
                       }
                       break;
                 }
           }
     }while((endflag1 == 1)||(endflag2 == 1));         /*左右の脚が停止したら終了 */

   sci1_strtx("Job Finished!");
   sci1_tx('\r');
   sci1_tx('\n');
   sci1_tx('\r');
   sci1_tx('\n');

     return;
}

/**********************************************************
関節の現在位置読み込み
**********************************************************/
pos_scan(void){
     /*A/D変換設定 SCANMODE ch0-3 */
     AD.CSR.BYTE = 0x33;
     while(AD.CSR.BIT.ADF == 0){}
     /*スキャン停止*/
     AD.CSR.BIT.ADST = 0;
     /*現在位置取り込み*/
     STATUS1L = AD.DRA>>11;
     STATUS2L = AD.DRB>>11;
     STATUS3L = AD.DRC>>11;
     STATUS4L = AD.DRD>>11;

     /*A/D変換設定 SCANMODE ch4-7 */
     AD.CSR.BYTE = 0x37;
     while(AD.CSR.BIT.ADF == 0){}
     /*スキャン停止*/
     AD.CSR.BIT.ADST = 0;
     /*現在位置取り込み*/
     STATUS1R = AD.DRA>>11;
     STATUS2R = AD.DRB>>11;
     STATUS3R = AD.DRC>>11;
     STATUS4R = AD.DRD>>11;
}

/**********************************************************
関節位置STATUSを2桁の文字列pos[]に変換して232Cから送信する
**********************************************************/
val_str_tx(int status){
char pos[3];

   pos[0] = status/10 | 0x30; /*10のケタ*/
   pos[1] = status%10 | 0x30; /*1のケタ*/
   pos[2] = '\0';
   sci1_strtx(pos);
   sci1_tx('\r');
   sci1_tx('\n');
}

/**********************************************************
割り込み処理
5ms毎に発生する割り込み処理で、モーターをPWM制御する。
***********************************************************/

void intimia2(void){

   P5.DR.BYTE = 0x02;      /*割込み期間中を示すLEDを点灯*/
   CNT++;                  /*割込み回数をカウントする*/
   if(CNT == 11){CNT = 1;} /*カウントは1〜10を繰り返す*/

/********************** 移動制限 **************************/

/*左太もも*/
   if((STATUS1L <= 3)&&(STATUS1L > STPPOS1L)){
       SPMODE1L = 0;
   }
   else if((STATUS1L >= 28)&&(STATUS1L < STPPOS1L)){
       SPMODE1L = 0;
   }
/*左ヒザ*/
   if((STATUS2L <= 3)&&(STATUS2L > STPPOS2L)){
       SPMODE2L = 0;
   }
   else if((STATUS2L >= 28)&&(STATUS2L < STPPOS2L)){
       SPMODE2L = 0;
   }
/*左足首*/
   if((STATUS3L <= 3)&&(STATUS3L > STPPOS3L)){
       SPMODE3L = 0;
   }
   else if((STATUS3L >= 28)&&(STATUS3L < STPPOS3L)){
       SPMODE3L = 0;
   }
/*左ガニマタ*/
   if((STATUS4L <= 3)&&(STATUS4L > STPPOS4L)){
       SPMODE4L = 0;
   }
   else if((STATUS4L >= 28)&&(STATUS4L < STPPOS4L)){
       SPMODE4L = 0;
   }
/*右太もも*/
   if((STATUS1R <= 3)&&(STATUS1R > STPPOS1R)){
       SPMODE1R = 0;
   }
   else if((STATUS1R >= 28)&&(STATUS1R < STPPOS1R)){
       SPMODE1R = 0;
   }
/*右ヒザ*/
   if((STATUS2R <= 3)&&(STATUS2R > STPPOS2R)){
       SPMODE2R = 0;
   }
   else if((STATUS2R >= 28)&&(STATUS2R < STPPOS2R)){
       SPMODE2R = 0;
   }
/*右足首*/
   if((STATUS3R <= 3)&&(STATUS3R > STPPOS3R)){
       SPMODE3R = 0;
   }
   else if((STATUS3R >= 28)&&(STATUS3R < STPPOS3R)){
       SPMODE3R = 0;
   }
/*右ガニマタ*/
   if((STATUS4R <= 3)&&(STATUS4R > STPPOS4R)){
       SPMODE4R = 0;
   }
   else if((STATUS4R >= 28)&&(STATUS4R < STPPOS4R)){
       SPMODE4R = 0;
   }

/*********************** 停止処理 ************************/
   if(SPMODE1L == 0){
       P1_L0 = 1;
       P1_L1 = 1;
   }
   if(SPMODE2L == 0){
       P2_L0 = 1;
       P2_L1 = 1;
   }
   if(SPMODE3L == 0){
       P3_L0 = 1;
       P3_L1 = 1;
   }
   if(SPMODE4L == 0){
       P4_L0 = 1;
       P4_L1 = 1;
   }
   if(SPMODE1R == 0){
       P1_R0 = 1;
       P1_R1 = 1;
   }
   if(SPMODE2R == 0){
       P2_R0 = 1;
       P2_R1 = 1;
   }
   if(SPMODE3R == 0){
       P3_R0 = 1;
       P3_R1 = 1;
   }
   if(SPMODE4R == 0){
       P4_R0 = 1;
       P4_R1 = 1;
   }
/* 左太もも PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE1L == 0){break;}
                   if(STATUS1L < STPPOS1L){           /*正転*/
                           P1_L0 = 1;
                           P1_L1 = 0;
                   }
                   else if(STATUS1L > STPPOS1L){      /*逆転*/
                           P1_L0 = 0;
                           P1_L1 = 1;
                   }
                   break;
       case 2: if(SPMODE1L == 1){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
       case 3: if(SPMODE1L == 2){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
       case 4: if(SPMODE1L == 3){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
       case 5: if(SPMODE1L == 4){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
       case 6: if(SPMODE1L == 5){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
       case 7: if(SPMODE1L == 6){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
       case 8: if(SPMODE1L == 7){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
       case 9: if(SPMODE1L == 8){
                   P1_L0 = 1;
                   P1_L1 = 1;
               }
               break;
   }

/* 左ヒザ PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE2L == 0){break;}
                   if(STATUS2L < STPPOS2L){           /*正転*/
                           P2_L0 = 1;
                           P2_L1 = 0;
                   }
                   else if(STATUS2L > STPPOS2L){      /*逆転*/
                           P2_L0 = 0;
                           P2_L1 = 1;
                   }
                   break;
       case 2: if(SPMODE2L == 1){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
       case 3: if(SPMODE2L == 2){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
       case 4: if(SPMODE2L == 3){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
       case 5: if(SPMODE2L == 4){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
       case 6: if(SPMODE2L == 5){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
       case 7: if(SPMODE2L == 6){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
       case 8: if(SPMODE2L == 7){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
       case 9: if(SPMODE2L == 8){
                   P2_L0 = 1;
                   P2_L1 = 1;
               }
               break;
   }

/* 左足首 PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE3L == 0){break;}
                   if(STATUS3L < STPPOS3L){           /*正転*/
                           P3_L0 = 1;
                           P3_L1 = 0;
                   }
                   else if(STATUS3L > STPPOS3L){      /*逆転*/
                           P3_L0 = 0;
                           P3_L1 = 1;
                   }
                   break;
       case 2: if(SPMODE3L == 1){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
       case 3: if(SPMODE3L == 2){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
       case 4: if(SPMODE3L == 3){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
       case 5: if(SPMODE3L == 4){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
       case 6: if(SPMODE3L == 5){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
       case 7: if(SPMODE3L == 6){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
       case 8: if(SPMODE3L == 7){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
       case 9: if(SPMODE3L == 8){
                   P3_L0 = 1;
                   P3_L1 = 1;
               }
               break;
   }

/* 左ガニマタ PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE4L == 0){break;}
                   if(STATUS4L < STPPOS4L){           /*正転*/
                           P4_L0 = 1;
                           P4_L1 = 0;
                   }
                   else if(STATUS4L > STPPOS4L){      /*逆転*/
                           P4_L0 = 0;
                           P4_L1 = 1;
                   }
                   break;
       case 2: if(SPMODE4L == 1){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
       case 3: if(SPMODE4L == 2){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
       case 4: if(SPMODE4L == 3){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
       case 5: if(SPMODE4L == 4){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
       case 6: if(SPMODE4L == 5){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
       case 7: if(SPMODE4L == 6){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
       case 8: if(SPMODE4L == 7){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
       case 9: if(SPMODE4L == 8){
                   P4_L0 = 1;
                   P4_L1 = 1;
               }
               break;
   }

/* 右太もも PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE1R == 0){break;}
                   if(STATUS1R < STPPOS1R){           /*正転*/
                           P1_R0 = 1;
                           P1_R1 = 0;
                   }
                   else if(STATUS1R > STPPOS1R){      /*逆転*/
                           P1_R0 = 0;
                           P1_R1 = 1;
                   }
                   break;
       case 2: if(SPMODE1R == 1){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
       case 3: if(SPMODE1R == 2){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
       case 4: if(SPMODE1R == 3){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
       case 5: if(SPMODE1R == 4){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
       case 6: if(SPMODE1R == 5){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
       case 7: if(SPMODE1R == 6){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
       case 8: if(SPMODE1R == 7){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
       case 9: if(SPMODE1R == 8){
                   P1_R0 = 1;
                   P1_R1 = 1;
               }
               break;
   }

/* 右ヒザ PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE2R == 0){break;}
                   if(STATUS2R < STPPOS2R){           /*正転*/
                           P2_R0 = 1;
                           P2_R1 = 0;
                   }
                   else if(STATUS2R > STPPOS2R){      /*逆転*/
                           P2_R0 = 0;
                           P2_R1 = 1;
                   }
                   break;
       case 2: if(SPMODE2R == 1){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
       case 3: if(SPMODE2R == 2){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
       case 4: if(SPMODE2R == 3){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
       case 5: if(SPMODE2R == 4){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
       case 6: if(SPMODE2R == 5){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
       case 7: if(SPMODE2R == 6){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
       case 8: if(SPMODE2R == 7){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
       case 9: if(SPMODE2R == 8){
                   P2_R0 = 1;
                   P2_R1 = 1;
               }
               break;
   }

/* 右足首 PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE3R == 0){break;}
                   if(STATUS3R < STPPOS3R){           /*正転*/
                           P3_R0 = 1;
                           P3_R1 = 0;
                   }
                   else if(STATUS3R > STPPOS3R){      /*逆転*/
                           P3_R0 = 0;
                           P3_R1 = 1;
                   }
                   break;
       case 2: if(SPMODE3R == 1){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
       case 3: if(SPMODE3R == 2){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
       case 4: if(SPMODE3R == 3){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
       case 5: if(SPMODE3R == 4){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
       case 6: if(SPMODE3R == 5){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
       case 7: if(SPMODE3R == 6){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
       case 8: if(SPMODE3R == 7){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
       case 9: if(SPMODE3R == 8){
                   P3_R0 = 1;
                   P3_R1 = 1;
               }
               break;
   }

/* 右ガニマタ PWM制御 *****************/
   switch(CNT){
       case 1:if(SPMODE4R == 0){break;}
                   if(STATUS4R < STPPOS4R){           /*正転*/
                           P4_R0 = 1;
                           P4_R1 = 0;
                   }
                   else if(STATUS4R > STPPOS4R){      /*逆転*/
                           P4_R0 = 0;
                           P4_R1 = 1;
                   }
                   break;
       case 2: if(SPMODE4R == 1){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
       case 3: if(SPMODE4R == 2){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
       case 4: if(SPMODE4R == 3){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
       case 5: if(SPMODE4R == 4){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
       case 6: if(SPMODE4R == 5){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
       case 7: if(SPMODE4R == 6){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
       case 8: if(SPMODE4R == 7){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
       case 9: if(SPMODE4R == 8){
                   P4_R0 = 1;
                   P4_R1 = 1;
               }
               break;
   }

	ITU2.TSR.BIT.IMFA = 0;	/* 検知フラグを戻して再開 */
}