モーション作成用プログラム


/* 
    4足歩行ロボットモーション作成プログラム
    プロトタイプ(four_cr.c)
    2003/10/06(Mon)
*/

#include<sci.c>
#include<3664.h>

#define MOTOR_NUM 12 /* モータの数 */
#define MOTION_NUM 16 /* モーション数の最大 */
#define MIN_P 600
#define PARA  20
/* パルスを MIN_P+PARA*データ で計算 */

unsigned char n=0;      /* PWM出力時に足を指定するためのフラグ */
unsigned char motn=0;   /* モーション指定用フラグ */
unsigned char m_num=0;  /* モーション数 */

unsigned char ref=1;      /* 繰り返し数カウント用 */
unsigned char exec=0;     /* モーションをどこまで実行したかカウント */
unsigned char speed=0;    /* どこまで補間したかカウント */
unsigned char mode_f=253; /* モード選択フラグ（デフォルト：ポーズ作成） */
int d_flag=0;             /* シリアルのデータを区別するためのフラグ */
int r_data=0, r_loop=0, r_motor=0, stend=0;
int flag_se=0;    /* 1:start位置から実行して、start位置に戻る */
int dammy1,dammy2;

unsigned char st[MOTOR_NUM]={90,150,120,90,150,120,90,30,60,90,30,60};
/* 初期値 */

unsigned int grbm[MOTOR_NUM];

int rc[MOTOR_NUM][MOTION_NUM]
/*={
{90,65,90,115},{120,150,150,150},{120,120,120,120},
{90,115,90,65},{150,150,120,150},{120,120,120,120},
{90,65,90,115},{30,30,60,30},{60,60,60,60},
{90,115,90,65},{60,30,30,30},{60,60,60,60}}*/
;
/* モーションデータ */

unsigned char speed_d[MOTION_NUM]
/*={3,3,3,3}*/
;
/* 1動作で何回割り込みをするか */

unsigned char exec_d
/*=MOTION_NUM*/
;
/* モーションをどこまで実行するか(データ数で) */

unsigned char ref_d=1;
/* モーションを何回繰り返すか */

int main(void)
{
  long i;
  int aa;

  DI;
  SCI3.SMR.BYTE=0x00; /* 調歩同期,8ビット,パリティなし,ストップビット1,クロックφ */
  SCI3.SCR3.BYTE=0x00;
  SCI3.BRR=12;       /* 38400bps */
  
  SCI3.SSR.BIT.OER=0;
  SCI3.SSR.BIT.FER=0;
  SCI3.SSR.BIT.PER=0;

  SCI3.SCR3.BIT.RIE=1; /* シリアル割り込み許可 */
  SCI3.SSR.BIT.RDRF=0;
  SCI3.SCR3.BIT.TE = 1;   // 送信許可
  
  SCI3.SCR3.BIT.RE=1; /* シリアル受信許可 */
  
  TW.TMRW.BYTE=0x48;  /* FTIO端子は使用しない */
  TW.TCRW.BYTE=0xBF;  /* 内部クロックの1/8、割り込みフラグの全て(IMFA-D)の出力を1 */
  TW.TIERW.BYTE=0x77; /* D以外の割り込みを可能に */
  TW.TSRW.BYTE=0x70;  /* 割り込みフラグをクリア */
  TW.TCNT=0;
  TW.GRA=6000;  /* 3mS(18mSの1/6) */
  TW.GRB=2400;  /* 1.2mS(ニュートラル) */
  TW.GRC=2400;
  
  TA.TMA.BYTE=0x1B;
  /* B5-7:このプログラムでは関係無い。
     B4  :常に1
     B3  :0ならインターバルタイマ、1なら時計用タイムベース
     B0-2:B3が1の時はオーバーフロー周期の設定
          000:1s   001:0.5s   010: 0.25s   011:0.03125s
  */
  IRR1.BIT.IRRTA=0;
  /* タイマＡ割込み要求フラグをクリア */
  IENR1.BIT.IENTA=0;
  /* タイマＡ割込み要求を不可 */

  IO.PCR1=0xFF;
  IO.PCR5=0xFF;
  IO.PDR1.BYTE=0x00;
  IO.PDR5.BYTE=0x00;

  TW.TMRW.BIT.CTS=1;
  EI;
  for(aa=0; aa<MOTOR_NUM; aa++){
      grbm[aa]=MIN_P+PARA*st[aa];
  }
  while(1){
    ;
  }
}

void st_mot(){
  int i,j;
  
  for(i=0; i<MOTOR_NUM; i++){
    grbm[i]=MIN_P+PARA*((rc[i][0]-st[i])*speed/speed_d[0]+st[i]);
  }
  speed++;
  if(speed > speed_d[0]){
    speed=1;
    motn++;
  }
}

