吉野家のロボットを作ろう

はい。
お久しぶりです。

前回で一応色の識別ができるようになったので、今回は「赤い物体の方に頭を向ける」という動きをPen4号にさせたいと思います。
具体的にはコントローラーのSELECTボタンを押すと赤い物体の方に頭を向けるという動作になります。

以下に、ソースコードで変更した箇所（およびその周辺）を貼り付けます。

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

pen4.h内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
  ・
  ・
  ・
/*joints.c に含まれている関数*/
void generate_signal0(struct SERVO_DATA *servo_data);
void wait_val(unsigned int out_data[3]);
void move_servo(struct SERVO_DATA *servo_data);
void servo_update(struct SERVO_DATA *servo_data);
void move_head(struct SERVO_DATA *servo_data,int head_dir, unsigned char head_dir_count);
unsigned int angle2servo_head(int head_dir);
int servo2angle_head(unsigned int servo_data);
  ・
  ・
  ・
/*camera.c に含まれている関数*/
void tr_read_frame(void);
void tr_pickup_red(unsigned char flg, struct SERVO_DATA *servo_data);
  ・
  ・
  ・

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

sio1.c内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
void sio_comm1(struct SIO *sio, struct SERVO_DATA *servo_data, struct ACTION *action)
{
unsigned char i, j, k;
  ・
  ・
  ・
 /*Trevaの画像　撮影＆送信*/
 if(sio->command == 'K')
 {
  /*準備完了*/
  sci_write_char(sio->command);
  /*撮影＆送信*/
  tr_read_frame();
 }

 /*カメラ画像から赤色抽出して1・0で表示*/
 if(sio->command == '3')
 {
  tr_pickup_red(0, servo_data);
 }

  ・
  ・
  ・
}

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

joints.c内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
int servo2angle_head(unsigned int servo_data)
{
int  head_dir;
int  servo_st_en[6];
float a, b;

 /*首の関節パラメータ定義（各機体の固有情報、要書き換え）*/
 servo_st_en[0] = -45;
 servo_st_en[1] = 1125;
 servo_st_en[2] = 0;
 servo_st_en[3] = 2175;
 servo_st_en[4] = 45;
 servo_st_en[5] = 3200;

 /*補間直線の傾きを求める*/
 if ((int)servo_data > servo_st_en[3])
  a = ((float)servo_st_en[4] - (float)servo_st_en[2])
    / ((float)servo_st_en[5] - (float)servo_st_en[3]);
 else
  a = ((float)servo_st_en[2] - (float)servo_st_en[0])
    / ((float)servo_st_en[3] - (float)servo_st_en[1]);
   

 /*補間直線のＹ切片を求める*/
 b = (float)servo_st_en[2] - a * (float)servo_st_en[3];

 head_dir = (int)((a * (float)servo_data + b));

 return(head_dir);
}

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

