<59>mbed入門 〜mbedでジャイロとサーボを動かそう 課題1〜

mbedに接続してみたいアイテムの一つにジャイロモジュールがあります。これまでは部品としてのジャイロは市販品はあっても高価でしたが、秋月電子より基板実装済みの圧電振動ジャイロモジュールAE-GYRO-SMDが安価に発売されたので挑戦しましょう。

■課題1

ジャイロ1の出力をmbedにAD変換して取り込み、LCDに表示させるプログラムを制作しましょう。

■動作原理

ジャイロは回転を検知して電圧に変換するセンサの仲間です。圧電セラミックに電圧を加えると振動します。振動している素子に回転力が加わるとコリオリの力が発生し、素子に微妙なゆがみを生じます。このゆがみを電圧として取り出すのが圧電振動ジャイロです。

■圧電振動ジャイロモジュール AE-GYRO-SMDの仕様

・使用ジャイロ:村田製作所圧電振動ジャイロ(ジャイロスター)
・電源電圧:2.7〜5.25V
・検出範囲:±300deg/sec
・静止時出力:1.35V
・応答性:50Hz
・感度:0.67mV/dev/sec
・内蔵アンプ出力倍率:10倍

ジャイロモジュールには2つのジャイロが実装されています。基板は完成品しているので、コネクタを取り付ければすぐに使うことができます。

1

ブレッドボードに刺すことができるように、4ピンコネクタを取り付けます。写真で横に取り付けられているがジャイロ1、縦に取り付けられているのがジャイロ2です。基板にはG1、G2と印刷されています。

2

ピンの取り付けははんだ付けにより行います。

3

コンデンサC6とC7を短絡することにより、微分回路をOFFにすることができます。スズメッキ線をチップコンデンサ表面に取り付けることで短絡します。

4

■mbed回路図

mbedに20桁4行LCDを接続したものを基本システムとします。これにジャイロモジュールを接続します。ジャイロの動作電源電圧範囲は2.7〜5.25Vと広いため、mbedと電源を共通にすることができます。

ジャイロ1の出力・・・p16
ジャイロ2の出力・・・p17

5

ジャイロモジュールはブレッドボードに両面テープなどで取り付けます。ぐらぐらすると誤動作するので、しっかり貼り付けてください。

6

■ジャイロモジュールとmbedの配線

■■ジャイロの配線

1・・・ジャイロ1出力(青)
2・・・ジャイロ2出力(緑)
3・・・+5V
4・・・GND

7

■■mbedの配線

p16・・・ジャイロ1の出力(青)
p17・・・ジャイロ2の出力(緑)

8

■完成!

以上で完成です。

9

■mbedプログラミング

ジャイロデータはmbedのアナログ入力に接続し、AD変換します。0〜1.0の値に変換してmbed取り込みます。

・ジャイロ接続のポート設定

AnalogIn gyro1_adc(p16);//ジャイロ1
AnalogIn gyro2_adc(p17);//ジャイロ2

・ジャイロ出力の読み込み

gy1_data=gyro1_adc.read();//ジャイロ1出力の読み込み

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test01/lxtb33

■■mbedソースリスト

//Gyro test01
// Akidukidensi AE-GYRO-SMD
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0

AnalogIn gyro1_adc(p16);//ジャイロ1
AnalogIn gyro2_adc(p17);//ジャイロ2

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

int main() {
  float gy1_data;
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test01***\n");         

  while(1){   
      gy1_data=gyro1_adc.read();//ジャイロ1データの読み込み
      lcd.locate(0,1);
      lcd.printf("Gyro1:%2.5f",gy1_data);
  }//while    
}//main      

■完成!

今回のプログラムではジャイロ1の出力だけを読み込みます。LCDに対して、手前と反手前方向にブレッドボードを回転させて、出力が変化すればジャイロ1の読み込みは成功です。

10

参考図書:ジャイロセンサ技術 多摩川精機編 東京電機大学出版局

長野県飯田工業高校 竹内浩一

========<コラム>=====================

ジャイロ・・・ジャイロスコープが一般名称です。

圧電振動ジャイロモジュール AE-GYRO-SMD・・・秋月電子で販売中です。http://akizukidenshi.com/catalog/g/gM-02758/

コリオリ力・・・コマを高速で回転します。このコマに傾けるなどの力を加えると元の状態を維持しようとする慣性力が発生します。この慣性力をコリオリ力または転向力といいます。

古いジャイロ・・・コリオリ力を検知して角速度として出力するのがジャイロです。古いタイプでは本物のコマを内蔵して回転させていたために、機械的振動に弱い、消費電流が多い、大型になるなどの弱点がありました。

ジャイロの種類・・・大きく分けて機械式、流体式、ガス式、光学式などがあります。今回使用する圧電振動ジャイロは機械式振動型に分類されます。