void end_mot(){
  int i,j;
  
  for(i=0; i<MOTOR_NUM; i++){
    grbm[i]=MIN_P+PARA*((st[i]-rc[i][exec_d-1])*speed/speed_d[exec_d-1]+rc[i][exec_d-1]);
  }
  speed++;
  if(speed > speed_d[0]){
    speed=1;
    motn++;
  }
}

void motion(){
  int i,j;
  
  for(i=0; i<MOTOR_NUM; i++){
    if(exec==exec_d-1){
      if(ref==ref_d-1){
        j=exec;
      }else{
        j=0;
      }
    }else{
      j=exec+1;
    }
    grbm[i]=MIN_P+PARA*((rc[i][j]-rc[i][exec])*speed/speed_d[exec]+rc[i][exec]);
  }
  speed++;
  if(speed > speed_d[exec]){
    speed=1;
    exec++;
    if(mode_f==249){
      IENR1.BIT.IENTA=0;
      /* タイマＡ割込み要求を不可 */
    }
    if(exec >= exec_d){
      exec=0;
      ref++;
      if(ref >= ref_d){
        ref=0;
        motn++;
      }
    }
  }
}

/* タイマＡを用いて、指定時間ごとに次の足位置データを指定 */
void int_timera(void)
{
  IRR1.BIT.IRRTA=0;
  switch(motn){
    case 0:
      if(flag_se==1){
        st_mot();
      }else{
        motn++;
      }
      break;
    case 1:
      motion();
      break;
    case 2:
      if(flag_se>0){
        end_mot();
      }
      break;
  }
}

void int_timerw(void)
{
  if(TW.TSRW.BIT.IMFB==1){
    switch(n){
      case 0:
        IO.PDR5.BIT.B0=0;
        break;
      case 1:
        IO.PDR5.BIT.B1=0;
        break;
      case 2:
        IO.PDR5.BIT.B2=0;
        break;
      case 3:
        IO.PDR5.BIT.B3=0;
        break;
      case 4:
        IO.PDR5.BIT.B4=0;
        break;
      case 5:
        IO.PDR5.BIT.B5=0;
        break;
    }
    TW.TSRW.BIT.IMFB=0;
  }
  if(TW.TSRW.BIT.IMFC==1){
    switch(n){
      case 0:
        IO.PDR1.BIT.B4=0;
        break;
      case 1:
        IO.PDR1.BIT.B5=0;
        break;
      case 2:
        IO.PDR1.BIT.B6=0;
        break;
      case 3:
        IO.PDR1.BIT.B0=0;
        break;
      case 4:
        IO.PDR1.BIT.B1=0;
        break;
      case 5:
        IO.PDR1.BIT.B2=0;
        break;
    }
    TW.TSRW.BIT.IMFC=0;
  }
  if(TW.TSRW.BIT.IMFA==1){
    switch(n){
      case 0:
        IO.PDR5.BIT.B1=1;
        IO.PDR1.BIT.B5=1;
        break;
      case 1:
        IO.PDR5.BIT.B2=1;
        IO.PDR1.BIT.B6=1;
        break;
      case 2:
        IO.PDR5.BIT.B3=1;
        IO.PDR1.BIT.B0=1;
        break;
      case 3:
        IO.PDR5.BIT.B4=1;
        IO.PDR1.BIT.B1=1;
        break;
      case 4:
        IO.PDR5.BIT.B5=1;
        IO.PDR1.BIT.B2=1;
        break;
      case 5:
        IO.PDR5.BIT.B0=1;
        IO.PDR1.BIT.B4=1;
        break;
    }
      TW.TSRW.BIT.IMFA=0;
      n++;
      if(n>5){
        n=0;
      }
      TW.GRB=grbm[n];
      TW.GRC=grbm[n+6];
  }
}

