モーション連続再生プログラム（各種歩行）


/* 
    4足歩行ロボットモーション再生プログラム
    データ間を補間するバージョン
    プロトタイプ(four_mot2.c)
    2003/10/05(Sun)
*/


#include<3664.h>

#define MOTOR_NUM 12 /* モータの数 */
#define MOTION_NUM0 4 /* モーション00のモーション数 */
#define MOTION_NUM1 14
#define MOTION_NUM2 3
#define MOTION_NUM3 4
#define MOTION_NUM4 4
#define MOTION_NUM5 4
#define MOTION_NUM6 4
#define MOTION_NUM7 4
#define MOTION_NUM8 4
#define MOTION_NUM9 4
#define MOTION_NUM10 4
#define MOTION_NUM11 12
#define MOTION_NUM12 3
#define MIN_P 600
#define PARA  20
/* パルスを MIN_P+PARA*データ で計算 */

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

unsigned char ref=0;      /* 繰り返し数カウント用 */
unsigned char exec=0;     /* モーションをどこまで実行したかカウント */
unsigned char speed=0;    /* どこまで補間したかカウント */
int flag_se=0;

int grbm[MOTOR_NUM];

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

int rc0[MOTOR_NUM][MOTION_NUM0]={
{90,65,90,115},{90,150,150,150},{120,120,120,120},
{90,115,90,65},{150,150,90,150},{120,120,120,120},
{90,65,90,115},{30,30,90,30},{60,60,60,60},
{90,115,90,65},{90,30,30,30},{60,60,60,60}};
/* モーションデータ */
int rc12[MOTOR_NUM][MOTION_NUM12]=
{{ 80, 80, 90},
 { 30,150,150},
 { 40,120,120},
 {179, 90, 90},
 {110,150,150},
 {110,120,120},
 {100, 90, 90},
 {150, 30, 30},
 {140, 60, 60},
 {  0, 90, 90},
 { 70, 70, 30},
 { 70, 70, 60}};
/*  0   1   2   3   4   5   6   7   8   9  10  11  12  13  14 */
int inter12[MOTION_NUM12]={ 50, 50, 30};

int rc11[MOTOR_NUM][MOTION_NUM11]=
{{ 90, 90, 90, 90, 90, 90, 80, 80, 80, 80, 80, 80},
 { 30, 30, 90, 90, 90, 90, 30, 30, 30, 30, 30, 30},
 { 60, 60, 60, 60, 60, 60, 60, 60, 40, 40, 40, 40},
 { 90,179,179, 90, 10, 10, 10, 10, 10, 90,179,179},
 { 30,  0,  0,  0,  0, 30, 30, 30, 10, 10, 80,110},
 { 60, 60, 60, 60, 60,120,120, 60, 60, 60, 60,110},
 { 90, 90, 90, 90, 90, 90,100,100,100,100,100,100},
 {150,150, 90, 90, 90, 90,150,150,150,150,150,150},
 {120,120,120,120,120,120,120,120,140,140,140,140},
 { 90,  0,  0, 90,170,170,170,170,170, 90,  0,  0},
 {150,179,179,179,179,150,150,150,170,170,100, 70},
 {120,120,120,120,120, 60, 60,120,120,120,120, 70}};
/*  0   1   2   3   4   5   6   7   8   9  10  11  12  13  14 */

int inter11[MOTION_NUM11]={15,25, 25, 25, 25,  15,  15,  15, 10, 15, 15,20};

int rc10[MOTOR_NUM][MOTION_NUM10]=
{
{90,65,90,115},{150,150,90,150},{120,120,120,120},
{90,115,90,65},{90,150,150,150},{120,120,120,120},
{90,65,90,115},{90,30,30,30},{60,60,60,60},
{90,115,90,65},{30,30,90,30},{60,60,60,60}
};
/*  0   1   2   3 */

int inter10[MOTION_NUM10]={15, 15, 15, 15};

int rc9[MOTOR_NUM][MOTION_NUM9]=
{
{90,65,90,115},{90,170,170,170},{100,100,100,100},
{90,115,90,65},{170,170,90,170},{100,100,100,100},
{90,65,90,115},{10,10,90,10},{80,80,80,80},
{90,115,90,65},{90,10,10,10},{80,80,80,80}
};
/*  0   1   2   3 */

int inter9[MOTION_NUM9]={2,2,2,2};

int rc8[MOTOR_NUM][MOTION_NUM8]=
{
{90,80,90,100},{90,120,120,120},{100,150,150,150},
{90,100,90,80},{120,120,90,120},{150,150,100,150},
{90,80,90,100},{60,60,90,60},{30,30,80,30},
{90,100,90,80},{90,60,60,60},{80,30,30,30}
};
/*  0   1   2   3 */

int inter8[MOTION_NUM8]={15, 15, 15, 15};

int rc7[MOTOR_NUM][MOTION_NUM7]=
{
{ 40, 70, 40, 10},{150,150,100,150},{120,120,120,120},
{130,160,130,100},{100,150,150,150},{120,120,120,120},
{130,100,130,160},{ 80, 30, 30, 30},{ 60, 60, 60, 60},
{ 40, 10, 40, 70},{ 30, 30, 80, 30},{ 60, 60, 60, 60}
};
/*  0   1   2   3 */

int inter7[MOTION_NUM7]={15, 15, 15, 15};

int rc6[MOTOR_NUM][MOTION_NUM6]=
{
{ 40, 70, 40, 10},{100,150,150,150},{120,120,120,120},
{130,160,130,100},{150,150,100,150},{120,120,120,120},
{130,100,130,160},{ 30, 30, 80, 30},{ 60, 60, 60, 60},
{ 40, 10, 40, 70},{ 80, 30, 30, 30},{ 60, 60, 60, 60}
};
/*  0   1   2   3 */