微分回路をOFF・・・搭載している村田製作所ジャイロスターの標準回路でもC6とC7に相当するコンデンサを取り付けるようになっていますが、角度を検出したい今回の用途では短絡してOFFにします。

ぐらぐらすると誤動作・・・電源ON時に静止時電圧を読み取るので、起動時はそっとしておいてください。

回転方向・・・村田製作所ジャイロスターは長手方向を軸にして回転させます。LCDディスプレイを手前、反手前に回転させます。回転させるのであって、傾けるのではないことに注意してください。


<60>mbed入門 〜mbedでジャイロとサーボを動かそう 課題2〜

ジャイロをmbedに接続することに成功しましたか?。接続した圧電振動ジャイロモジュール AE-GYRO-SMDにはもう一つジャイロを搭載しています。こちらも使えるようにしましょう。

■課題2

ジャイロ1に加えて、ジャイロ2の出力をmbedに取り込み、LCDに表示するプログラムを制作しましょう。実際の電圧に変換して表示します。

■mbedプログラミング

・ジャイロ1とジャイロ2の出力をを表示します。mbedのADコンバータは0〜3.3Vを0〜1.0に変換して読み込みます。3.3をかけることで実際の電圧に戻すことができます。

 lcd.locate(0,1);
 lcd.printf("Gyro1:%2.5f %2.2fV",gy1_data,gy1_data*3.3);//ジャイロ1
 lcd.locate(0,2);
 lcd.printf("Gyro2:%2.5f %2.2fV",gy2_data,gy2_data*3.3);//ジャイロ2

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test02/lxsxjx

■■mbedソースリスト

//Gyro test02
// Akidukidensi AE-GYRO-SMD
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0

AnalogIn gyro1_adc(p16);
AnalogIn gyro2_adc(p17);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

int main() {
  float gy1_data,gy2_data;
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test02 ***\n");         

  while(1){   
      gy1_data=gyro1_adc.read();
      gy2_data=gyro2_adc.read();//ジャイロ2出力読み込み
      lcd.locate(0,1);
      lcd.printf("Gyro1:%2.5f %2.2fV",gy1_data,gy1_data*3.3);//表示と電圧変換
      lcd.locate(0,2);
      lcd.printf("Gyro2:%2.5f %2.2fV",gy2_data,gy2_data*3.3);
  }//while    
}//main      

■完成!

ジャイロ1に加えてジャイロ2の出力と、実際の電圧を表示します。

ジャイロ1の静止時電圧:約1.24V
ジャイロ2の静止時電圧:約2.39V

ジャイロの静止時電圧は個体差があるようです。

11

長野県飯田工業高校 竹内浩一

========<コラム>=====================

2つのジャイロ・・・2つのジャイロは直行する形で搭載されています。相互に影響を与えないように違う型番です。

静止電圧・・・1.35Vが仕様です。ジャイロ1は1.24Vで低めです。ジャイロ2は2.39Vでかなり高めです。試作実験には支障がないためにこのまま使用しています。


<61>mbed入門 〜mbedでジャイロとサーボを動かそう 課題3プレ〜

2つのジャイロをうまくmbedに接続することができましたか?。取り込んだ値を角度に変換しましょう。

■課題3プレ

ジャイロは角度の変化を検知して角速度として出力します。出力された角速度を積分すると角度に変換することができます。ジャイロの角速度から角度を計算するmbedプログラムを作成しましょう。次の課題3のプレ課題(準備課題)になります。

■■積分による角度計算

圧電振動ジャイロは感知した角速度を電圧に変換して出力します。静止電圧を中心として正回転=右回転であれば静止電圧より大きく=(+)、逆回転であれば小さく=(-)出力します。この電圧を一定時間毎に定期的に読み込むことをサンプリングといいます。次の図はt0秒ごとにサンプリングし、角速度wk0、wk1を電圧として得ています。

37

サンプリングした角速度を感度で割ると角度の瞬時値になります。この瞬時値を積分すると角度を得ることができます。

角度=∫ジャイロ出力(0〜t)

■■実際の計算方法

ジャイロ出力電圧:wk1(V)
一つ前のジャイロ出力電圧:wk0(V)
取り込み間隔=サンプリングタイム:t0(s)
角度:sgm (°)
ジャイロ感度:k (mV/dev/sec)
静止電圧:center(V)
内蔵アンプ倍率:10倍

mbedは直接積分計算をすることはできません。そこで、台形法により近似値として計算します。サンプリングタイムをt0とするとt0時間の角度変化は台形abcdの面積Sになります。

S=(ab+cd)×t0×1/2

サンプリングタイムt0が0.1sであれば、10回サンプリングすれば1秒間になり、1秒後の角度を求めることができます。

■■計算の手順

ジャイロ出力電圧より静止電圧centerを引くとwk1を求めることができます。

wk1=ジャイロ出力電圧-center