void int_sci3(void){
  int dammy, aa,bb;
  while(SCI3.SSR.BIT.RDRF!=1){
    ;
  }
  dammy=SCI3.RDR;
  if(dammy==254){
    r_loop=0;
    r_data=0;
    r_motor=0;
  }else if(dammy==253){
    DI;
      IENR1.BIT.IENTA=0;
  /* タイマＡ割込み要求を不可 */
      TW.TMRW.BIT.CTS=1;
  /* タイマＷ許可 */
    EI;
    mode_f=253;
  }else if(dammy==252){
    DI;
      IENR1.BIT.IENTA=0;
  /* タイマＡ割込み要求を不可 */
      TW.TMRW.BIT.CTS=0;
  /* タイマＷ不可 */
    EI;
    r_loop=0;
    r_data=0;
    r_motor=0;
    mode_f=252;
  }else if(dammy==251){
    DI;
  IRR1.BIT.IRRTA=0;
  /* タイマＡ割込み要求フラグをクリア */
      IENR1.BIT.IENTA=1;
  /* タイマＡ割込み要求を許可 */
  TW.TSRW.BYTE=0x70;  /* 割り込みフラグをクリア */
      TW.TMRW.BIT.CTS=1;
  /* タイマＷ許可 */
    EI;
    mode_f=251;
    motn=0;
    ref=0;
    exec=0;
    speed=0;
  }else if(dammy==250){
    DI;
  IRR1.BIT.IRRTA=0;
  /* タイマＡ割込み要求フラグをクリア */
      IENR1.BIT.IENTA=0;
  /* タイマＡ割込み要求を不可 */
  TW.TSRW.BYTE=0x70;  /* 割り込みフラグをクリア */
      TW.TMRW.BIT.CTS=1;
  /* タイマＷ許可 */
    EI;
  for(aa=0; aa<MOTOR_NUM; aa++){
      grbm[aa]=MIN_P+PARA*st[aa];
  }
    mode_f=250;
  }else if(dammy==249){
    DI;
  IRR1.BIT.IRRTA=0;
  /* タイマＡ割込み要求フラグをクリア */
      IENR1.BIT.IENTA=1;
  /* タイマＡ割込み要求を許可 */
  TW.TSRW.BYTE=0x70;  /* 割り込みフラグをクリア */
      TW.TMRW.BIT.CTS=1;
  /* タイマＷ許可 */
    EI;
    motn=0;
    ref=0;
    exec=0;
    speed=0;
    mode_f=249;
 }else if(dammy==248){
    DI;
  IRR1.BIT.IRRTA=0;
  /* タイマＡ割込み要求フラグをクリア */
      IENR1.BIT.IENTA=1;
  /* タイマＡ割込み要求を許可 */
    EI;
    mode_f=249;
 }else if(dammy==247){
    DI;
      IENR1.BIT.IENTA=0;
      TW.TMRW.BIT.CTS=0;
    EI;
    r_loop=0;
    mode_f=247;
  }else if(dammy<180){
    if(mode_f==253){
      if(d_flag==0){
        dammy1=dammy;
        d_flag=1;
      }else{
        dammy2=dammy;
        grbm[dammy1]=dammy2*20+600;
        d_flag=0;
      }
    }else if(mode_f==252){
      if(r_data==0){
        exec_d=dammy;
        m_num=exec_d;
        r_data=1;
      }else if(r_data==1){
        ref_d=dammy;
        r_data=2;
    r_loop=0;
    r_motor=0;
      }else if(r_data==2){
        if(r_loop==exec_d){
          r_loop=0;
          r_motor++;
        }
        rc[r_motor][r_loop]=dammy;
        r_loop++;
      }
    }else if(mode_f==247){
      if(r_data==2){
        speed_d[r_loop]=dammy;
        r_loop++;
        if(r_loop==exec_d){
          r_loop=0;
          r_data=1;
        }
      }else if(r_data==1){
        flag_se=dammy;
      }
    }
  }
}


/* 
    Windows版シリアル送信(m_check.c)
    三点倒立
    2003/09/()
*/

#include <windows.h>
#include <stdio.h>
#include <ctype.h>

#define MOTOR_NUM 12
#define MOTION_NUM 14


unsigned char rc[MOTOR_NUM][15]=
{{ 90, 90, 90, 90, 90, 90,100,100,100,100,100,100,100,100,100},
 {150,150, 90, 90, 90, 90,100,100,100,130,130,130,130,150,100},
 {100,100,100,100,100,100,120,120,120,120,120,120,120,120,120},
 {179,179,179, 90,  0,  0,  0,  0,  0,  0,  0, 90,179,179, 90},
 {150,179,179,179,179,179,179,150,150,150,170,170, 80, 70,150},
 {100,100,100,100,100,  0,  0,  0,100,100,100,100,100,100,100},
 { 90, 90, 90, 90, 90, 90, 80, 80, 80, 80, 80, 80, 80, 80, 80},
 { 30, 30, 90, 90, 90, 90, 80, 80, 80, 50, 50, 50, 50, 30, 80},
 { 80, 80, 80, 80, 80, 80, 50, 50, 50, 50, 50, 50, 50, 50, 50},
 {  0,  0,  0, 90,179,179,179,179,179,179,179, 90,  0,  0, 90},
 { 30,  0,  0,  0,  0,  0,  0, 30, 30, 30, 10, 10,100,110, 30},
 { 80, 80, 80, 80, 80,179,179,179, 80, 80, 80, 80, 80, 80, 80}};
