忍者ブログ
たまにロボットを考えるブログ・・・。
ジャイロを買って、サーボも買って、mbedもかって・・・

シリアル用の回路つけてなんにもしてない日々が流れていたので、ちょっとあそびました。

◆まずはじめに、近藤の半2重通信をmbedにどうつなぐかです。
回路図はこんな感じ
mbedからの信号を5Vにしつつ半2重通信をします。
なぜそういうことをしようと思ったのか、それはもう思い出せません。

 
↑は赤がサーボのピン、緑がマイコンのRXD、青がマイコンのTXDそれぞれの電圧です。
マイコンから出た信号がマイコンに入っていますが、まー読まないから関係ないと思います??
ほんとかなぁ。

◆ほんでもって、ICS3.5をさーぼとやるので、シリアルをはいてやる必要があります。
コマンド方式サーボKRS-6003HVの制御 その1
こちらのプログラムを参考にさせていただきました。

また、本家のリファレンスも参考にしました。

ざっくり、こんな感じ
Serial ko(p9,p10);//txrx

void setServoDeg(float deg)//+-135deg
{
    deg=deg+135;
    if(deg>270)deg=270;
    if(deg<0)deg=0;
    targetPosi = 3500 + (int)((deg*800)/27);
}

//↑3500~11500で0~270°動きます。

float getServoDeg(void)
{
    return (((float)(feedbackPosi-3500)*27)/800)-135;
}

/*----------------------------------*/
/* KONDOと角度                      */
/*----------------------------------*/
int set_posi(char id, int pos)
{
  unsigned char tx[3];
  unsigned char rx[6];
  int i;
  int data=0;
 
  tx[0] = 0x80 | id;
  tx[1] = (unsigned char)( (pos >> 7) & 0x7f);
  tx[2] = (unsigned char)(pos &0x7f);
 
  for(i = 0; i<3; i++){
    ko.putc(tx[i]);
  }
  //wait(16);
  for(i = 0; i<6; i++){
    rx[i] = ko.getc();
  }
 
  data = (int)(rx[4] & 0x7f);
  data = (data << 7) + (int)(rx[5] & 0x7f);
 
  return data;
}

int main(){

ko.baud(115200);
ko.format(8, Serial::Even, 1);

setServoDeg(もくひょうかくど);
feedbackPosi=set_posi(1, targetPosi);

↑それっぽいところだけ抜粋。
変数の宣言とか足りないと思う。

◆動かすだけじゃつまらないので、ジャイロとコラボしました。


カメラのジンバルっていうか、そんな感じの動画。
ジャイロの角速度"積分値"を、角度として入力すると、
回路の動いた角度だけ、サーボは逆転動作します。
つまり動いてないかのよう。
今回は5msごとにジャイロ取得、サーボに指令してます。
積分は何にも考えないで、
Σ(現在のジャイロ値×0.005)
です。



ジャイロは角速度を電圧にするんですけれでも、
ドリフトっていって動いてない時の電圧がだんだん増えていく弱点があります。

そこで、ハイパスフィルターとか淹れてみようって準備しました。
けど、動画の通りそこまで必要じゃなかったかっていう。

◆ハイパスフィルター
ゆっくりした変化を無視する方法がハイパスフィルターです。

こちらのプログラムを参考にしました。
簡単なデジタルフィルタの実装

ざっくりこんな感じで真似させていただきました。
// それぞれの変数は下記のとおりとする
// float samplerate … サンプリング周波数
// float freq … カットオフ周波数
// float q    … フィルタのQ値
float omega,alpha,a0,a1,a2,b0,b1,b2;
float freq=.01;
float q=0.7071;
float samplerate=200;
/*----------------------------------*/
/* ハイパスフィルターセット            */
/* Data入力                         */
/*----------------------------------*/
void setHPF(void)

omega = 2.0 * 3.14159 *  freq/samplerate;
alpha = sin(omega) / (2.0* q);
 
a0 =   1.0 + alpha;
a1 =  -2.0 * cos(omega);
a2 =   1.0 - alpha;
b0 =  (1.0 + cos(omega)) / 2.0;
b1 = -(1.0 + cos(omega));
b2 =  (1.0 + cos(omega)) / 2.0;
}
float in1=0,in2=0,out1=0,out2=0;
/*----------------------------------*/
/* ハイパスフィルタ出力            */
/* Data入力                         */
/*----------------------------------*/
float outHPF(float input)
{
// それぞれの変数は下記のとおりとする
//  float input[]  …入力信号の格納されたバッファ。
//  flaot output[] …フィルタ処理した値を書き出す出力信号のバッファ。
//  int   size     …入力信号・出力信号のバッファのサイズ。
//  float in1, in2, out1, out2  …フィルタ計算用のバッファ変数。初期値は0。
//  float a0, a1, a2, b0, b1, b2 …フィルタの係数。 別途算出する。
// 入力信号にフィルタを適用し、出力信号として書き出す。
float output;
output = b0/a0 * input + b1/a0 * in1  + b2/a0 * in2
                             - a1/a0 * out1 - a2/a0 * out2;
 
in2  = in1;       // 2つ前の入力信号を更新
in1  = input;  // 1つ前の入力信号を更新
 
out2 = out1;      // 2つ前の出力信号を更新
out1 = output; // 1つ前の出力信号を更新

return out1;
}