ジャイロ出力電圧をジャイロ感度で割ることによりサンプリングタイムt0での角度の瞬時値に変換できます。

wk1=wk1/k

ab=wk0、cd=wk1、bdは直線とみなします。すると台形abcdの面積Sは、次の式に置き換えることができます。一番最初のwk0は0とします。

S=(wk0+wk1)×t0×1/2

Sはサンプリングタイムt0の角度変化なので、ある時間t秒後の角度は、t0ごとに求めたSを順次加えたものになります。t0が0.1s間隔であれば、10回Sを加えると1秒後の角度sgmを求めることができます。

■mbedプログラミング

■■C言語での記述

C言語では次のように記述します。

・静止電圧の準備

  gy1_center=gy1_adc.read()*3.3;  

・ジャイロ出力電圧の取り込み

  gy1_data=gy1_adc.read()*3.3;

・取り込み間隔は0.1秒

  wait(0.1);

・wk1の変換 0.67mV ×10倍=6.7mV

  gy1_wk1=(gy1_data-gy1_center)*1000/6.7;

・台形法による積分

  gy1_sgm=gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2;

・一つ前のデータの準備

  gy1_wk0=gy1_wk1;

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test03-00/lxsjvv

■■mbedソースリスト

//Gyro test03-00
// Akidukidensi AE-GYRO-SMD
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define Gy1_offset 0.01

AnalogIn gy1_adc(p16);
AnalogIn gy2_adc(p17);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

int main() {
  float gy1_data;
  float gy1_center;
  float gy1_sgm=0;
  float gy1_wk0=0,gy1_wk1=0;
  int i;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("** Gyro test03-00 ***\n");

  gy1_center=gy1_adc.read()*3.3;  
  
  while(1){
    gy1_data=gy1_adc.read()*3.3;
    wait(0.1);
    
    gy1_wk1=(gy1_data-gy1_center)*1000/6.7;
    gy1_sgm=gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2;
    gy1_wk0=gy1_wk1;
    
    if(gy1_sgm > 90){
      gy1_sgm=90;  
    }
    if(gy1_sgm < -90){
      gy1_sgm=-90;
    }
    
    lcd.locate(0,1);
    lcd.printf("%3.4f %3.4f",gy1_center,gy1_data);
    lcd.locate(0,2);
    lcd.printf("Deg:%3.0f",gy1_sgm);
  }//while    
}//main 

■完成!

Deg:で表示される角度がブレッドボードを手前(逆回転)、反手前(正回転)に回転させることにより 増えたり、減ったりすれば成功です。

12

手前に回転すると、角度は増加します。

13

反手前に回転すると角度は0=>マイナスへと減少します。

14

長野県飯田工業高校 竹内浩一

========<コラム>=====================

角度・・・角速度を積分すると角度を得ることができます。

積分・・・初めて微分や積分を数学で習ったとき、”こんなものが何の役に立つのだろう?”と感じましたが、役に立つんですね〜。

正回転・・・右回転を正回転とします。

サンプリング・・・細かく数は多いほど、良い場合もあるのですが、このジャイロではあまりサンプリング数を増やすとノイズも増加するために思ったような結果を得ることができないときがあります。

静止電圧とドリフト・・・起動時の静止電圧がそのまま変化しなければ何も問題は起こらないのですが、ふらふらと上昇、下降をしてしまうことが発生します。これをドリフトといいます。このジャイロでは温度変化の影響によるドリフトが目立つようです。試しにジャイロに指を当てると指先の体温でジャイロが加熱されて静止電圧が変化するのがわかります。

ジャイロ感度・・・ジャイロ感度は0.67 mV/dev/secですが、オペアンプで10倍に増幅しているので、6.7mV/dev/secが実際の感度です。センサの回転軸が1秒間に1度進む速さで回転しているときの出力電圧が(静止電圧+6.7mV)であることを意味します。ちょうど1秒間で10度動かせば静止電圧が1.35Vの場合、1.35+67mV=1.35+0.067=1.417Vになります。

一つ前のジャイロ出力電圧:wk0(V)・・・一番最初は0を代入します。2回目からはwk1を代入します。

手前と反手前・・・ジャイロの右回転が正回転なので、LCDを反手前に回転すると本来の正回転です。感覚的に逆なので、ここでは手前回転を正回転としています。

ドリフトの影響・・・大きなドリフトが発生すると手前<=>反手前を繰り返すほどに誤差が積算されます。手前=>水平にしても0に戻らなくなります。


<62>mbed入門 〜mbedでジャイロとサーボを動かそう 課題3〜

課題3予備のようにジャイロ出力値を積分すれば角度になりますが、次のような問題点があります。

■問題点

・静止していても角度が勝手に増える、または減ってしまう。
  =>温度変化に敏感で、静止時電圧が変化=ドリフトするための現象です。