camera.c内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
void tr_pickup_red(unsigned char flg, struct SERVO_DATA *servo_data)
{
unsigned char xx, yy, v, u, y1, y2, cnt;
unsigned int tcnt;
int    cur_agl, tgt_agl;
float   VV, UU, R1, G1, B1, tmp_agl;
unsigned long tnum;

 tcnt = 0;
 tnum = 0;

 /*画像データの先頭までビット飛ばす*/
 tr_find_startbit();

 /*画像データ取得＆RGB変換＆赤抽出*/
 for(yy = 0; yy < 72; yy++)
 {
  xx = 0;
  cnt = 0;
  while(xx < 96)
  {
   v = tr_read_byte();
   y1 = tr_read_byte();
   u = tr_read_byte();
   y2 = tr_read_byte();
   /*RGB変換*/
   UU = (float)u - (float)128;
   VV = (float)v - (float)128;
   R1 = UU + (float)y1;
   G1 = (float)0.98 * (float)y1 - (float)0.53 * UU - (float)0.19 * VV;
   B1 = VV + (float)y1;

   if( R1 > 255 ) R1 = 255;
   if( G1 > 255 ) G1 = 255;
   if( B1 > 255 ) B1 = 255;
   if( R1 < 0 ) R1 = 0;
   if( G1 < 0 ) G1 = 0;
   if( B1 < 0 ) B1 = 0;

   /*赤色かどうかの簡易判別*/
   if( R1 / G1 > (float)2.0 && R1 / B1 > (float)2.0 )
    if( flg == 0 ) /*flgにはテキスト出力の場合は0、頭動かす場合は１が入っている*/
     sci_write_char('1'); /*赤と判断*/
    else
     cnt++;
   else
    if( flg == 0 )
     sci_write_char('0'); /*赤以外と判断*/
   
   xx = xx + 2;
  }
  
  if( flg == 0 )
  {
   /*改行コードを送信する*/
   sci_write_char(0x0d);
   sci_write_char(0x0a);
  }
  else
  {
   /*赤色の点を加重平均するための処理*/
   tnum = tnum + (unsigned long)cnt * (unsigned long)yy;
   tcnt = tcnt +cnt;
  }
 }

 /*頭を動かす処理*/
 if( flg == 1)
 {
  tnum = tnum / (unsigned long)tcnt;
  /*頭を動かす角度算出*/
  tmp_agl = ( (float)tnum - (float)36 ) * (float)0.478; /*テキストによると1ピクセル＝0.478度*/
  cur_agl = servo2angle_head(servo_data->head); /*頭の現在の角度取得*/
  tgt_agl = cur_agl + (int)tmp_agl;
  /* sci_print_int(tgt_agl); */
  move_head(servo_data, tgt_agl, 1); /*左右60度を超えていても関数中で制限されるので大丈夫*/
 }
}

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

remocon.c内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
void remocon_mode1(struct ACTION *action, struct SIO *sio, struct SERVO_DATA *servo_data)
{
unsigned char case_number;
  ・
  ・
  ・
  /*STARTボタン・・・ 自律モードスタート*/
  if(action->rc_in[3] == 247) case_number = 10;
  /*SELECTボタン・・・赤い物体の方向に頭を動かす*/
  if(action->rc_in[3] == 254) case_number = 11;
 }
  ・
  ・
  ・
 /*自律モード*/
 if(case_number == 10)
 {
  /*自律モード開始*/
  sci_print_string("<< Auto Mode ON >>");
  action->auto_mode = 2;
  auto_mode1(action, sio, servo_data);
  return;
 }

 /*頭を動かす*/
 if(case_number == 11)
 {
  tr_pickup_red(1, servo_data);
  return;
 }

}

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

はい。
以上です。

詳細は、ソースコードを見てください．．．
．．．だとあまりにも不親切なので、ざっと解説します。

まず、tr_pickup_redですが、前回は1ドットごとに赤か否かの判断処理をしていたのですが、そのままだとプログラム領域のメモリをオーバーしてしまったので泣く泣く（？）2ドット毎の処理としました。
横軸方向に単純に加重平均をとって、赤い物体の中心部分の位置を横軸方向のピクセル数に相当する0から71の数で割り出しています。
1ドットあたりの角度＝0.478はテキストの590ページの図から算出しました。
本当は、自分のPen4号でちゃんと計測した方がよいのでしょうが、横着させてもらいました。
あとは、頭の現在の角度をservo2angle_headで取得してそれと算出した角度を足し合わせて頭の動く角度を算出しています。

で、実際に動かしてみました。
　
Pen4号の前に柄の部分の赤いドライバーを適当な高さに（視野内に）置きます。

で、SELECTボタンをプッシュ！

はいっ。
赤い方を向きました！

．．．判断して頭が動くまで4秒ぐらいかかってしまうのは御愛嬌でしょうか．．．
．．．なにかコーディングに問題があるのかな？
もう少し早く動かしたいですね．．．