int inter6[MOTION_NUM6]={15, 15, 15, 15};

int rc5[MOTOR_NUM][MOTION_NUM5]=
{
{ 70,110,110, 70},{150,150,100,100},{120,120,120,120},
{110, 70, 70,110},{100,100,150,150},{120,120,120,120},
{110, 70, 70,110},{ 60, 60, 10, 10},{ 60, 60, 60, 60},
{ 70,110,110, 70},{ 10, 10, 60, 60},{ 60, 60, 60, 60}
};
/*  0   1   2   3 */

int inter5[MOTION_NUM5]={15, 15, 15, 15};

int rc4[MOTOR_NUM][MOTION_NUM4]=
{
{ 70,110,110, 70},{100,100,150,150},{120,120,120,120},
{110, 70, 70,110},{150,150,100,100},{120,120,120,120},
{110, 70, 70,110},{ 10, 10, 60, 60},{ 60, 60, 60, 60},
{ 70,110,110, 70},{ 60, 60, 10, 10},{ 60, 60, 60, 60}
};
/*  0   1   2   3 */

int inter4[MOTION_NUM4]={15, 15, 15, 15};

int rc3[MOTOR_NUM][MOTION_NUM3]=
{
{90,65,90,115},{90,30,30,30},{60,60,60,60},
{90,115,90,65},{30,30,90,30},{60,60,60,60},
{90,65,90,115},{150,150,90,150},{120,120,120,120},
{90,115,90,65},{90,150,150,150},{120,120,120,120}
};
/*  0   1   2   3 */

int inter3[MOTION_NUM3]={15, 15, 15, 15};


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

int rc2[MOTOR_NUM][MOTION_NUM2]=
{{ 80, 80, 90},
 {150,150, 30},
 {120,120, 60},
 {179, 90, 90},
 { 70, 30, 30},
 {100, 60, 60},
 {100,100, 90},
 { 30, 30,150},
 { 50, 50,120},
 {  0, 90, 90},
 {110,150,150},
 { 80,120,120}};
/*  0   1   2   3   4   5   6   7   8   9  10  11  12  13  14 */

int inter1[MOTION_NUM1]={15,25, 25, 25, 25,  5,  5,  5, 10, 15, 15, 15, 30, 10};
int inter2[MOTION_NUM2]={ 50, 50, 30};

int speed_d0[MOTION_NUM0]={14,14,14,14};
/* 1動作で何回割り込みをするか */

unsigned char exec_d[8]={4,MOTION_NUM4,MOTION_NUM5,MOTION_NUM6,MOTION_NUM7,MOTION_NUM8,MOTION_NUM10,4};
/* モーションをどこまで実行するか(データ数で) */

unsigned char ref_d[8]={4,4,4,4,4,4,4,8};
/* モーションを何回繰り返すか */

int main(void)
{
  int aa;

  DI;
  
  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=1;
  /* タイマＡ割込み要求を許可 */

  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 rc[MOTOR_NUM][motion_num], int speed_d[]){
  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;
    flag_se=1;
  }
}

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

void motion(int rc[MOTOR_NUM][motion_num], int speed_d[]){
  int i,j;
  
  for(i=0; i<MOTOR_NUM; i++){
    if(exec==exec_d[motn]-1){
      if(ref==ref_d[motn]-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(exec >= exec_d[motn]){
      exec=0;
      ref++;
      if(ref >= ref_d[motn]){
        ref=0;
        flag_se=2;
      }
    }
  }
}

/* タイマＡを用いて、指定時間ごとに次の足位置データを指定 */
void int_timera(void)
{
  IRR1.BIT.IRRTA=0;
  switch(motn){
    case 0:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc0,speed_d0);
      }else if(flag_se==1){
      motion(rc0,speed_d0);
      }else 
      if(flag_se==2){
        end_mot(rc0,speed_d0);
      }
      break;
    case 1:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc4,inter4);
      }else if(flag_se==1){
      motion(rc4,inter4);
      }else 
      if(flag_se==2){
        end_mot(rc4,inter4);
      }
      break;
    case 2:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc5,inter5);
      }else if(flag_se==1){
      motion(rc5,inter5);
      }else 
      if(flag_se==2){
        end_mot(rc5,inter5);
      }
      break;
    case 3:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc6,inter6);
      }else if(flag_se==1){
      motion(rc6,inter6);
      }else 
      if(flag_se==2){
        end_mot(rc6,inter6);
      }
      break;
    case 4:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc7,inter7);
      }else if(flag_se==1){
      motion(rc7,inter7);
      }else 
      if(flag_se==2){
        end_mot(rc7,inter7);
      }
      break;
    case 5:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc8,inter8);
      }else if(flag_se==1){
      motion(rc8,inter8);
      }else 
      if(flag_se==2){
        end_mot(rc8,inter8);
      }
      break;
    case 6:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc10,inter10);
      }else if(flag_se==1){
      motion(rc10,inter10);
      }else 
      if(flag_se==2){
        end_mot(rc10,inter10);
      }
      break;
    case 7:
    motion_num=exec_d[motn];
      if(flag_se==0){
        st_mot(rc0,inter9);
      }else if(flag_se==1){
      motion(rc0,inter9);
      }else 
      if(flag_se==2){
        end_mot(rc0,inter9);
      }
      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];
  }
}
モーション連続再生プログラム（各種歩行）

サンプルプログラム(four_mot2c)

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

トップページへ

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