・0度から手前に回転し、0度に戻しても0度を表示しない。
  =>これもドリフトが原因です。回転中に静止時電圧が変化し続けています。

これらの問題点を少しでも解消できるようなプログラムを考えましょう。

■課題3

問題点を対策したジャイロプログラムを作成しましょう。

■mbedプログラミング

■■対策1

小数点以下3桁目を四捨五入する関数round3()を用意し、微少なジャイロ出力をノイズとしてカットします。

double round3(double x){
  double y;
  y=double(int((x+0.005)*100))/100;
  return(y);
}

■■対策2

静止時電圧は起動時に大きく変化することが多いです。そこで、静止時電圧:gy1_centerを起動時に0.001秒間隔で10回読み取り、平均値とします。

 gy1_center=0;
  for(i=0;i<10;i++){
    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
    wait(0.01);
  }
  gy1_center=round3(gy1_center/10);

■■対策3

ジャイロ出力を0.01秒間隔で読み取り、平均値を積分し、ノイズ対策とします。したがって、読み取り間隔は0.1秒間隔となります。

  for(i=0;i<10;i++){
      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
      wait(0.01);
    }
    gy1_avg=round3(gy1_sum/10);

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test03/lxrxi5

■■mbedソースリスト

//Gyro test03
// Akidukidensi AE-GYRO-SMD
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define Gy1_offset 0.01

DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
AnalogIn gy1_adc(p16);
AnalogIn gy2_adc(p17);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

double round3(double x){
  double y;
  y=double(int((x+0.005)*100))/100;
  return(y);
}

int main() {
  float gy1_data;
  float gy1_center;
  float gy1_sgm=0;
  float gy1_wk0=0,gy1_wk1=0;
  float gy1_avg=0,gy1_sum;
  int i;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test03 ****\n");

  gy1_center=0;
  for(i=0;i<10;i++){
    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
    wait(0.01);
  }
  gy1_center=round3(gy1_center/10);
  
  
  while(1){
    gy1_sum=0;
    for(i=0;i<10;i++){
      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
      wait(0.01);
    }
    gy1_avg=round3(gy1_sum/10);
    
    gy1_wk1=round3((gy1_avg-gy1_center)*1000/6.7);
    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
    gy1_wk0=gy1_wk1;
    
    if(gy1_sgm > 90){
      gy1_sgm=90;  
    }
    else if(gy1_sgm < -90){
      gy1_sgm=-90;
    }
    
    lcd.locate(0,1);
    lcd.printf("%3.4f %3.4f",gy1_avg,gy1_center);
    lcd.locate(0,2);
    lcd.printf("Deg:%2.0f",gy1_sgm);
  }//while    
}//main      

■完成!

静止状態での勝手な移動=ドラフトは少し収まっています。

15

手前に回転させます。

16

反手前に回転します・・・その後、水平に戻すと、ほぼ角度が0になるように改善されています。

17

いくつかの方法を組み合わせて対策しましたが、完璧ではないです。時間が経過するにしたがってドラフトは発生します。さらなる工夫を重ねてみてください。

長野県飯田工業高校 竹内浩一

========<コラム>=====================

問題点・・・ジャイロを上手に使うには問題点が目立たないように使うこと大切です。角度を数値で正確に表示するとドリフトが目立ちます。ラジコンで使うジャイロのように操縦者が必要に応じて補正するシステムにすれば目立たなくなります。

対策・・・ここでの対策は完全なものではありません。起動後、しばらくはドリフトに追従することができますが、ニュートラルが動き出してしまうこともあります。ここで紹介するプログラムを何かに応用するときは充分な実験を重ねて検証してから利用してください。利用した結果損害が生じても筆者はその責任を負うことはできません。


<63>mbed入門 〜mbedでジャイロとサーボを動かそう 課題4〜

ジャイロの動作は安定してきましたか?。表示を工夫して見やすくしましょう。

■課題4

ジャイロ1の出力を角度だけではなく、”*”を使って視覚的に表示しましょう。課題3までは角度を数値で表示していましたが、*を左右に動かして、視覚的に表示します。

■mbedプログラミング

・目盛りの表示

 lcd.printf("--------+--------");

・角度からLCD桁への変換

  gy1_lc=(gy1_sgm-5)/10+9;

・桁(0〜19の20桁)のはみ出し処理

  if(gy1_lc >20){
      gy1_lc=19;
  }
  if(gy1_lc <0){
      gy1_lc=0;
  }

・”*”の表示 一つ前の場所の*を消して、次を書くことを繰り返します。

  lcd.locate(gy1_lc_old,3);
  lcd.printf(" ");
  lcd.locate(gy1_lc,3);
  lcd.printf("*");
  gy1_lc_old=gy1_lc;

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test04/lxsxaq

■■mbedソースリスト

//Gyro test04
// Akidukidensi AE-GYRO-SMD
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define Gy1_offset 0.01

DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
AnalogIn gy1_adc(p16);
AnalogIn gy2_adc(p17);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

double round3(double x){
  double y;
  y=double(int((x+0.005)*100))/100;
  return(y);
}

int main() {
  float gy1_data;
  float gy1_center;
  float gy1_sgm=0;
  float gy1_wk0=0,gy1_wk1=0;
  float gy1_avg=0,gy1_sum;
  int i,gy1_lc=9,gy1_lc_old=0;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test04 ****\n");

  gy1_center=0;
  for(i=0;i<300;i++){
    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
    wait(0.001);
  }
  gy1_center=round3(gy1_center/300);
  
  while(1){
    gy1_sum=0;
    for(i=0;i<20;i++){
      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
      wait(0.005);
    }
    gy1_avg=round3(gy1_sum/20);
    
    gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);
    if(gy1_wk1 < 0){
      gy1_wk1=gy1_wk1*1.1;
    }
    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
    gy1_wk0=gy1_wk1;
    
    if(gy1_sgm > 90){
      gy1_sgm=90;  
    }
    else if(gy1_sgm < -90){
      gy1_sgm=-90;
    }
    
    lcd.locate(0,1);
    lcd.printf("%3.3f %3.3f d:%3.0f",gy1_avg,gy1_center,gy1_sgm);
    lcd.locate(0,2);
    lcd.printf("--------+--------");
    gy1_lc=(gy1_sgm-5)/10+9;
    if(gy1_lc >20){
      gy1_lc=19;
    }
    if(gy1_lc <0){
      gy1_lc=0;
    }
    lcd.locate(gy1_lc_old,3);
    lcd.printf(" ");
    lcd.locate(gy1_lc,3);
    lcd.printf("*");
    gy1_lc_old=gy1_lc;
  }//while    
}//main      

■完成!

起動直後のLCD表示です。”*”がセンターに表示されています。

18

手前に34度回転しました。”*”は右に移動しています。

19

戻して、反手前方向に22度回転しています。”*”は左側に移動しています。

20

長野県飯田工業高校 竹内浩一

========<コラム>=====================

視覚的・・・ジャイロ1を回転すると”*”が左右に動く様子を見ていると往年のテニスゲームを思い出します。キャラクタしか表示できない当時のパソコンで■や*などを使ってゲームをプログラミングしていました。

桁数・・・LCDは20桁表示できるので、左右は10桁ずつになります。偶数ずつなのでちょうど真ん中が困ります。そこで、左:0〜8、センタ:9、右10〜18としています。


<64>mbed入門 〜mbedでジャイロとサーボを動かそう 課題5〜

ジャイロ出力から角度を求めることができましたか?。ここではサーボを接続して、ジャイロの角度に合わせてサーボを動かします。サーボはmbedのPWM出力を活用して駆動します。

■課題5

ジャイロ出力に合わせて、接続したサーボを動かしましょう。

■mbed回路図

サーボはmbedのp21に接続し、PWMで駆動します。

21

■mbedプログラミング

・サーボ接続はp21です

  PwmOut servo1(p21);

・サーボ出力周期は20ms=50Hzです。

   servo1.period_ms(20);

・ジャイロの角度をサーボのパルス幅に適合する値に変換します。
 ジャイロの角度:-90〜+90
 サーボパルス幅:1.0ms〜2.0ms
 サーボの回転方向と手前への回転方向を一致させるために角度を反転しています。

  pwidth=-gy1_sgm;
  pwidth=(pwidth+90)/180/1000+0.001;

・パルスをサーボに出力します。

 servo1.pulsewidth(pwidth);

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test05/lxtvvv

■■mbedソースリスト

//Gyro test05
// Akidukidensi AE-GYRO-SMD
// with Servo
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define Gy1_offset 0.01

AnalogIn gy1_adc(p16);
AnalogIn gy2_adc(p17);
PwmOut servo1(p21);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

double round3(double x){
  double y;
  y=double(int((x+0.005)*100))/100;
  return(y);
}