はい、61日目です。
さらっと続けます。
まだ、時間のあるうちに進めるところまで進みたいので．．．

61日目では、今までVBでおこなっていた画像認識（赤色の識別）を、Pen4号本体側でおこないます。
といっても、Pen4号の限られたハードウェアリソースの範囲内で、ですが．．．

具体的には、カメラから読み込んだ色を１ドットずつ赤色かどうかを判断し、赤色の場合はRS232経由で「1」をそうでない場合は「0」を出力するというものです。

変更もしくは追加した個所は、以下の通りです。
今後は、ソースコードは画像ではなくテキストで貼り付けます。
その方が、もし皆さんが入力される場合に手間が省けるでしょうから．．．

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

camera.c内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

void tr_find_startbit(void)
{
unsigned char i;

 /*100bit連続して１が来るのを待つ*/
 i = 0;
 while(i < 100)
 {
  if(tr_read_bit() == 1) i++;
  else     i = 0;
 }

 /*65bit分の０を検出*/
 i = 0;
 while(i < 65)
 {
  if(tr_read_bit() == 0) i++;
  else     i = 0;
 }

 /*続く２バイト分のデータは無視*/
 for(i = 0; i < 16; i++) tr_read_bit();

}

unsigned char tr_read_byte(void)
{
unsigned char d,i;

 d = 0;
 for(i = 0; i < 8; i++)
 {
  d = d * 2;
  if(tr_read_bit() == 1) d = d | 0x01;
 }
 return(d);
}

void tr_read_frame(void)
{
unsigned int i;

 /*画像データの先頭までビット飛ばす*/
 tr_find_startbit();

 /*画像データ取得＆送信開始*/
 for(i = 0; i < IMG_SIZE; i++) sci_write_char(tr_read_byte());
}

void tr_pickup_red(void)
{
unsigned char xx, yy, v, u, y1, y2;
float   VV, UU, R1, G1, B1, R2, G2, B2;

 /*画像データの先頭までビット飛ばす*/
 tr_find_startbit();

 /*画像データ取得＆RGB変換＆赤抽出*/
 for(yy = 0; yy < 72; yy++)
 {
  xx = 0;
  while(xx < 96)
  {
   v = tr_read_byte();
   y1 = tr_read_byte();
   u = tr_read_byte();
   y2 = tr_read_byte();
   /*RGB変換*/
   UU = (float)u - (float)128;
   VV = (float)v - (float)128;
   R1 = UU + (float)y1;
   R2 = UU + (float)y2;
   G1 = (float)0.98 * (float)y1 - (float)0.53 * UU - (float)0.19 * VV;
   G2 = (float)0.98 * (float)y2 - (float)0.53 * UU - (float)0.19 * VV;
   B1 = VV + (float)y1;
   B2 = VV + (float)y2;

   if( R1 > 255 ) R1 = 255;
   if( R2 > 255 ) R2 = 255;
   if( G1 > 255 ) G1 = 255;
   if( G2 > 255 ) G2 = 255;
   if( B1 > 255 ) B1 = 255;
   if( B2 > 255 ) B2 = 255;

   if( R1 < 0 ) R1 = 0;
   if( R2 < 0 ) R2 = 0;
   if( G1 < 0 ) G1 = 0;
   if( G2 < 0 ) G2 = 0;
   if( B1 < 0 ) B1 = 0;
   if( B2 < 0 ) B2 = 0;

   /*赤色かどうかの簡易判別*/
   if( R1 / G1 > (float)2.0 && R1 / B1 > (float)2.0 )
    sci_write_char('1'); /*赤と判断*/
   else
    sci_write_char('0');
   if( R2 / G2 > (float)2.0 && R2 / B2 > (float)2.0 )
    sci_write_char('1'); /*赤と判断*/
   else
    sci_write_char('0');

   xx = xx + 2;
  }
  /*改行コードを送信する*/
  sci_write_char(0x0d);
  sci_write_char(0x0a);
 }

}

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

