|
二足歩行ロボット歩く ２０１４年９月〜２０１４年１２月） |
|
|
遂に二足歩行ロボットが歩き出しました。
何とか歩行が形になりだしたのは、歩行モーションパターンをプログラム
内部に内蔵しようと思いついて、それが実現できてからです。
ただ、実験中に転倒して左腕が壊れました。
これがその動画です。
|
次のプログラムはロボット側のマイコンのためのものです。
12個のサーボモータを順番に制御しています。
|
/*ｖｂ側はnisokurtest/**/
#include "3048f.h"
#include "nisokusrv.h"
unsigned char sci1_rxWait(void);
void init_io(void)
{
/*ITU０、１のクロックを８分週 3125：１ｍｓ、4687.5：１．５ｍｓ、6250：２ｍｓ、62500：２０ｍｓ*/
ITU0.TCR.BYTE = 0x03; /* ITU0、TCNTクリア禁止 clock 1/8 */
ITU1.TCR.BYTE = 0x03;
/* ITU1、TCNTクリア禁止 clock 1/8 */
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
ITU.TSTR.BIT.STR1 = 1; /* ITU1 TCNTｶｳﾝﾄ開始 */
/* シリアル通信初期化*******************************/
SCI1.SCR.BYTE = 0x00; /* SCI1設定 stop,内部クロック */
SCI1.SMR.BYTE = 0x00; /* data8.stop1,pari non */
SCI1.BRR = 40; /* 19200bps */
ITU0.TCNT = 0; /* ITU0 TCNTｶｳﾝﾄを０にクリア */
while(ITU0.TCNT <= 3125);/*１ｍｓ待つ*/
SCI1.SCR.BYTE = 0x30; /* Tx,Rx有効 ,割込み無効 */
SCI1.SSR.BYTE &= 0x80; /* エラーフラグのクリア */
return;
}
/* SCI1から１文字受信する****************************/
char sci1_rx(void){
char data;
if((SCI1.SSR.BYTE & 0x78)==0) return 0; /* 受信とエラー以外は０を返す*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return(0xff); /* エラー時はFFを返す */
}
}
/*unsigned int型変数（２バイト）をシリアルポートから入力する*/
/*備考：データが来るまで待つ */
unsigned int sci_read_uintA(void)
{
unsigned int data;
unsigned char data_L, data_H;
/*unsigned charのデータ２個を受け取る*/
data_H = sci1_rxWait();
data_L = sci1_rxWait();
/*unsigned intのデータに変換する*/
data = (unsigned int)data_H * 256 + (unsigned int)data_L;
return(data);
}
/* SCI1から１文字受信する データが来るまで待つ*/
unsigned char sci1_rxWait(void)
{
unsigned char data;
while((SCI1.SSR.BYTE & 0x78)==0) ; /* 受信とエラー以外は待つ*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return((unsigned char)0xff); /* エラー時はFFを返す */
}
}
/* SCI1に１文字送信する */
void sci1_tx(char data){
while(SCI1.SSR.BIT.TDRE == 0); /*未送信データが送られるまで待つ*/
SCI1.TDR = data; /*送信データのセット*/
SCI1.SSR.BIT.TDRE = 0; /*送信フラグのクリア*/
return;
}
/* SCI1に文字列を送信する 文字列は'\0'で締めくくっておく**************/
void sci1_strtx(char *str){
while(*str != '\0'){ /* 文字が\0になるまで繰り返す */
sci1_tx(*str); /* 1文字送信*/
str++; /* 次の文字に移る*/
} return;
}
/*サーボ番号をピン番号に割り当てる*/
void srvWari(int srvNo, int x)
{
switch(srvNo){
case 0: P3.DR.BIT.B0 = x;break;
case 1: P3.DR.BIT.B1 = x;break;
case 2: P3.DR.BIT.B2 = x;break;
case 3: P3.DR.BIT.B3 = x;break;
case 4: P3.DR.BIT.B4 = x;break;
case 5: P3.DR.BIT.B5 = x;break;
case 6: P3.DR.BIT.B6 = x;break;
case 7: P3.DR.BIT.B7 = x;break;
case 8: P1.DR.BIT.B0 = x;break;
case 9: P1.DR.BIT.B1 = x;break;
case 10:P1.DR.BIT.B2 = x;break;
case 11:P1.DR.BIT.B3 = x;break;
}
}
void order3(unsigned int *order,unsigned int *data);
void order3(unsigned int *order,unsigned int *data)
{
int i,max,min;
max = 0; min = 65535;
for(i=0;i<=2;i++) {
if( data[i]>=max) {
max = data[i];
order[0] = i;
}
if( data[i]<=min) {
min = data[i];
order[2] = i;
}
}
for(i=0;i<=2;i++){
if(i!= order[0] && i!=order[2]) order[1] = i;
}
}
/*プログラムの始まり*/
main()
{
char signal;
int i, j,k,count,Width, pWidth[12],checkState[12],servoNo,switchSignal;
int id, idTog, *p, countNum;
unsigned int junNo[4];
unsigned int data[4], sData1,sData2,max_num, min_num;
unsigned int out_data[4];
P2.DDR = 0x00; /*port2入力に設定 ディップスイッチ用*/
P2.PCR.BYTE = 0xff; /*port2プルアップon */
P3.DDR = 0xff; /*PORT3を出力に設定*/
P1.DDR = 0xff; /*PORT1を出力に設定*/
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
/*Ｈ８ハードの初期化を行う*/
init_io();
Width = 4687;/*パルス幅 初期値*/
/*無限ループ開始*/
switchSignal ='1'; idTog = 0;
while(1){
/*switch3,switch5は故障で使用不可*/
if (P2.DR.BIT.B1 == 0 || P2.DR.BIT.B2 == 0 || P2.DR.BIT.B3 == 0 || P2.DR.BIT.B4 == 0){
if (P2.DR.BIT.B4 == 0) { /*switch2 前進*/
id=2; p = ForWalk[0]; countNum = ForWalkNum;
}
else if (P2.DR.BIT.B3 == 0) { /*switch4 後退*/
id=5; p = BackWalk[0]; countNum = BackRunNum;
}
else if (P2.DR.BIT.B2 == 0) { /*switch6 左回転*/
idTog = idTog % 2 +1; id=idTog + 7;
if(id==8) {p = LeftRoundF[0]; countNum = LeftRoundFNum;}
if(id==9) {p = LeftRoundL[0]; countNum = LeftRoundLNum;}
}
else if (P2.DR.BIT.B1 == 0) { /*switch7 右回転*/
idTog = idTog % 2 +1; id=idTog + 9;
if(id==10) {p = RightRoundF[0]; countNum = RightRoundFNum;}
if(id==11) {p = RightRoundL[0]; countNum = RightRoundLNum;}
}
for(count = 1;count<=countNum ;count++){
/*パルスを生成*/
/*ＩＴ1のカウント値をゼロにする*/
ITU1.TCNT = 0;
for(i=0;i<=3;i++) {
/*ＩＴ0のカウント値をゼロにする*/
ITU0.TCNT = 0;
for(j=0;j<=2;j++) {
k = 3*i+j;
srvWari(k,1);
out_data[j] = *(p+12*count-12+k);
data[j] = 7365 - out_data[j];
}
order3(junNo,out_data);/*out_dataの大小を決める*/
for(j=0;j<=2;j++){
while (ITU0.TCNT < data[junNo[j]]);
srvWari(3*i+junNo[j] ,0);
}
}/*i end*/
while(ITU1.TCNT < 60000);/*ITU1が20ms経過するまで待つ*/
ITU1.TCNT = 0;
while(ITU1.TCNT < 20000);/*待ち時間*/
}/*for count end*/
} /*switch 2,4,6,7 end*/
else if (P2.DR.BIT.B0 == 0) { /*switch１ */
signal = sci1_rx();/*コマンドが受信されているか確認する*/
if(signal !=0) sci1_tx(signal);
if(signal == 'A'){/*ＲＣサーボ制御信号出力ＯＮ／ＯＦＦ命令受信*/
/*スイッチデータ受信*/
switchSignal = sci1_rx();
}
if (switchSignal == '1'){
if( signal =='B') {
for(i=0;i<12;i++) pWidth[i] = sci_read_uintA();/*ﾊﾟﾙｽ幅12個分*/
for(i=0;i<12;i++) checkState[i] =sci_read_uintA();/*ｻｰﾎﾞﾁｪｯｸ状態*/
for(i=0;i<12;i++)
if(checkState[i]==1) {
servoNo = i;
Width = pWidth[i];
}
}
for(i=1;i<=20;i++){
ITU0.TCNT = 0; /* ITU0 TCNTｶｳﾝﾄを０にクリア */
srvWari(servoNo, 1);/*該当のピン出力を１にする*/
while(ITU0.TCNT< 7375 - Width);/*設定パルス幅*/
srvWari(servoNo, 0);/*該当のピン出力を０にする*/
while(ITU0.TCNT < 40000);/*１３ｍｓまで待つ*/
}
}
}
}
}
|
|
下半身ロボット歩く ２０１４年２月〜２０１４年8月） |
|
|
二足歩行の基礎実験をするために足だけ（下半身）の部分を作ったのですが、モーションパターンを作成して
そのパターン通りに足が動くようになってきたとき、ひょっとしてこれで歩けるのでは？ と思いました。
基礎実験としていろいろやっているときは、足を上向けて試していたのですが、上向けた足が空中で
バタバタしているのを見るうちに、普通どおりに足を下向けて床に置いて見ました。
すぐ倒れはするのですが、調整すれば二足歩行するような気がしました。
何度か調整すると歩きました。
これがその動画です。
|
吉野耕司氏著の書物である「二足歩行ロボット自作入門」では、使用しているマイコンはＨ８（３０６７）です。
私が使用しているマイコンはＨ８／３０５２です。この二つのマイコンには微妙な違いがあります。
Ｈ８／３０６７はＦＬＡＳＨＲＯＭ１２８ＫＢ、ＲＡＭ４ＫＢ、ワークエリアとして３２ＫＢのＳＲＡＭを搭載
して２０ＭＨ動作。
Ｈ８／３０５２はＦＬＡＳＨＲＯＭ５１２Ｋバイト，ＲＡＭ８ＫＢ内蔵の２５ＭＨｚ動作です。
Ｈ８／３０５２はＨ８／３０６７と比較すると、ＳＲＡＭがない分ＦＬＡＳＨＲＯＭとＲＡＭの容量が４倍、２倍で、
さらに動作周波数が１．２５倍です。
Ｈ８／３０５２は、ＳＲＡＭがないことに目をつぶれば高い能力を有しているので私はＨ８／３０５２を
使っています。
この書物では、ＰＣでモーションパターンをＣＳＶファイルに作成して、それをケーブル経由でロボットの
マイコン内部のＳＲＡＭに転送してロボットを歩かせています。
私の使っているマイコンであるＨ８（３０５２）では、ＳＲＡＭがありませんが、その分ＲＯＭの容量が
４倍なのでＲＯＭにモーションデータを置こうと思います。プログラム本体はＲＯＭに置かれるのでプログラム
中にモーションデータを置こうと思いました。
プログラム中に歩行モーションデータを持つことにより、ＰＣからプログラムをロボットのマイコンに転送した
あとはＰＣとロボットとのケーブルをはずしてロボットのみで動くことができるようになりました。
これは僕にとってすごいことでした。
吉野耕司氏のサイトには、書物のための支援ページがありそこにＣＳＶファイルを作製するＣ言語プログラムが
載っています。これをダウンロードして安直に使うことができます。
このプログラムで生成されるのは次のようなＣＳＶファイルです。
id, 0 , 腰を落とす
data_num, 30
2920, 2579, 1679, 3120, 2175, 3100, 1633, 1404, 2696, 2911, 2829, 1425,
2919, 2580, 1680, 3119, 2175, 3100, 1633, 1404, 2696, 2911, 2829, 1425,
id, 0 , 腰を落とす
とありますが、「『腰を落とす』という動作で、『そのＩＤ番号が０』という意味です。
この動作のほかに、全部で１１個ほどの動作がありその動作ごとに３０行ほどがついています。
data_num, 30
とありますが、これは「このようなデータが３０行続いています。」という意味です。
すると、すべての動作を合わせると何百行にもなります。
Ｃ言語プログラムで使えるデータ形式は次のような形です。
int Wdown[][12]={{2920, 2579, 1679, 3120, 2175, 3100, 1633, 1404, 2696, 2911, 2829, 1425},
{2919, 2580, 1680, 3119, 2175, 3100, 1633, 1404, 2696, 2911, 2829, 1425},
{2916, 2583, 1683, 3116, 2175, 3100, 1633, 1404, 2696, 2911, 2829, 1425},
int WdownNum = 30;
手作業でこの形に直すのは簡単ですが、百行を超えると嫌になってきます。
そこでこのための変換プログラムを作ることにしました。
以下にこの変換プログラム（Ｃ言語ＤＯＳ）を載せます。
マイコンのためのプログラムのメインのソースファイルにデータ部分を入れると非常に見にくく
なります。
そこでこのデータだけを別のファイル（motionData.c）に持ちます。
この別ファイルのデータを使うために外部変数を使うという宣言文であるextern int が要ります。
このextern 文をヘッダーファイル中（nisokusrv.h）に入れます。
まず変換プログラムです。
|
#include "stdio.h"
#include "string.h"
#include "stdlib.h"
#include "conio.h"
#include "osamu.h"
/* コンマ区切りの文字列を部分部分に分解する。
byte *string 文字列motionData.c
*ptr[] 部分文字列のポインタ
戻り値＝部分文字列の数
*/
int csv_part(char *string,char *ptr[])
{
char *pointer;
int i;
i=0;
ptr[i] = pointer = string;
while(1){
if ((pointer=strstr(ptr[i],","))==NULL) break;
i++;
*pointer = '\0';
ptr[i] = pointer+1;
}
return(i+1);
} /*the end of csv_part()*/
int main(void){
char numStr[20],*ptr[30];
int data_num,i,num[20],k,num1;
errno_t err;
FILE *fpo2,*fp;
char line[256],str[300];
printf("\n");
printf("*** 番号を選んで下さい ***\n");
printf(" 1:ＣＳＶから配列のファイルを作る\n");
printf(" 2:終了（1以外も・・・）\n");
scanf_s ("%d",&num1, 3);
if(num1 != 1) exit(0);
fopen_s(&fpo2, "motionData.c" , "wt") ;
//ファイルを開く
if ((err = fopen_s(&fp,"pen4_motiondata.csv" , "rt")) != 0)
exit(0);
while(1){
if(fgets(line, 256, fp) == NULL) break;//１行目を読み込む
if (strstr(line,"id")!=NULL) {
data_num=csv_part(line,ptr);
strcpy_s(str,ptr[1]);
i =atoi(str);
fgets(line, 256, fp); //２行目を読み込む
data_num=csv_part(line,ptr);
num[i] = atoi(ptr[1]);//何行あるかを読み込む
switch(i){
case 0: {
strcpy_s(str,"int Wdown[][12]={{");
break;
}
case 1: strcpy_s(str,"};\nint Stand[][12]={{");break;
case 2: strcpy_s(str,"};\nint ForWalk[][12]={{");break;
case 3: strcpy_s(str,"};\nint FirstOne[][12]={{");break;
case 4: strcpy_s(str,"};\nint LastOne[][12]={{");break;
case 5: strcpy_s(str,"};\nint BackWalk[][12]={{");break;
case 6: strcpy_s(str,"};\nint BackFirst[][12]={{");break;
case 7: strcpy_s(str,"};\nint BackLast[][12]={{");break;
case 8: strcpy_s(str,"};\nint LeftRoundF[][12]={{");break;
case 9: strcpy_s(str,"};\nint LeftRondL[][12]={{");break;
case 10: strcpy_s(str,"};\nint RightRoundF[][12]={{");break;
case 11: strcpy_s(str,"};\nint RightRoundL[][12]={{");break;
}
fgets(line, 256, fp);
}
else strcpy_s(str,",\n{");
*(line+strlen(line)-3)=0;
strcat_s(str,line);
strcat_s(str,"}");
fprintf(fpo2, str);
}
strcpy_s(str,"};\n");//最終行の設定
fprintf(fpo2, str);
k = i;
for(i=0;i<=k;i++) {
switch(i){
case 0: {
strcpy_s(str,"int WdownNum = ");
break;
}
case 1: strcpy_s(str,"int StandNum = ");break;
case 2: strcpy_s(str,"int ForWalkNum = ");break;
case 3: strcpy_s(str,"int FirstOneNum = ");break;
case 4: strcpy_s(str,"int LastOneNum = ");break;
case 5: strcpy_s(str,"int BackRunNum = ");break;
case 6: strcpy_s(str,"int BackFirstNum = ");break;
case 7: strcpy_s(str,"int BackLastNum = ");break;
case 8: strcpy_s(str,"int LeftRoundFNum = ");break;
case 9: strcpy_s(str,"int LeftRondLNum = ");break;
case 10: strcpy_s(str,"int RightRoundFNum = ");break;
case 11: strcpy_s(str,"int RightRoundLNum = ");break;
}
_itoa_s(num[i],numStr,10);
strcat_s(str,numStr);
strcat_s(str,";\n");
fprintf(fpo2, str);
}
fclose(fp);
fclose(fpo2);
printf("書き込み終了です");
_getch();
return 1;
}
|
|
次にヘッダーファイル（nisokusrv.h）です。
|
extern int Wdown[][12];
extern int Stand[][12];
extern int ForWalk[][12];
extern int FirstOne[][12];
extern int LastOne[][12];
extern int BackWalk[][12];
extern int BackFirst[][12];
extern int BackLast[][12];
extern int LeftRoundF[][12];
extern int LeftRondL[][12];
extern int RightRoundF[][12];
extern int RightRoundL[][12];
extern int WdownNum;
extern int StandNum;
extern int ForWalkNum;
extern int FirstOneNum;
extern int LastOneNum;
extern int BackRunNum;
extern int BackFirstNum;
extern int BackLastNum;
extern int LeftRoundFNum;
extern int LeftRondLNum;
extern int RightRoundFNum;
extern int RightRoundLNum;
|
|
マイコンのＣ言語プログラムです。
|
#include "3048f.h"
#include "nisokusrv.h"
unsigned char sci1_rxWait(void);
void init_io(void)
{
/*ITU０、１のクロックを８分週 */
/*3125：１ｍｓ、4687.5：１．５ｍｓ、6250：２ｍｓ、62500：２０ｍｓ*/
ITU0.TCR.BYTE = 0x03; /* ITU0、TCNTクリア禁止 clock 1/8 */
ITU1.TCR.BYTE = 0x03;
/* ITU1、TCNTクリア禁止 clock 1/8 */
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
ITU.TSTR.BIT.STR1 = 1; /* ITU1 TCNTｶｳﾝﾄ開始 */
/* シリアル通信初期化*******************************/
SCI1.SCR.BYTE = 0x00; /* SCI1設定 stop,内部クロック */
SCI1.SMR.BYTE = 0x00; /* data8.stop1,pari non */
SCI1.BRR = 40; /* 19200bps */
ITU0.TCNT = 0; /* ITU0 TCNTｶｳﾝﾄを０にクリア */
while(ITU0.TCNT <= 3125);/*１ｍｓ待つ*/
SCI1.SCR.BYTE = 0x30; /* Tx,Rx有効 ,割込み無効 */
SCI1.SSR.BYTE &= 0x80; /* エラーフラグのクリア */
return;
}
/* SCI1から１文字受信する****************************/
char sci1_rx(void){
char data;
if((SCI1.SSR.BYTE & 0x78)==0) return 0; /* 受信とエラー以外は０を返す*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return(0xff); /* エラー時はFFを返す */
}
}
/*unsigned int型変数（２バイト）をシリアルポートから入力する*/
/*備考：データが来るまで待つ */
unsigned int sci_read_uintA(void)
{
unsigned int data;
unsigned char data_L, data_H;
/*unsigned charのデータ２個を受け取る*/
data_H = sci1_rxWait();
data_L = sci1_rxWait();
/*unsigned intのデータに変換する*/
data = (unsigned int)data_H * 256 + (unsigned int)data_L;
return(data);
}
/* SCI1から１文字受信する データが来るまで待つ*/
unsigned char sci1_rxWait(void)
{
unsigned char data;
while((SCI1.SSR.BYTE & 0x78)==0) ; /* 受信とエラー以外は待つ*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return((unsigned char)0xff); /* エラー時はFFを返す */
}
}
/* SCI1に１文字送信する */
void sci1_tx(char data){
while(SCI1.SSR.BIT.TDRE == 0); /*未送信データが送られるまで待つ*/
SCI1.TDR = data; /*送信データのセット*/
SCI1.SSR.BIT.TDRE = 0; /*送信フラグのクリア*/
return;
}
/* SCI1に文字列を送信する 文字列は'\0'で締めくくっておく**************/
void sci1_strtx(char *str){
while(*str != '\0'){ /* 文字が\0になるまで繰り返す */
sci1_tx(*str); /* 1文字送信*/
str++; /* 次の文字に移る*/
} return;
}
/*サーボ番号をピン番号に割り当てる*/
void srvWari(int srvNo, int x)
{
switch(srvNo){
case 0: P3.DR.BIT.B0 = x;break;
case 1: P3.DR.BIT.B1 = x;break;
case 2: P3.DR.BIT.B5 = x;break;
case 3: P3.DR.BIT.B3 = x;break;
case 4: P3.DR.BIT.B4 = x;break;
case 5: P3.DR.BIT.B7 = x;break;
case 6: P3.DR.BIT.B6 = x;break;
case 7: P3.DR.BIT.B7 = x;break;
case 8: P1.DR.BIT.B0 = x;break;
case 9: P1.DR.BIT.B1 = x;break;
case 10:P1.DR.BIT.B2 = x;break;
case 11:P1.DR.BIT.B3 = x;break;
}
}
/*プログラムの始まり*/
main()
{
char signal;
int i, j,k,count,Width, pWidth[12],checkState[12],servoNo,switchSignal;
int *p, countNum;
unsigned char junNo[3];
unsigned int data[4], sData1,sData2,max_num, min_num;
unsigned int out_data[4];
unsigned int data0[4][3];
P2.DDR = 0x00; /*port2入力に設定 ディップスイッチ用*/
P2.PCR.BYTE = 0xff; /*port2プルアップon */
P3.DDR = 0xff; /*PORT3を出力に設定*/
P1.DDR = 0xff; /*PORT1を出力に設定*/
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
/*Ｈ８ハードの初期化を行う*/
init_io();
Width = 4687;/*パルス幅 初期値*/
/*無限ループ開始*/
switchSignal ='1';
while(1){
p = ForWalk[0]; countNum = ForWalkNum;
for(count = 1;count<=countNum ;count++){
/*パルスを生成*/
/*ＩＴ0のカウント値をゼロにする*/
ITU0.TCNT = 0;
srvWari(0,1); srvWari(1,1);
for(j=0;j<=1;j++) {
out_data[j] = *(p+12*count-12+j);
data[j] = 7365 - out_data[j];
}
if(out_data[0] >= out_data[1] ) {
junNo[0] = 0;
junNo[1] = 1;
}
else {
junNo[0] = 1;
junNo[1] = 0;
}
for(j=0;j<=1;j++){
while (ITU0.TCNT < data[junNo[j]]);
srvWari(junNo[j] ,0);
}
/*ＩＴＵ0のカウント値をゼロにする*/
ITU0.TCNT = 0;
srvWari(2,1); srvWari(3,1);
for(j=0;j<=1;j++) {
out_data[j] = *(p+12*count-12+j+2);
data[j] = 7365 - out_data[j];
}
if(out_data[0] >= out_data[1] ) {
junNo[0] = 0;
junNo[1] = 1;
}
else {
junNo[0] = 1;
junNo[1] = 0;
}
for(j=0;j<=1;j++){
while (ITU0.TCNT < data[junNo[j]]);
srvWari(2+junNo[j] ,0);
}
while(ITU0.TCNT < 60000);/*20ms経過するまで待つ*/
ITU0.TCNT = 0;
while(ITU0.TCNT < 60000);/*20ms経過するまで待つ*/
}/*for count end*/
}/*while end*/
}
|
二足歩行の基礎実験（足 ２０１３年１０月〜２０１４年１月）
|
二足歩行ロボットを歩かせるためのプログラムを考え試しているのですが、なかなか思い通りに
動作してくれません。
「基本的にどうなん…」ということを検証するために、足だけの部分を作りました。
サーボは、以前に使用電力が多すぎてすぐにバッテリーが消耗するものがあったのでそれを
利用しました。
上下２段に分けて、下の段にマイコン基盤とバッテリーを配置しました。
以下に、ＰＣ側で操作した通りに足が動作する最も基本的なプログラムを載せます。
この装置を使っていろいろな実験を試みるつもりです。
最初にＰＣで操作するvisual Basicで組んだプログラム（動作画面とソースプログラム）を、
２番目にマイコン部のＣ言語プログラムを載せます。
左がＰＣ側のＶＢの実行画面です。
４つのチェックボックスがあります。
どれか一つをチェックして右の矢印をクリックするとチェックした個所の数字が増減します。
送信ボタンをクリックすると該当の足の膝関節が設定した数字の位置まで動きます。
次がそのためのＶＢのプログラムです。
|
Public Class Form1
'複数のイベントで使用する変数の定義********************
Dim servo_max, servo_min, targetServo, checkState(14) As Integer
Dim ar_checkbox(12) As CheckBox
Dim i As Integer
Private Sub Form1_Load(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles MyBase.Load
Dim response As MsgBoxResult
'シリアルポートをオープンする
Try
SerialPort1.Open()
Catch ex As IO.IOException
response = MsgBox("エラー！シリアルポートのオープンに失敗しました。
このまま起動しますか？", MsgBoxStyle.YesNo, "起動の確認")
End Try
'Noの場合はプログラム終了
If response = MsgBoxResult.No Then End
'変数の初期化
servo_max = 5565 'ＲＣサーボ出力最大値
servo_min = 0 'ＲＣサーボ出力最小値
'テキストボックスの配列初期化
ar_checkbox(0) = CheckBox1
ar_checkbox(1) = CheckBox2
ar_checkbox(2) = CheckBox3
ar_checkbox(3) = CheckBox4
ar_checkbox(4) = CheckBox5
ar_checkbox(5) = CheckBox6
ar_checkbox(6) = CheckBox7
ar_checkbox(7) = CheckBox8
ar_checkbox(8) = CheckBox9
ar_checkbox(9) = CheckBox10
ar_checkbox(10) = CheckBox11
ar_checkbox(11) = CheckBox12
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を25増やす********************
Private Sub Button4_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button4.Click
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
'サーボの出力値を250減らす
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) + 25)
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を25減らす********************
Private Sub Button5_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button5.Click '変数定義
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
'サーボの出力値を25減らす
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) - 25)
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を300増やす********************
Private Sub Button6_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button6.Click
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
'サーボの出力値を300増やす
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) + 300)
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を300減らす********************
Private Sub Button7_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button7.Click
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) - 300)
End Sub
'マイコンにチェックされたＲＣサーボ信号出力値を送信する********************
Private Sub Button8_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button8.Click
'変数定義
Dim i, checkState(12) As Integer
Dim err_msg As String
Dim buff(64) As Byte
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
Else
checkState(i) = 0
End If
Next
'テキストボックスの数値が最大／最小範囲にあるかチェックする
i = targetServo
If Val(ar_checkbox(i).Text) < servo_min Or Val(ar_checkbox(i).Text) > servo_max
Then
err_msg = Format(i) + "番サーボの出力値が制限範囲 ("
err_msg = err_msg + Format(servo_min) + "〜" + Format(servo_max) + ")
を超えています。
出力を見合わせます。"
MsgBox(err_msg, MsgBoxStyle.OkOnly, )
Exit Sub
End If
'マイコンにＲＣサーボ制御信号 状態変更コマンドを送信
write_sio_command("B")
'targetサーボに信号出力値を送信する
For i = 0 To 11
write_sio_int(ar_checkbox(i).Text)
Next
'ターゲットサーボのチェック状態を送る
For i = 0 To 11
write_sio_int(Str(checkState(i)))
Next
End Sub
'ロボット制御コマンドキャラクター送信********************
Private Sub write_sio_command(ByVal command As Char)
'変数定義
Dim command_char(64), buff(64) As Byte
'マイコンにコマンドを送信
command_char(0) = Asc(command)
SerialPort1.Write(command_char, 0, 1)
'受信確認待ちとエラー処理
Try
buff(0) = 0
SerialPort1.Read(buff, 0, 1)
' MsgBox(buff(0))
Catch ex As TimeoutException
MsgBox("通信エラーです。ロボットが規定時間以内に応答しません")
Exit Sub
End Try
If command_char(0) <> buff(0) Then
MsgBox("通信エラーです。ロボットがコマンドに正しく応答しません " )
Exit Sub
End If
End Sub
'２バイトの整数をシリアルポートから出力する（引数は文字列）********************
Private Sub write_sio_int(ByVal input_str As String)
'変数定義
Dim buff(64) As Byte
Dim output_val As Integer
'テキストボックスの文字列を整数に変換
output_val = Val(input_str)
'変換した整数の出力値を上位バイトと下位バイトに分割してバッファにセット
buff(0) = Int(output_val / 256)
buff(1) = output_val - Int(output_val / 256) * 256
'シリアルポートから出力
SerialPort1.Write(buff, 0, 2)
End Sub
'ＲＣサーボの制御信号出力開始********************
Private Sub Button2_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button2.Click
'変数定義
Dim command_char(64) As Byte
'マイコンにＲＣサーボ制御信号 状態変更コマンドを送信
write_sio_command("A")
'ＲＣサーボ制御信号 開始コマンドを送信
command_char(0) = Asc("1")
SerialPort1.Write(command_char, 0, 1)
End Sub
'ＲＣサーボの制御信号出力停止********************
Private Sub Button3_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button3.Click
'変数定義
Dim command_char(64) As Byte
'マイコンにＲＣサーボ制御信号 状態変更コマンドを送信
write_sio_command("A")
'ＲＣサーボ制御信号 停止コマンドを送信
command_char(0) = Asc("0")
SerialPort1.Write(command_char, 0, 1)
End Sub
Private Sub Button1_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button1.Click
End
End Sub
End Class
|
|
次が足の基盤に設置したマイコン側のＣ言語プログラムです。
|
#include "3048f.h"
/* 待ち時間発生 引数に、必要なミリ秒を指定する***********/
void init_io(void)
{
/*ITU０のクロックを８分週 3125：１ｍｓ、4687.5：１．５ｍｓ、6250：２ｍｓ、62500：２０ｍｓ*/
ITU0.TCR.BYTE = 0x03; /* ITU0、TCNTクリア禁止 clock 1/8 */
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
/* シリアル通信初期化*******************************/
SCI1.SCR.BYTE = 0x00; /* SCI1設定 stop,内部クロック */
SCI1.SMR.BYTE = 0x00; /* data8.stop1,pari non */
SCI1.BRR = 40; /* 19200bps */
ITU0.TCNT = 0; /* ITU0 TCNTｶｳﾝﾄを０にクリア */
while(ITU0.TCNT <= 3125);/*１ｍｓ待つ*/
SCI1.SCR.BYTE = 0x30; /* Tx,Rx有効 ,割込み無効 */
SCI1.SSR.BYTE &= 0x80; /* エラーフラグのクリア */
return;
}
/* SCI1から１文字受信する****************************/
char sci1_rx(void){
char data;
while((SCI1.SSR.BYTE & 0x78)==0); /* 受信とエラーのフラグが立つまで待つ*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return(0xff); /* エラー時はFFを返す */
}
}
/*unsigned int型変数（２バイト）をシリアルポートから入力する*/
/*備考：データが来るまで待つ */
unsigned int sci_read_uintA(void)
{
unsigned int data;
unsigned char data_L, data_H;
/*unsigned charのデータ２個を受け取る*/
data_H = sci1_rxWait();
data_L = sci1_rxWait();
/*unsigned intのデータに変換する*/
data = (unsigned int)data_H * 256 + (unsigned int)data_L;
return(data);
}
/* SCI1から１文字受信する データが来るまで待つ****************************/
unsigned char sci1_rxWait(void){
unsigned char data;
while((SCI1.SSR.BYTE & 0x78)==0) ; /* 受信とエラー以外は待つ*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return((unsigned char)0xff); /* エラー時はFFを返す */
}
}
/* SCI1に１文字送信する */
void sci1_tx(char data){
while(SCI1.SSR.BIT.TDRE == 0); /*未送信データが送られるまで待つ*/
SCI1.TDR = data; /*送信データのセット*/
SCI1.SSR.BIT.TDRE = 0; /*送信フラグのクリア*/
return;
}
/* SCI1に文字列を送信する 文字列は'\0'で締めくくっておく**************/
void sci1_strtx(char *str){
while(*str != '\0'){ /* 文字が\0になるまで繰り返す */
sci1_tx(*str); /* 1文字送信*/
str++; /* 次の文字に移る*/
} return;
}
/*サーボ番号をピン番号に割り当てる*/
void srvWari(int srvNo, int x)
{
switch(srvNo){
case 0: P3.DR.BIT.B0 = x;break;
case 1: P3.DR.BIT.B1 = x;break;
case 2: P3.DR.BIT.B2 = x;break;
case 3: P3.DR.BIT.B3 = x;break;
case 4: P3.DR.BIT.B4 = x;break;
case 5: P3.DR.BIT.B5 = x;break;
case 6: P3.DR.BIT.B6 = x;break;
case 7: P3.DR.BIT.B7 = x;break;
case 8: P1.DR.BIT.B0 = x;break;
case 9: P1.DR.BIT.B1 = x;break;
case 10:P1.DR.BIT.B2 = x;break;
case 11:P1.DR.BIT.B3 = x;break;
}
}
/*プログラムの始まり*/
/*ＰＣから3つの信号を受け取り該当のサーボを動作させる */
/*１：S（制御コマンド）２：サーボ番号０〜１１ ３：パルス幅相当数値を順番に受けて動作*/
main()
{
char signal;
int i, j,Width, pWidth[12],checkState[12],servoNo,switchSignal;
P3.DDR = 0xff; /*PORT3を出力に設定*/
P1.DDR = 0xff; /*PORT1を出力に設定*/
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
/*Ｈ８ハードの初期化を行う*/
init_io();
Width = 4687;/*パルス幅 初期値*/
/*無限ループ開始*/
switchSignal ='1';
while(1){
signal = sci1_rx();/*コマンドが受信されているか確認する*/
sci1_tx(signal);
if(signal == 'A'){/*ＲＣサーボの制御信号 出力ＯＮ／ＯＦＦ命令受信*/
/*スイッチデータ受信*/
switchSignal = sci1_rx();
}
if (switchSignal == '1'){
if( signal =='B') {
for(i=0;i<12;i++) pWidth[i] = sci_read_uintA();/*ﾊﾟﾙｽ幅12個分*/
for(i=0;i<12;i++) checkState[i] =sci_read_uintA();/*ｻｰﾎﾞﾁｪｯｸ状態*/
for(i=0;i<12;i++)
if(checkState[i]==1) {
servoNo = i;
Width = pWidth[i];
}
}
for(i=1;i<=20;i++){
ITU0.TCNT = 0; /* ITU0 TCNTｶｳﾝﾄを０にクリア */
srvWari(servoNo, 1);/*該当のピン出力を１にする*/
while(ITU0.TCNT< 7375 - Width) ;/* 設定値のパルス幅*/
srvWari(servoNo, 0);/*該当のピン出力を０にする*/
while(ITU0.TCNT < 40000);/*１３ｍｓ経過するまで待つ*/
}
}
}
}
|
ＰＡのＢｉｔ２、Ｂｉｔ４、Ｂｉｔ６にサーボの信号線をつなぎます。ＰＢのＢｉｔ０、Ｂｉｔ２にサーボの信号線をつなぎます。
左図のように２０ｍｓ間隔で１ｍｓから２ｍｓの間のパルスを発生させればいいわけです。
このようなパルスをプログラムで発生させるアルゴリズムは左図のようなものです。