int main() {
  float gy1_center;
  float gy1_sgm=0;
  float gy1_wk0=0,gy1_wk1=0;
  float gy1_avg=0,gy1_sum;
  int i,gy1_lc=9,gy1_lc_old=0;
  float pwidth;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test05 ****\n");
  servo1.period_ms(20);

  gy1_center=0;
  for(i=0;i<300;i++){
    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
    wait(0.001);
  }
  gy1_center=round3(gy1_center/300);
  servo1.pulsewidth(0.0015);
  
  while(1){
    gy1_sum=0;
    for(i=0;i<20;i++){
      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
      wait(0.005);
    }
    gy1_avg=round3(gy1_sum/20);
    
    gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);
    if(gy1_wk1 > 0){
      gy1_wk1=gy1_wk1*1.07;// migi hosei
    }
    if(gy1_wk1 < 0){
      gy1_wk1=gy1_wk1*0.95;//hidari hosei
    }
    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
    gy1_wk0=gy1_wk1;
    
    if(gy1_sgm > 90){
      gy1_sgm=90;  
    }
    else if(gy1_sgm < -90){
      gy1_sgm=-90;
    }
    
    pwidth=-gy1_sgm;
    pwidth=(pwidth+90)/180/1000+0.001;
    gy1_lc=(gy1_sgm-5)/10+9;
    if(gy1_lc >20){
      gy1_lc=19;
    }
    if(gy1_lc <0){
      gy1_lc=0;
    }
    
    lcd.locate(0,1);
    lcd.printf("%3.2f %3.2f %2.0f %2.1f",gy1_avg,gy1_center,gy1_sgm,pwidth*1000);
    lcd.locate(0,2);
    lcd.printf("--------+--------");
    lcd.locate(gy1_lc_old,3);
    lcd.printf(" ");
    lcd.locate(gy1_lc,3);
    lcd.printf("*");
    gy1_lc_old=gy1_lc;
    servo1.pulsewidth(pwidth);
  }//while    
}//main      

■完成!

起動すると、サーボは初期化され、ニュートラル位置に移動します。

22

手前に回転するとサーボは右方向に回転します。角度31度、パルス1.3msです。

23

反手前に方向に回転するとサーボは左方向に回転します。角度-39度、パルス幅1.7msです。

24

長野県飯田工業高校 竹内浩一

========<コラム>=====================

サーボについて・・・mbedでサーボを動かすには次のURLも参考にしてください。http://www.eleki-jack.com/arm/2011/09/mbedmbed1-1.html


<65>mbed入門 〜mbedでジャイロとサーボを動かそう 課題6〜

これまでにジャイロ出力を1つ処理するプログラムを作成しました。同じアルゴリズムでジャイロ2の出力を処理するプログラムを用意します。

■課題6

ジャイロモジュールに搭載している2つのジャイロの動きを”*”で表示するプログラムを作成しましょう。この課題ではまだサーボ2は接続しません。

■mbedプログラミング

・gy2_が先頭にある変数を準備し、ジャイロ2の処理に使います。

 float gy2_center;
 float gy2_sgm=0;
 float gy2_wk0=0,gy2_wk1=0;
 float gy2_avg=0,gy2_sum;
 int gy2_lc=9,gy2_lc_old=0; 

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test06/lxt6kk?action=edit

■■mbedソースリスト

//Gyro test06
// Akidukidensi AE-GYRO-SMD
// 2Gyro
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define Gy1_offset 0.01

DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
AnalogIn gy1_adc(p16);
AnalogIn gy2_adc(p17);
PwmOut servo1(p21);
PwmOut servo2(p22);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

double round3(double x){
  double y;
  y=double(int((x+0.005)*100))/100;
  return(y);
}

int main() {
  float gy1_center;
  float gy1_sgm=0;
  float gy1_wk0=0,gy1_wk1=0;
  float gy1_avg=0,gy1_sum;
  int i,gy1_lc=9,gy1_lc_old=0;

  float gy2_center;
  float gy2_sgm=0;
  float gy2_wk0=0,gy2_wk1=0;
  float gy2_avg=0,gy2_sum;
  int gy2_lc=9,gy2_lc_old=0;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test06 ****\n");

  gy1_center=0;
  gy2_center=0;
  for(i=0;i<300;i++){
    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
    gy2_center=gy2_center+round3(gy2_adc.read()*3.3);
    wait(0.001);
  }
  gy1_center=round3(gy1_center/300);
  gy2_center=round3(gy2_center/300);
  
  while(1){
    gy1_sum=0;
    gy2_sum=0;
    for(i=0;i<20;i++){
      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
      gy2_sum=gy2_sum+round3(gy2_adc.read()*3.3);
      wait(0.005);
    }
    gy1_avg=round3(gy1_sum/20);
    gy2_avg=round3(gy2_sum/20);
    
    gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);
    gy2_wk1=round3((gy2_avg-gy2_center)*1000/5);
    if(gy1_wk1 > 0){
      gy1_wk1=gy1_wk1*1.07;// migi hosei
    }
    if(gy1_wk1 < 0){
      gy1_wk1=gy1_wk1*0.95;//hidari hosei
    }
    
    if(gy2_wk1 > 0){
      gy2_wk1=gy2_wk1*1.07;// migi hosei
    }
    if(gy2_wk1 < 0){
      gy2_wk1=gy2_wk1*0.95;//hidari hosei
    }
    
    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
    gy2_sgm=round3(gy2_sgm+(gy2_wk0+gy2_wk1)*0.1/2);
    gy1_wk0=gy1_wk1;
    gy2_wk0=gy2_wk1;
    
    if(gy1_sgm > 90){
      gy1_sgm=90;  
    }
    else if(gy1_sgm < -90){
      gy1_sgm=-90;
    }
    
    if(gy2_sgm > 90){
      gy2_sgm=90;  
    }
    else if(gy2_sgm < -90){
      gy2_sgm=-90;
    }

    gy1_lc=(gy1_sgm-5)/10+9;
    if(gy1_lc >20){
      gy1_lc=19;
    }
    if(gy1_lc <0){
      gy1_lc=0;
    }
    
    gy2_lc=(gy2_sgm-5)/10+9;
    if(gy2_lc >20){
      gy2_lc=19;
    }
    if(gy2_lc <0){
      gy2_lc=0;
    }
    
    lcd.locate(0,1);
    lcd.printf("--------+--------");
    lcd.locate(gy1_lc_old,2);
    lcd.printf(" ");
    lcd.locate(gy1_lc,2);
    lcd.printf("*");
    gy1_lc_old=gy1_lc;
    lcd.locate(gy2_lc_old,3);
    lcd.printf(" ");
    lcd.locate(gy2_lc,3);
    lcd.printf("*");
    gy2_lc_old=gy2_lc;
  }//while    
}//main      