ハイパスフィルターですから動画見たいに、動きが無くなる=高周波成分は0になろうとします。

普通はローパス分を別のセンサーとかを入れて相補するっぽいですが難しいことは・・・。


◆◆◆想像以上に積分値安定してるんで感動した。



拍手[0回]

PR
BlueUSBの続編です。

BlueUSBのUSB通信のところは難解なので、必要な部分はブラックボックスのままで引き抜く方針で、その結果をインターバル割込みで読みます。

+アウトライン+
①BlueUSB+Wiiリモコンの動きを確認します。
②必要な部分だけを見つけます。
③割り込みでデータを見ます。
④自分用にまとめてみる。

①いつもの手法で行きます。
プログラムのいろんなところにprintfをばらまいて動きを見ます。
深さ優先探索です。

BlueUSBの中にmain.cppがあり、
main関数は
TestShell();で終わっています。

なので、その先に行きます。

TestShell.cppの中にTestShell();があります。
その中には
    USBInit();

    gApp.Run();
があります。


でまず、
    USBInit();
 printf("A\n");
    gApp.Run();
   printf("B\n");

見たいにすると、Bが出力されないことが分かります。
なのでgApp.Run();に肝があると思います。

で、TestShell.cppの中に

void Run()があって、
ForとかIfとかがあるので↓の様にします。

   void Run()
    {
        printf("C\n");
        for(;;)
        {
            printf("D\n");
            const char* cmd = ReadLine();
            if (strcmp(cmd,"scan") == 0 || strcmp(cmd,"inquiry") == 0)
            {    printf("E\n");
                Inquiry();}
            else if (strcmp(cmd,"ls") == 0)
             {   printf("F\n");
  ・・・・・・

こうすると、Dまで出力されることが分かります。
なので、肝はReadLine();の中にあると思います。

同様にReadLine();は↓のようにします。
    const char* ReadLine()
    {
        printf("L\n");
        int i;
        for (i = 0; i < 255; )
        {
            printf("M%2d\n",i);
            USBLoop();
            printf("O\n");
            int c = GetConsoleChar();
            if (c == -1)
                continue;
            if (c == '\n' || c == 13)
                break;
            _line[i++] = c;
        }
        _line[i] = 0;
        return _line;
    }

USBLoopが怪しいです。しかもMに続く数字はずっと'0'でした。
つまりUSBLoopになんどもなんども入っています。
で、USBHost.cppの中にVoid Loop()があります。
同様に下のようにします。
    void Loop()
    {
        printf("P\n");
        u16 elapsed = CommunicationArea.FrameNumber - (u16)_frameNumber;    // extend to 32 bits
        _frameNumber += elapsed;

        // Do callbacks, if any
        while (_callbacksPending)
        {
            printf("Q\n");
            for (int i = 0; i < MAX_ENDPOINTS_TOTAL; i++)
            {
                printf("R”);
                Endpoint* endpoint = Endpoints + i;
                if (endpoint->CurrentState == Endpoint::CallbackPending)
・・・・・

してみると、初期設定的なWiiリモコンとペアリングっぽいことをやっている間はPとQとRが出てきます。
なんか知りませんがPの次がRだったりして良くわかりません。(すべてをTeraterm出来ているわけではない?)

で、Wiiとの通信が確立するとRがいっぱいとQになります。
たぶんwhileから抜け出していません。










②以上のことを踏まえると、

    USBInit();のあとに、いろいろすっとばして
    USBLoop();が回っていればいいと思います。

ですので、
void TestShell()
{
    USBInit();
    while(1){
    printf("AA\n");
    USBLoop();
    printf("AB\n");
    }
・・・・・
としてみました。
いまのところ問題は見つかっていません。








③割り込みと同時にいろいろ起きて大丈夫か?にチャレンジします。
main.cppに書き加えます
Lチカと割り込み時に時間をカウントしてみましょう。
void TestShell();
/***/
Ticker intvTimer;       // インターバルタイマ
DigitalOut Led2(LED2);  // LED2
extern int32_t TimeMs=0;
void isrInterval(void) {
    Led2=(TimeMs/50)%2;
    TimeMs+=5;
}
/***/

int main()
{
            // インターバルタイマ 割込み設定、タイマスタート       
    intvTimer.attach_us(isrInterval, 5000);  // 500ms
    pc.baud(9600);
    printf("BlueUSB\nNow get a bunch of usb or bluetooth things and plug them in\n");
    TestShell();
}
で、それに加えLoop内で時間を確認します。
    void Loop()
    {
        printf("P\n");
        u16 elapsed = CommunicationArea.FrameNumber - (u16)_frameNumber;    // extend to 32 bits
        _frameNumber += elapsed;

        // Do callbacks, if any
        while (_callbacksPending)
        {
            printf("Q\n");
            for (int i = 0; i < MAX_ENDPOINTS_TOTAL; i++)
            {
                printf("R%d\n",TimeMs);
                Endpoint* endpoint = Endpoints + i;
                if (endpoint->CurrentState == Endpoint::CallbackPending)
                {
注意)extern int32_t TimeMs;をUSBHost.cppの上の方に書き加えてます。
複数のファイルにまたがるので、(あまりexternの使い方わかってない)
で出力は以下の用になりました。
BlueUSB
Now get a bunch of usb or bluetooth things and plug them in
USB INIT (Controller is 1396 bytes)
AA
P
HubStatusChange Hub:0 Port:1 00010101
ResetPort Hub:0 Port:1
AB
AA
P
AB
AA
P
HubStatusChange Hub:0 Port:1 00100103
AB
AA
P
AB
がはじめのほう。
で通信がはじまると
================wii====================
P
P
0000 A1 30 00 03                                     .0..
R16110
R16120
R16125
R16135
R16140
R16150
R16155
R16165
R16170
R16175
R16185
R16190
Q
R16200
R16210
R16215
R16225
OnHidControl
0000 00                                              .
R16300
R16310
R16315
R16325
R16330
R16340
R16345
R16350
R16360
R16365
R16375
R16380
Q
R16390
R16400
R16405
R16415
OnHidControl
0000 00                                              .
R16490
R16500
R16505
R16515
R16520
R16525
R16535
R16540
R16550
R16555
R16565
R16570
Q
R16580
R16590
R16595
R16605
WII 0300 529 546 650
R16630
R16640
R16645
R16655
R16660
R16670
R16675
R16685
R16690
R16695
R16705
R16710
Q
R16720
R16730
R16735
R16745
WII 0300 529 546 650

という感じになります。
通信が始まるのに16秒ぐらいかかってますね。
Teratermにはいてる時間が一番ながいんでしょうけど。




④自分用にまとめるとは?
この後サーボ動かしたり、加速度を読んだりしたいのでBluetoothの通信は簡素にしてしまいたいし、通信と同じループ内でサーボを動かしたりしたいので、汎用ループを作ってしまいたい所存です。

Try1.
割り込み内で、通信以外を全部やる。
→結果無理。
無理だった作戦。
・Wiiリモコン値をグローバル変数に格納
・インターバル割り込みでprintfしてteratermする。
結果:
teratermするとBluetoothとの通信が終わっちゃう。
割り込みはしてもいいけどシリアル通信は無理っぽい。
時間的になのか、リソースの使い方的なものなのかは不明。(今回はどうでもいい)
もしかすると確率的に、軽い割り込みであったとしても、Bluetoothが止まるかも知れない。
(遊びなのでどうでもいい。)

Try2.
USBLoopから抜けているので、汎用ループ内にUSBLoopを置く。
結果:
今のところOk。

USBHost.cppは何もしない
TestShell.cppは↓
                case 0x37: // Accelerometer http://wiki.wiimoteproject.com/Reports
                {
                    int pad = (d[2] & 0x9F) | ((d[3] & 0x9F) << 8);
                    int x = (d[2] & 0x60) >> 5 | d[4] << 2;
                    int y = (d[3] & 0x20) >> 4 | d[5] << 2;
                    int z = (d[3] & 0x40) >> 5 | d[6] << 2;
                    //printf("WII %04X %d %d %d\n",pad,x,y,z);
                    m_pad=pad;
                    m_x = x;
                    m_y = y;
                    m_z = z;
                    wii_f=1;
                }
                break;
各値をグローバルに入れる&Wiiリモコンとの通信出来てますフラグを立てる。
main.cppは↓
int main()
{
            // インターバルタイマ 割込み設定、タイマスタート       
    intvTimer.attach_us(isrInterval, 5000);  // 500ms
    pc.baud(9600);
    printf("BlueUSB\nNow get a bunch of usb or bluetooth things and plug them in\n");
    //TestShell();
    USBInit();
    while(1){
    USBLoop();
        if(wii_f)
        {
        printf("main %04X %d %d %d %d\n",m_pad,m_x,m_y,m_z,TimeMs);
        }
    }
}

で出力は、
BlueUSB
Now get a bunch of usb or bluetooth things and plug them in
USB INIT (Controller is 1396 bytes)
HubStatusChange Hub:0 Port:1 00010101
ResetPort Hub:0 Port:1
HubStatusChange Hub:0 Port:1 00100103
Connect Hub:0 Port:1 full
と始まって、
Wiiリモコンと通信すると
================wii====================
0000 A1 30 00 03                                     .0..
OnHidControl
0000 00                                              .
OnHidControl
0000 00                                              .
main 0300 529 546 650 14760
main 0300 528 546 650 14785
main 0300 528 546 650 14815
main 0300 528 546 650 14845
こんな感じになります。

さしあてってWii_fのif内でいろいろはできるんじゃないでしょうか。
終わり。

拍手[0回]

最後にmbedの記事を書いたのは半年以上前になってますから、何もしてなかったんですねきっと。

この備忘録は、mbed、Wiiリモコン、Bluetoothドングルを買って遊び始めるときのお話です。

いろいろなHPを見て下の回路でいんじゃねとなりました。
数多のHPを書いてくれた人ありがとう。

レギュレータで5Vとります。
USBのバスパワーとレギュレータがぶつかるかもしれないので、ドングルのところはDが入ってます。mbedのボードにはそもそも入ってます。

ちな、加速度センサつきです。

PWMサーボ4つピンを付けます。

USBのドングルは秋月の基板がついてるメスAでつけます。
ドングルはグリーンハウスのGH-BHDA42です。

Wiiリモコンは黒いです。

以上、ハードの下ごしらえ終わり。



ソフトは、
mbedの開発環境にログインして、
BlueUSBをプログラムでインポートします。
自分のところでコンパイルします。
mbedをつなぎます。
mbed内に保存します。
terateamを受信LFで408000みたいなそういう雰囲気のボーレートにします。
mbedをリセットします。なんか文字が出てきます。
Wiiリモコンを準備します。
電源を入れて1と2を推しっぱなしにします。
すかさずmbedをリセットします。
文字が出てきます。
待ちます。
文字が出てきます。
待ちます。
Wii 0000 0000  みたいなのが流れ始めます。

終わり。

拍手[0回]

3Dプリンタがたのしかったもんで、長いことmbedを放置していました。
メカの方が性に合っているんでしょう。

さて、これまでにmbedではprintf、mpu-6050の出力値の確認をやってきました。

注しあたっての目的は:
・角度を得る
・角度の精度
・角度の遅れ

もっと先の話としては、ロボット内で姿勢角度フィードバックをすることにあります。

mpu-6050の出力値から算定します。

算定には出力値を時系列で得る必要があり、かつ遅れや、ロボットへの実装を考慮するには数分から数十分の値を得たいです。

その後表計算ソフトなりなんなりで、角度を算定してみるなり、フィルタを調整するなりということがしたいとおもってます。



なので


たくさんのデータを保存しなければなりません。

そこでSDカードに保存という流れです。

いくつかのサイト様を参考にしました。


・mbedで初めてのマイコン開発 メモリ・カードを使ったデータの読み書き<1/3>
http://www.eleki-jack.com/arm/2010/12/mbed-5.html
使うライブラリ、関数を簡単に網羅的に紹介してます。

・ARM SDカード操作
http://akikanlab.blog.fc2.com/blog-entry-54.html
SDカードスロットは秋月でこちらと同様のものを使用しました。

もっとも簡単にmbedでSDカードにR/WするにはSDFileSystem_HelloWorldのプログラムをDLです。


こういうfopenの記述って(fopenに限らず、boolの使い方とかも)、普通にサンプルに使われてますけどよく考えられていて、感銘を受けます。自分で思いつけません。
----
    // ファイルを操作する場合は、ファイル・ポインタの宣言をする。
    FILE *fp;
    if ( (fp = fopen("/sd/mpu6050.txt","a")) == NULL ) {
        printf("Open Failed. \n") ;
        return -1;
    }
----

ちなみに、参考がてら出力結果を以下に:
系列1から
加速度x,y,z、角速度x,y,z、角速度のニュートラル値のとの差を積分x,y,z
の9要素です。
スケールは大体何倍かしています。
また重ならないように足し算して上方向に平行移動しています。
横軸はなんだか忘れました・・・時間じゃなくて出力回数だったような・・・。
単純に積分するだけでも角度っぽいものになっていますね。


といってSDカードに入れることもできました。
しかし、これで積分が上手くてきているか確認できますか?
これは手で動かしただけです。
角度値がどんなふうになっていれば、確からしい値と言えますか?

というわけで、次回はサーボモータで決まった角度だけ動かしたときに測定してみます。

拍手[0回]

引き続き、mbedとmpu-6050のコンビと戯れています。

今回は
連続してmpu6050からデータ取得およびターミナル出力を繰り返していると一定時間経過して止まってしまう問題に取り組みました。
一応の解決をしました。

■実験向けにマイコンとセンサを固定

とりあえず箱型にして、90度ずつの姿勢をとれるようにした図。

■問題の起きている状況
mpu-6050_testのプロジェクトを展開して実験を進めています。

whileループ内で
----
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
----

----
pc.printf("t %d,ax %.3f;ay %.3f;az %.3f;gx %.3f;gy %.3f;gz%.3f,\n\r"
        ,TimeMs,(float)(ax)/16384,(float)(ay)/16384,(float)(az)/16384
        ,(float)(gx)/131,(float)(gy)/131,(float)(gz)/131);
----
をただただ回すだけです。

これをやると、1130回出力すると止まります。
何が止まっているのか分かりません。PCとの通信が途切れているかもしれません。
しかし、getMotion6を2文重ねると565回で止まります。

つまり、mpu6050との通信の回数に限界があるような気がします。
よくあるパターンとして、何かのカウンタとかがオーバーフローしてしまうとかが考えられます。
ハードウェア的な問題であれば毎回同じ回数で止まるっていうのは考えにくいです。

■問題のあるところ
getMotion6を実行することに問題があるのでそれの中身を、mpu6050.cpp→i2cdev.cppという様に順を追って確認します。
そこでなにか回数を重ねたら変化しそうなものは無いかを探ります。

mpu6050.cpp内では
----
void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
    i2Cdev.readBytesM6_2(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
    *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
    *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
    *az = (((int16_t)buffer[4]) << 8) | buffer[5];
    *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
    *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
    *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
}
----
としか書かれていません。
i2cdev.cppでは
----
int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
{
    char command[1];
    command[0] = regAddr;
    char *redData = (char*)malloc(length);
    i2c.write(devAddr<<1, command, 1, true);
    i2c.read(devAddr<<1, redData, length);
    for(int i =0; i < length; i++) {
        data[i] = redData[i];
    }
    return length;
}
----
と書かれています。

グローバル変数とかは利用されていません。

ここで一番臭いのはmallocです。
動的なメモリなんとかだったと思いますが(プログラムとかにわかだしわからん。)
あらかじめ決められた数の配列とかではなくて、状況におおじて使うメモリの大きさを変更するためにプログラム内でメモリを確保する関数です。

lengthの大きさを使ってますね。
mpu6050.cppでは14を入れています。

まーつまり、センサとかから帰ってくる値の大きさによって受け取る数を調整しているんだと思われます。

ちなみにmallocは確保したメモリをずっと確保しているので、毎回使うと次々にメモリを使っていきます。(大学3年の時にハマった経験があったので…しっている)
なので確保した奴を要らなくなったら開放してやる必要があります。
そこらへんはmallocでググればわかると思います。

■解決策
にわかな僕はいくつか解決策を考えました。

・getMotion6専用の関数としてredData[14]を書いちゃう
----
int8_t I2Cdev::readBytesM6(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
{
    char command[1];
    command[0] = regAddr;
    char redData[14];
    i2c.write(devAddr<<1, command, 1, true);
    i2c.read(devAddr<<1, redData, length);
    for(int i =0; i < length; i++) {
        data[i] = redData[i];
    }
    return length;
}
----

・mallocを使わない
----
int8_t I2Cdev::readBytesM6_(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
{
    char command[1];
    command[0] = regAddr;
    char redData[length];
    i2c.write(devAddr<<1, command, 1, true);
    i2c.read(devAddr<<1, redData, length);
    for(int i =0; i < length; i++) {
        data[i] = redData[i];
    }
    return length;
}
----

・freeをつかって開放する
----
int8_t I2Cdev::readBytesM6_2(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
{
    char command[1];
    command[0] = regAddr;
    char *redData = (char*)malloc(length);
    i2c.write(devAddr<<1, command, 1, true);
    i2c.read(devAddr<<1, redData, length);
    for(int i =0; i < length; i++) {
        data[i] = redData[i];
    }
    free(redData);
    return length;
}
----

どれもすべて上手くいきました。
ではなぜ、そもそもmallocだったのか・・・わかりません。
重要な何かを見逃しているかもしれませんし、
にわかにわからないようなスマートなやり方があるのかもしれません。
分かる人は教えてください。


■オマケ
タイマー割り込みを使う。
こちらのサイト様をそのまま使わせていただきます(http://www.wsnak.com/wsnakblog/?p=3449)

1秒Lチカを腕時計で確認しました。細かい誤差とかは分かりません。

----
// インターバルタイマ 割込みハンドラ
void isrInterval(void) {
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    TimeMs++; 
}
----
TimeMsとかグローバルで宣言して時間をはかるのをいつもやります。
そんなものはそもそも用意されているのだと思いますが・・・。


といって割り込をつかったデータを取ってエクセルで表示してみてフィルタのちょうしとかためす予定だったんですが・・・
explore落ちたのでもうやる気ないです。(よくブログ更新した、おれえらい。)



拍手[1回]

mpu-6050が結構な話題になってるのに、全然遊べてなかったのです。
それのついででmbedを始めたといっても過言ではないのです。

Amazonでたたき売りのmpu-6050をかって、mbedとつなぎます。
今回必要だったハードは、つなぐ線4本です。
こちらのサイト様を参考(http://blog.goo.ne.jp/roboz80/e/593cf982f83fc241ffce380df78382c7)

同様に
プロジェクトをインポートします。(https://mbed.org/users/garfieldsg/code/mpu6050_test/)

既に6軸をprintfするプログラムがmain.cppになっているのでコンパイルしてDLするのみです。

----
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
----

↑の一文にすべてが入っていると思うと、恐ろしいやら、頼もしいやら。
引数の名前の通りの値が得られます。

各軸の向きはmpu-6050上に印刷されてますから確認します。

前回同様TeraTermをつかって見てみます。

----
ax 1272;ay -16384;az 652;gx -127;gy -29;gz65,
ax 1236;ay -16400;az 440;gx -183;gy 53;gz68,
ax 1200;ay -16372;az 684;gx -167;gy 5;gz78,
ax 1244;ay -16424;az 604;gx -230;gy 6;gz57,
ax 1244;ay -16492;az 424;gx -126;gy 63;gz62,
ax 1292;ay -16324;az 516;gx -207;gy -50;gz45,
ax 1220;ay -16372;az 328;gx -153;gy 66;gz60,
ax 1188;ay -16440;az 540;gx -229;gy -12;gz35,
ax 1236;ay -16352;az 516;gx -114;gy 13;gz105,
ax 1280;ay -16388;az 456;gx -267;gy 44;gz21,
ax 1204;ay -16456;az 480;gx -135;gy 54;gz66,
ax 1212;ay -16332;az 540;gx -233;gy -6;gz19,
ax 1140;ay -16364;az 580;gx -134;gy 64;gz75,
ax 1132;ay -16312;az 692;gx -219;gy 17;gz59,
ax 1120;ay -16364;az 616;gx -147;gy -29;gz69,
ax 1168;ay -16292;az 492;gx -194;gy -20;gz64,
ax 1296;ay -16428;az 584;gx -174;gy 80;gz77,
ax 1196;ay -16384;az 460;gx -188;gy -24;gz47,
ax 1208;ay -16340;az 680;gx -200;gy 80;gz70,
ax 1216;ay -16300;az 600;gx -187;gy 2;gz70,
ax 1216;ay -16312;az 536;gx -244;gy 46;gz33,
ax 1228;ay -16376;az 428;gx -158;gy -15;gz63,
ax 1204;ay -16420;az 532;gx -199;gy 43;gz53,
ax 1264;ay -16380;az 472;gx -208;gy 16;gz16,
ax 1268;ay -16428;az 500;gx -196;gy 0;gz65,
ax 1224;ay -16456;az 544;gx -182;gy 55;gz43,
ax 1276;ay -16304;az 696;gx -183;gy -57;gz70,
----
↑の様に出力が見られます。(ax、ayとかは前述にプログラムに書きくわえています。)

さてこれの単位換算が必要です。

mpu6050.cppなるファイルに各換算係数があります。
しかし、mpu-6050のリビジョンによって異なるようです。
私の場合は異なっていました。

また、スケールを4種類から設定できるのでその確認もした方がいいかもしれません。

私の場合(リビジョンとプログラムの組合せ)ではデフォルトで

加速度
+/- 2g →16384 LSB/mg

角速度
+/- 250 degrees/s →131 LSB/deg/s

になっていました。
加速度はmpu6050.cppにコメントされている者とことなり困惑しました。

以下のデータシートを確認してわかりました。(正しくはこれがあっているかどうかではなく、これの通りにしたら正しかったというだけです。)
(http://invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf)

加速度については合っているかどうか重力の向き大きさが明らかなので簡単ですが、角速度の方はというと、人間が素手で等速回転をさせることは不可能なので無理です。
なにか台形加速をさせるような円運動を出力してくれるようなサーボモータを用意して、後日実験しようと思います。

換算後は
----
fa 0,fg 0,ax 0.008;ay -0.990;az 0.198;gx -2.504;gy 0.084;gz0.191,
fa 0,fg 0,ax 0.014;ay -1.008;az 0.210;gx -0.931;gy 0.534;gz0.672,
fa 0,fg 0,ax 0.009;ay -0.989;az 0.192;gx -1.748;gy 0.885;gz0.305,
fa 0,fg 0,ax 0.009;ay -0.999;az 0.215;gx -1.466;gy 0.023;gz0.374,
fa 0,fg 0,ax 0.010;ay -0.994;az 0.198;gx -1.870;gy -0.168;gz0.267,
fa 0,fg 0,ax 0.008;ay -0.992;az 0.204;gx -1.252;gy 0.466;gz0.664,
fa 0,fg 0,ax 0.010;ay -1.002;az 0.213;gx -1.679;gy -0.481;gz0.000,
fa 0,fg 0,ax 0.011;ay -0.998;az 0.203;gx -1.160;gy 0.466;gz0.573,
fa 0,fg 0,ax 0.011;ay -0.999;az 0.204;gx -1.481;gy -0.023;gz0.191,
fa 0,fg 0,ax 0.006;ay -0.992;az 0.209;gx -1.580;gy -0.008;gz0.298,
fa 0,fg 0,ax 0.005;ay -0.998;az 0.206;gx -1.214;gy 0.198;gz0.359,
fa 0,fg 0,ax 0.005;ay -0.996;az 0.199;gx -1.504;gy 0.137;gz0.137,
fa 0,fg 0,ax 0.009;ay -0.998;az 0.206;gx -1.260;gy -0.145;gz0.313,
fa 0,fg 0,ax 0.008;ay -0.997;az 0.211;gx -1.344;gy 0.275;gz0.519,
fa 0,fg 0,ax 0.008;ay -0.991;az 0.205;gx -1.473;gy -0.221;gz0.382,
fa 0,fg 0,ax 0.007;ay -0.991;az 0.204;gx -1.725;gy -0.015;gz0.412,
fa 0,fg 0,ax 0.009;ay -0.996;az 0.207;gx -1.443;gy 0.145;gz0.427,
fa 0,fg 0,ax 0.012;ay -1.000;az 0.205;gx -1.420;gy 0.130;gz0.382,
----
上の様な出力がされます。
fa,fgは付け加えました。これは、加速度角速度のスケールの設定ビットを読みだしたものです。
↓の関数で得ることができます。またget→setとすれば変更できます。どのように変更するかはmpu6050.cppを見ます。

mpu.getFullScaleAccelRange();
mpu.getFullScaleGyroRange();

というわけで、データの読み出しに関しては容易にできました。
というかなんにもしてません。線を4本作っただけです。

今のところ加速度と角速度しか得られていませんので、今後角度を得る手法について攻めていきます。

拍手[1回]

社会人のゆとりある開発を始めるため・・・mbedを使っていこうと思います。
なんでmbed?
探すのもゆとりの邪魔になるので、聞いたことあるやつ・・・情報が多い奴・・・ってなるとarduino or mbedという感じだったのでクロックの速いほうを選んだだけです。

で、まずはPC側の準備
mbedつないで見つかったフォルダの中身を開いて、HPからアカウントsignupするだけです。
くわしく説明されているサイト様はたくさんありますから備忘する必要は無いですね。

エディタを開いてhelloWorldというLチカプログラムをコンパイル→.binをmbedに保存

mbedの真ん中にあるスイッチを推します。

チカチカします。

次に、今後の開発の中でデバッグの必要があります。
そのためにTeraTermなどでの表示は必須です。

で、シリアル通信したいので(PcとはUSB接続されている)仮想シリアルポートとして認識させないといけません。
そのソフトは
http://mbed.org/handbook/Windows-serial-configuration
にあります。DLしてインストールします。
mbedをPCにつないだままやります。
おわったらデバイスマネージャでcomポートを確認しておきます。

<ポート>
--COMxx
--COMxx


--mbed Serial Port(COMxx)
って感じでok!
参考サイト(http://www.wsnak.com/wsnakblog/?p=3502)
参考サイト(http://d.hatena.ne.jp/nakamura001/20101129/1291053275)

その後プログラムを書きます。
(このエディタは全角入力するとバグる、main以外のファイルを開いてみたりすると回復)
----ソース
#include "mbed.h"

DigitalOut myled0(LED1);
DigitalOut myled1(LED2);
DigitalOut myled2(LED3);
DigitalOut myled3(LED4);


int main() {
    int a=0;
    while(1) {
        myled0 = a&0x01;
        myled1 = (a&0x02)>>1;
        myled2 = (a&0x04)>>2;
        myled3 = (a&0x08)>>3;
        wait(0.2);
        if(++a==16)a=0;
        printf("a = %5d\n\r",a);
    }
}
----
LEDがmbedに4つついてますので、2進数っぽくちかちかするプログラムで
4bit=16まで数えます。
今何番目を数えているかを表示するプログラム。

結果:

なんかTV見えてるけどまー仕方ない。
画面上に表示されて、LEDもついてカンペキ。

拍手[0回]

プロフィール
HN:
Adel
年齢:
27
性別:
男性
誕生日:
1989/09/17
職業:
会社員
趣味:
モチベーション探し
自己紹介:
ロボットつくるのが夢
ブログ内検索
カウンター
君は何人目かな
カレンダー
05 2017/06 07
S M T W T F S
1 2 3
4 5 6 7 8 9 10
11 12 13 14 15 16 17
18 19 20 21 22 23 24
25 26 27 28 29 30
最新コメント
[06/22 fake serpenti watch bvlgari]
[06/22 where to buy hermes clic clac bracelet faux]
[04/09 blacklab]
[02/16 オイルマン]
[09/11 M.A(CIT)]
最新トラックバック
コガネモチ
忍者AdMax
アクセス解析
忍者ブログ [PR]