/*  0   1   2   3   4   5   6   7   8   9  10  11  12  13  14 */

unsigned char inter[15]={15,25, 25, 25, 25,  5,  5,  5, 10, 15, 15, 15, 150, 150, 20};

typedef enum {        // ボーレート設定リスト
  br2400  = 2400,
  br4800  = 4800,
  br9600  = 9600,
  br19200 = 19200,
  br31250 = 31250,
  br38400 = 38400,
  br57600 = 57600
} BaudRate;


/* シリアルポート初期化関数 */
void InitSer(DCB dcb, HANDLE* hComm, BaudRate baud)
{

    *hComm = CreateFile(
        "COM1",                       /* シリアルポートの指定 */
        GENERIC_WRITE, /* アクセスモード */
        0,                            /* 共有モード */
        NULL,                         /* セキュリティ属性 */
        OPEN_EXISTING,                /* 作成フラグ */
        FILE_ATTRIBUTE_NORMAL,        /* 属性 */
        NULL                          /* テンプレートのハンドル */
    ); 
  
    GetCommState(*hComm, &dcb); /* DCB を取得 */
    dcb.BaudRate = baud;
    dcb.ByteSize = 8;
    dcb.Parity = NOPARITY;
    dcb.StopBits = ONESTOPBIT;
    SetCommState(*hComm, &dcb); /* DCB を設定 */

}
int chop(char* input)
{
    int i, c_stat, r_stat=1;
    
    for(i=0; input[i]!='\n'; i++){
        c_stat=isdigit(input[i]);
        if(c_stat==0){
            r_stat=-1;
        }
    }
    input[i]='\0';
    return(r_stat);
}

int checkn(int maxn)
{
    char input[1024];   /* 入力用 */
    int status, status2, value=-1;  /* エラー処理，数値格納 */

    while(1){
        printf("\n送信数値を入力(0-%d)%dで終了：", maxn,maxn+1);
        fgets(input, 1024, stdin);

        status=sscanf(input, "%d", &value);
        status2=chop(input);
        if(status!=1 || status2<0){
            printf("数値以外が入力されました\n");
            printf("入力文字%s\n", input);
        }else if(value>=0 && value <=maxn+1){
            if(value==maxn+1){
                return(-1);
            }
            return(value);
        }else if(status==1){
            printf("0-%dの範囲外の数値が入力されました\n",maxn);
            printf("入力数値%d\n", value);
        }
        value=-1;
    }
}   
int main(void)
{
    DCB dcb;            /* 通信パラメータ */
    HANDLE hComm;       /* シリアルポートのハンドル */
    DWORD writesize;    /* ポートへ書き込んだバイト数 */
    BaudRate baud=br38400;          /* BaudRate設定用 */
    unsigned char sendd1,sendd2; /* 数値格納 */
    int echeck1,echeck2;

    InitSer(dcb, &hComm, baud);
    
    sendd1=252;
    WriteFile(hComm, &sendd1, 1, &writesize, NULL); /* シリアルポートに書き込み */
    sendd1=MOTION_NUM;
    WriteFile(hComm, &sendd1, 1, &writesize, NULL); /* シリアルポートに書き込み */
    sendd1=1;
    WriteFile(hComm, &sendd1, 1, &writesize, NULL); /* シリアルポートに書き込み */
    for(echeck1=0; echeck1<MOTOR_NUM; echeck1++){
        for(echeck2=0; echeck2<MOTION_NUM; echeck2++){
            WriteFile(hComm, &rc[echeck1][echeck2], 1, &writesize, NULL); /* シリアルポートに書き込み */
        }
    }
    sendd1=247;
    WriteFile(hComm, &sendd1, 1, &writesize, NULL);
    for(echeck1=0; echeck1<MOTION_NUM; echeck1++){
        WriteFile(hComm, &inter[echeck1], 1, &writesize, NULL);
    }

    sendd1=254;
    WriteFile(hComm, &sendd1, 1, &writesize, NULL); /* シリアルポートに書き込み */
    while(1){
        echeck1=checkn(253);
        sendd1=echeck1;
        if(echeck1<0){
            break;
        }
        WriteFile(hComm, &sendd1, 1, &writesize, NULL); /* シリアルポートに書き込み */
    }
    CloseHandle(hComm);


    return(0);
}

モーション作成用プログラム

サンプルプログラム(four_cr.c)

マイコン側プログラム例(m_check.c)

4足歩行ロボットの目次へ

トップページへ

SEO		[PR] 爆速!無料ブログ無料ホームページ開設無料ライブ放送