■完成!

起動すると、”*”はセンターに表示されます。

25

手前方向と右方向に回転すると”*”はこのように移動します。

26

左方向に戻しました。

27

長野県飯田工業高校 竹内浩一

========<コラム>=====================

もう一組・・・ジャイロ1がうまく動いているのであれば、ジャイロ2用のプログラムを付加するのはそれほど難しくはないです。関数化すればさらに簡単になると思うので、各自工夫してください。

ジャイロ2・・・LCDに対して左右の回転を検知します。


<66>mbed入門 〜mbedでジャイロとサーボを動かそう 課題7 モーショントラッキングシステムへの発展 〜

2つのジャイロ出力で2つのサーボで制御してモーショントラッキングシステムを作成します。

■課題7

2つのジャイロと2つのサーボを利用してMTRS=モーショントラッキングシステムを作成しましょう。モーショントラッキングシステムは頭に2つのジャイロを取り付けて首の縦横の回転を検出し、接続したサーボを頭の動きと同じ方向に動かすシステムです。頭を動かす動作をサーボを使って再現します。頭以外に装着することも可能です。

■mbed回路図

サーボはp21とp22に接続します。

サーボ1・・・p21
サーボ2・・・p22

29

■mbedプログラミング

・サーボ2はp22に接続します。

PwmOut servo2(p22);

・右、左の感度が違うために、同じように左右に動かしているのに違和感が感じられるときは、次の部分で補正します。ジャイロ毎に左右の感度を補正します。

・・ジャイロ1補正

  if(gy1_wk1 > 0){
    gy1_wk1=gy1_wk1*1.07;// 右回転補正
  }
  if(gy1_wk1 < 0){
    gy1_wk1=gy1_wk1*1.0;// 左回転補正
  }

・・ジャイロ2補正

 if(gy2_wk1 > 0){
  gy2_wk1=gy2_wk1*1.0;// 右回転補正
 }
 if(gy2_wk1 < 0){
  gy2_wk1=gy2_wk1*1.1;// 左回転補正
 }

■■次のURLでPublishしています。

http://mbed.org/users/takeuchi/programs/2bk0819_Gyro_test07/lxsreg

■■mbedソースリスト

// Gyro test07
// Akidukidensi AE-GYRO-SMD
// with 2Gyro+2Servo
#include "mbed.h"
#include "TextLCD0420.h"

#define ON 1
#define OFF 0
#define Gy1_offset 0.01

DigitalOut mled0(LED1);
DigitalOut mled1(LED2);
AnalogIn gy1_adc(p16);
AnalogIn gy2_adc(p17);
PwmOut servo1(p21);
PwmOut servo2(p22);

TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3

double round3(double x){
  double y;
  y=double(int((x+0.005)*100))/100;
  return(y);
}

