<<前へ
次へ>>
モーション再生プログラム(プロトタイプ)
サンプルプログラム(four_mot.c)
/*
4足歩行ロボットモーション再生プログラム
データ間を補間するバージョン
プロトタイプ(four_mot.c)
2003/10/05(Sun)
*/
#include<3664.h>
#define MOTOR_NUM 12 /* モータの数 */
#define MOTION_NUM00 4 /* モーション00のモーション数 */
#define MIN_P 600
#define PARA 20
/* パルスを MIN_P+PARA*データ で計算 */
unsigned char n=0; /* PWM出力時に足を指定するためのフラグ */
unsigned char motn=0; /* モーション指定用フラグ */
unsigned char ref=0; /* 繰り返し数カウント用 */
unsigned char exec=0; /* モーションをどこまで実行したかカウント */
unsigned char speed=0; /* どこまで補間したかカウント */
unsigned int grbm[MOTOR_NUM];
unsigned char st[MOTOR_NUM]={90,150,120,90,150,120,90,30,60,90,30,60};
/* 初期値 */
int rc00[MOTOR_NUM][MOTION_NUM00]={
{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}};
/* モーションデータ */
unsigned char speed_d00[MOTION_NUM00]={14,14,14,14};
/* 1動作で何回割り込みをするか */
unsigned char exec_d[1]={4};
/* モーションをどこまで実行するか(データ数で) */
unsigned char ref_d[1]={20};
/* モーションを何回繰り返すか */
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 motion(int rc[][MOTION_NUM00], unsigned char 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;
motn++;
}
}
}
}
/* タイマＡを用いて、指定時間ごとに次の足位置データを指定 */
void int_timera(void)
{
IRR1.BIT.IRRTA=0;
switch(motn){
case 0:
motion(rc00, speed_d00);
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];
}
}