pen4.h内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

   ・
   ・
   ・
/*camera.c に含まれている関数*/
void tr_read_frame(void);
void tr_pickup_red(void);
   ・
   ・
   ・

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

sio1.c内

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

void sio_comm1(struct SIO *sio, struct SERVO_DATA *servo_data, struct ACTION *action)
{
   ・
   ・
   ・
 /*カメラ画像から赤色抽出して1・0で表示*/
 if(sio->command == '3')
 {
  tr_pickup_red();
 }

 /*リモコンコントローラボタン状態表示*/
 if(sio->command == '4')
 {
   ・
   ・
   ・

−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

以上です。
．．．何をしているのかはソースコードを見ていただければわかると思いますが、まず、繰り返し使用する処理は関数化しました。
それから、色の判断のところは基本的にVBのソースコードを参照しました。ただ、CPUパワーが弱いことを考慮し処理を簡素化しています。

ターミナルソフトから「3」を入力すると結果を返してきます。

はい。こんな感じです。
実際に出力されてきたものをカメラの設置方向に合わせて90度回転させています。
．．．縦横比が変なのはテキストベースですのでご容赦を．．．
（実際は縦長の画像のはずです）

この際に対象としたもの（プリントアウトした紙）をpen4_vbでも撮影し、閾値35で赤色を抽出しました。

おっ。
結構良い感じじゃないですか。

次回は、これを発展させてPen4号に何らかの動きをさせてみたいと思います。

ついにこの日が来ました．．．
嬉しいような、悲しいような．．．
そう、テキスト卒業の日です。
60日目です。

．．．まあ、勝手に入学・勝手に卒業なので何を勝手に感動しているんだというツッコミをされそうですが．．．そもそもいつ「入学」したんだっけ？入学許可はもらったの？というツッコミは無しです。

吉野さん、この本を書いてくれて本当にありがとう。
この本から色々なことを学びました。
ここで学んだことは今後もきっと役に立つと思います。

4月からはロボット技術者の端くれとして活動する予定です。
吉野さんは「ロボット関連技術の普及と発展の方向に寄与」したいという動機からこの本を書かれたとのことですが、少なくとも私一人の今後進む方向に対してこの本は大きな影響を与えました。
まあ、私一人が頑張ったところでどの程度ロボット関連技術の発展に寄与出来るかどうかは分かりませんが、こういう小さな一歩が集まると大きな一歩になるのではないかと考えています。

4月からちょっと忙しくなりそうなのでこのブログの更新頻度は落ちてしまうと思いますが、可能な限りPen4号に改良を加えて発表していきたいと思います。

はい。59日目です。
いよいよ作業日としては最終日となります。
59日目は無線でPen4号を操縦出来るようにします。
ハードウェアとソフトウェアの製作を全て1作業日で実行しますので、結構時間がかかります。

あ、その前に。
58日目で、なんか色が悪いな（かつ青のボールなのに赤成分が入っていて変だな）と思いつつも先へ進んでしまいましたが、原因が分かりました。
画像をカメラの設置位置にあわせて縦横を置き換える際に色の変換式を置き換えるのを忘れていました。
そのため、変な色になっていたようです。
修正後に同じものを撮影しました。

はい。問題ありませんね。
閾値35でもばっちり色分けされています。
お騒がせ致しました。

さて、ハードウェアの工作です。
テキストの593ページに4.7KΩの抵抗が二つ出てきます。
正確にこの数値である必要はないのでしょうが、お買い物リストの通りお買い物をされた皆さんは近い値の抵抗が手元にないと思います。
私も無いと思いあきらめていました。

が、
抵抗二つのために秋葉原に行く気にはなれず家中を引っかき回して既存の電気回路から4.7KΩの抵抗を二つ調達しました。
．．．これはこれで問題かも．．．
ま、いいか。
成仏してくれ、ワンダーキットの「電話ベルスイッチ」よ。

あと、テキスト593ページの回路図中の当該抵抗が接続されるVCCですが、これは5Vではなく3.3Vであろうと判断し、そのように半田付けしました。
595ページの写真からはそのように見えますし、受信機からの出力は3.3Vになるのがスジだと思ったからです。

で、テキストの通り半田付け→テスターでショートがないかチェック→I/Oボード単体で電圧チェック、をします。

受信機も粛々と作業をします。

．．．受信機をこのような状態にする前に、一度プレイステーションに接続して不良品ではないかチェックすることをおすすめします。
こうしてから不良品だと判明してもお店側は交換してくれないでしょうから．．．

で、胴体部へインストールです。

うーん。
かなりゴチャゴチャしてきました。
でも、なんとか入っています。
これ以上のハードウェア導入は難しそうだなぁ．．．

次は、ソフトウェア部分の製作です。
ダウンロードしたソースコードをコピーしてペーストでOKです。

ただ、そのままだと補間モーションで作成した「あいさつ」と「伏せ」と「伏せからの起きあがり」が実行出来ませんので、それを実行したい方は以下のようにremocon_mode1に手を加えます。
　
．．．まあ、ご参考までにということで。
ちなみに、それぞれ順番に○ボタン、×ボタン、△ボタンに割り当てました。
ついでに、スタートボタンを押したら自律モードが開始するようにもしました。

これだけだと、「伏せ」の時に加速度センサが働いてサーボが脱力してしまうので、もう一カ所mainの中身を書き換えます。

こんな感じです。

これで一応今までPC経由で行っていた動作は全てリモコンから出来るようになりました。
後はカメラに関してもリモコンを使って何らかの操作をしたいですね．．．
まあそれは61日目以降ということで．．．

そうそう。
先送りになっていた頭のカバーを作りました。

色々候補を考えたのですが、最終的に一番左側のにしました。

　
．．．なんか、ペンギンからは離れてしまったなぁ．．．
でも、カメラを頭部に付けるためだ。仕方あるまい。

以上で、59日目も終了です。

はい。58日目です。
58日目は56日目から続いているカメラによる画像処理の最終回です。

いつもの如く、ソースコードをダウンロードして．．．って、いちいちこのコメントは要らないか．．．

そうそう。
57日目のところで「後日と言うことで」と書いていた、カメラ設置方向にpen4_vbの表示をあわせる作業をしました。
テキスト通りにカメラを設置している皆さんには関係のない作業ですが．．．
ついでに、原寸（？）の画像も欲しかったので、カメラの1ピクセルがvb上で1ドットに表示されるPictureBoxを追加しました。

我らがテキストブックのオビの部分を撮影したものです。
「60日後にこの姿！！」とあります。
いやー、60日では無理っすよ．．．私には．．．

いよいよ、赤や青や黄色のボールを撮影．．．って、そんなもん家にありません！
なので、「ペイント」を使って適当な画像を作成し、それを印刷したものをPen4号に見せます。

まあ、こんな感じの画像を印刷すればよいのではないでしょうか。
どうせ、Pen4号にはこれが2次元なのか3次元なのか分からないでしょうから。

で、撮影したものを閾値35で赤の抜き出しを試みます。

．．．うーん．．．
なんだか、青いボールの赤い成分を拾ってますね．．．
そんな成分入っていないのに．．．
大丈夫か？

あきらめずに、閾値25で挑戦です。

おっ！だいぶ良いですね。
後一歩。

ということで、閾値20。

はいっ。
いいでしょう。

以上で58日目も終了です。
ここから発展させるには、PCとのシリアル通信に10秒かかってしまうことを考えると、Pen4号単体で信号処理をしなければいけませんよねぇ．．．
うーん、大変そう。
まあ、それは61日目以降ということで．．．