int main() {
  float gy1_center;
  float gy1_sgm=0;
  float gy1_wk0=0,gy1_wk1=0;
  float gy1_avg=0,gy1_sum;
  int i,gy1_lc=9,gy1_lc_old=0;
  float pwidth1;
  float gy2_center;
  float gy2_sgm=0;
  float gy2_wk0=0,gy2_wk1=0;
  float gy2_avg=0,gy2_sum;
  int gy2_lc=9,gy2_lc_old=0;
  float pwidth2;
  
  lcd.cls();
  lcd.locate(0,0);
  lcd.printf("*** Gyro test07 ****\n");
  servo1.period_ms(20);
  servo2.period_ms(20);

  gy1_center=0;
  gy2_center=0;
  for(i=0;i<300;i++){
    gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
    gy2_center=gy2_center+round3(gy2_adc.read()*3.3);
    wait(0.001);
  }
  gy1_center=round3(gy1_center/300);
  gy2_center=round3(gy2_center/300);
  servo1.pulsewidth(0.0015);
  servo2.pulsewidth(0.0015);
  
  while(1){
    gy1_sum=0;
    gy2_sum=0;
    for(i=0;i<20;i++){
      gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
      gy2_sum=gy2_sum+round3(gy2_adc.read()*3.3);
      wait(0.005);
    }
    gy1_avg=round3(gy1_sum/20);
    gy2_avg=round3(gy2_sum/20);
    
    gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);
    gy2_wk1=round3((gy2_avg-gy2_center)*1000/5);
    if(gy1_wk1 > 0){
      gy1_wk1=gy1_wk1*1.07;// migi hosei
    }
    if(gy1_wk1 < 0){
      gy1_wk1=gy1_wk1*1.0;//hidari hosei
    }
    
    if(gy2_wk1 > 0){
      gy2_wk1=gy2_wk1*1.0;// migi hosei
    }
    if(gy2_wk1 < 0){
      gy2_wk1=gy2_wk1*1.1;//hidari hosei
    }
    
    gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
    gy2_sgm=round3(gy2_sgm+(gy2_wk0+gy2_wk1)*0.1/2);
    gy1_wk0=gy1_wk1;
    gy2_wk0=gy2_wk1;
    
    if(gy1_sgm > 90){
      gy1_sgm=90;  
    }
    else if(gy1_sgm < -90){
      gy1_sgm=-90;
    }
    
    if(gy2_sgm > 90){
      gy2_sgm=90;  
    }
    else if(gy2_sgm < -90){
      gy2_sgm=-90;
    }

    pwidth1=-gy1_sgm;
    pwidth1=(pwidth1+90)/180/1000+0.001;
    gy1_lc=(gy1_sgm-5)/10+9;
    if(gy1_lc >20){
      gy1_lc=19;
    }
    if(gy1_lc <0){
      gy1_lc=0;
    }
    
    pwidth2=gy2_sgm;
    pwidth2=(pwidth2+90)/180/1000+0.001;
    gy2_lc=(gy2_sgm-5)/10+9;
    if(gy2_lc >20){
      gy2_lc=19;
    }
    if(gy2_lc <0){
      gy2_lc=0;
    }
    
    lcd.locate(0,1);
    //lcd.printf("%3.2f %3.2f %2.0f %2.1f",gy1_avg,gy1_center,gy1_sgm,pwidth*1000);
    //lcd.locate(0,2);
    lcd.printf("--------+--------");
    lcd.locate(gy1_lc_old,2);
    lcd.printf(" ");
    lcd.locate(gy1_lc,2);
    lcd.printf("*");
    gy1_lc_old=gy1_lc;
    servo1.pulsewidth(pwidth1);
    lcd.locate(gy2_lc_old,3);
    lcd.printf(" ");
    lcd.locate(gy2_lc,3);
    lcd.printf("*");
    gy2_lc_old=gy2_lc;
    servo2.pulsewidth(pwidth2);
  }//while    
}//main      

■完成!

2つのサーボを写真のように組み合わせますることで、上下左右の回転を実現します。下がサーボ1でジャイロ1の横回転に対応します。上がサーボ2でジャイロ2の立て回転に対応します。

30

ウルトラマンのフィギュアをサーボアームに取り付けて、回転の目安としました。ウルトラマンでなくてもかまいません。

31

ジャイロモジュールにフラットケーブルを取り付けて、延長し、自由に動かすことができるようにしています。

32

ジャイロを回転するとウルトラマンもこのように回転します。

33

Youtubeに動画を登録しました。モーショントラッキングシステムの動きをご覧ください。

http://www.youtube.com/watch?v=eMc3hsISmpQ

<object style="height: 390px; width: 640px"><param name="movie" value="http://www.youtube.com/v/eMc3hsISmpQ?version=3"><param name="allowFullScreen" value="true"><param name="allowScriptAccess" value="always"><embed src="http://www.youtube.com/v/eMc3hsISmpQ?version=3" type="application/x-shockwave-flash" allowfullscreen="true" allowScriptAccess="always" width="640" height="390"></object>

34

ラジコン飛行機などに搭載し、操縦者が首を動かすと取り付けているカメラが動くようにするといいですね。

35

長野県飯田工業高校 竹内浩一

========<コラム>=====================

モーショントラッキングシステム・・・ジャイロモジュールを頭に取り付け、サーボにはカメラを取り付けます。頭を動かす通りにカメラが動くので、ラジコン飛行機やロボットに搭載すれば頭を動かすとカメラをそちらに向けることができます。操縦者の意思をダイレクトに反映することが可能となり、操縦しやすくなります。

補正・・・ジャイロやサーボには個体差があります。少しでも違和感を取り除くために補正が必要です。