元高専生のロボット作り

元高専生のロボット作り

主にプログラミング, 電子系について書きます。たまに機械系もやります。メモ代わりの記事ばっか書きます

Excelで２つの時系列データの位相を推定する

Excelで２つの時系列データの位相と周期を求めます。
位相はこちらから指定してやり、計算から指定した位相を求められるかというのを試します。

こんな感じのことをやります。
f:id:sgrsn1711:20210610152333p:plain

以下にExcelデータ置いときます。
https://docs.google.com/spreadsheets/d/e/2PACX-1vQKYRR9fKz5s8oWPWFO0jw8JrfoK3Aej_kXkqlB6FtjwwOtDlL5uH8BFwOYJGNlf5-O68wyW0_mdMEv/pub?output=xlsx



まずは、時間tを0.1秒ステップで10秒くらいまで連続データで作って、
さらに位相Φを適当に指定しておきます。

肝心のデータ２つをf(t)、g(t)として以下のように用意します。
f(t) = sin(t + RAND()*N)
g(t) = sin(t + RAND()*N - Φ)

ここでtは時間、Φは位相、RAND()は0～1までの値をランダムで返します。

データが用意できたので畳み込み積分によって位相を求めます。
畳み込み積分をF(τ)とすると、
F(τ) = ∫f(t)g(τ-t)dt
だったような気がします。

Excelでは畳み込み積分は簡単に行えます。
具体的には、f(t)のセルの範囲がB4からB104、
g(t)のセルの範囲がC4からC104だとして、

Dの列に畳み込み積分を書くならば
D4に、
=SUM(B$4:B$104*C4:C104)
と入力し、Ctrl+SHift+Enterでτ=0の畳み込み積分ができます。
あとはD4からD104まで連続データとしてやれば
τ=0～10までのステップ0.1の畳み込み積分データが取得できます。

この畳み込み積分の最大値の時のτが位相です。（詳しくは後述）

さらに畳み込み積分２波目の最大値はτ=Φ+Tの時のものなので、周期も求めることができます。

プロキシに接続するバッチファイル

学校の無線LANはプロキシの設定をいちいち変えないと接続することができません。

なので僕はバッチファイルを作ってそれを実行して接続しています。

以下がそのバッチファイルのコードです。

reg add "HKEY_CURRENT_USER\SOFTWARE\Microsoft\Windows\CurrentVersion\Internet Settings" /f /v ProxyEnable /t reg_dword /d 1
reg add "HKEY_CURRENT_USER\SOFTWARE\Microsoft\Windows\CurrentVersion\Internet Settings" /f /v ProxyServer /t reg_sz /d xxxxxxxxxxxxxxxxxxxxxxxxxxxxxx:00000
reg add "HKEY_CURRENT_USER\SOFTWARE\Microsoft\Windows\CurrentVersion\Internet Settings" /f /v ProxyOverride /t reg_sz /d "*.contoso.com;<local>
cd "C:\Program Files\Internet Explorer"
Iexplore "https://www.google.co.jp/"

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxx:00000　の部分に使ってるプロキシのアドレスとポート番号をにします。

プロキシ設定を変更した後
Iexplore "https://www.google.co.jp/"
を行うことでプロキシ設定を反映させています。

絶対もっと適した方法があるので探してください。

SparkFunのラインフォロワアレイをmbedで使う

スイッチサイエンスでラインフォロワアレイというものが売ってます。
www.switch-science.com

赤外線LEDとフォトリフレクタが8つ付いていて、I2Cで読み取れるらしいです。
ライントレースとかを手軽にやるには便利そうなので購入しました。

Arduinoライブラリはあるんですけどmbed用のものはないので作りました。
os.mbed.com


ライブラリを使用すると以下のようになります。
i2cアドレスがセンサーに書いてあるものとは記述が異なるのでそこだけ注意ですね。

#include "mbed.h"
#include "SensorBar.h"

const uint8_t SX1509_ADDRESS = 0x3E<<1;  // SX1509 I2C address (00)

int main()
{
    I2C i2c(p28, p27);
    SensorBar mySensorBar(&i2c, SX1509_ADDRESS);
    mySensorBar.setBarStrobe();
    mySensorBar.clearInvertBits();
    uint8_t returnStatus = mySensorBar.begin();
    printf("next, %d\r\n", returnStatus);
    if(returnStatus)
    {
        printf("sx1509 IC communication OK");
    }
    else
    {
        printf("i2c failed");
        wait_ms(10);
        while(1);
    }
    
    while(1)
    {
        uint8_t rawValue = mySensorBar.getRaw();

        for( int i = 7; i >= 0; i-- )
        {
            printf("%d", (rawValue >> i) & 0x01);
        }
        printf("b, ");
        printf("%d\r\n", mySensorBar.getPosition());
        wait_ms(10);
    }
}

LPC1768のVUの出力を上げる

LPC1768にはVUという5Vを出力するピンがあります。

ピン番号39ですね。
f:id:sgrsn1711:20180821145102p:plain

今回はこのVUから出力できる電流を増やします。

まず、回路図を見てみますと、
https://developer.mbed.org/media/uploads/chris/mbed-005.1.pdfより、

以下の回路がUSBの電源、VBUSより接続されています。

f:id:sgrsn1711:20180821143458p:plain

IC FPF2123が接続されており、また、電流のIlimが460 mAであることが示されています。

ここで、FPF2123のデータシート見てみます。

http://www.onsemi.jp/pub/Collateral/FPF2125JP-D.pdfの、p10にRsetと遮断電流の関係、また表が記載されています。

先ほどのLPC1768の回路図では Rset=1 kΩであったので、遮断電流 Ilimは、

Ilim=\frac{460}{Rset}

より、なるほど確かに遮断電流は460 mAです。

また、データシートより、遮断電流を上げるには、Rsetの抵抗値を下げればよいということが分かります。

今回はとりあえず、1 A程度まで遮断電流を上げたいので、Rset=460 Ω 以下にします。

既にある1kΩの抵抗に並列でつけるので、800 Ωの抵抗を付加します。

LPC1768のRsetは以下の場所にありますので、そこに800 Ωの抵抗を載せればOKです。
f:id:sgrsn1711:20180821145407j:plain

B3MサーボとAXサーボを並列で動かす

近藤のB3MサーボとDynamixelのAXサーボを並列で動かす必要があったのでやってみました。

使用したのは
・KONDO B3M-SC-1170-A
・Dynamixel AX-12A

です。

どちらも通信方式は半二重通信なのでLTC485を使用すれば使い分けることができます。B3Mは差動通信なのでD+とD-が必要ですが、AXシリーズはD+だけ接続すればよいです。


接続は以下のようにしてください。TX, RXはデバイスのTX、RXです。DEはHighなら送信モード、Lowなら受信モードになります。送信のみの使用なら+5Vに接続してもいいです。
f:id:sgrsn1711:20180817211702p:plain

実は、この接続に秋月のUSBシリアル変換モジュール(FT232)を接続すると、公式の設定用ソフトが使えてしまいます。
USBシリアル変換モジュールFT232

ソフト
・B3M Managerソフトウェア | 近藤科学(B3Mサーボ用)
・Besttechnology - Dynamixel Configurator [DXCONF] - ナレッジベース(AXサーボ用)

この場合は以下のように接続です。

f:id:sgrsn1711:20180817212655p:plain


とりあえず、mbedでプログラムを書いてみました。

ライブラリは以下です
B3MServo : B3MServo - Library for Kondo B3M Servo Motor | Mbed
AX12A : AX-12 | Mbed

多分、バージョンの関係でAX12のライブラリからエラーが出ると思うんですけど、その場合は、AX12.hの183行目の SerialHalfDuplex を消して、代わりに Serial としてください。

#include "mbed.h"
#include "AX12.h"
#include "B3MServo.h"

int main()
{
    B3MServo myServo(p13, p14, NC);
    AX12 myax12(p13, p14, 0xFE, 115200);
    myServo.onTorque(0);
    while(1)
    {
        myax12.SetGoal(0);
        myServo.controlAngle(0, 0);
        wait_ms(1000);
        
        myax12.SetGoal(90);
        myServo.controlAngle(0, 90);
        wait_ms(1000);
    }
}

mbedでJY901を使う

JY901というものを紹介します。

加速度、ジャイロ、地磁気センサが入っていて、カルマンフィルタを通して3軸それぞれの角度を出力してくれる便利なやつです。

WitMotion WT901 センサー MPU9250 モジュール 3 軸加速度計ジャイロスコープ角度磁気フィールド TTL & I2C Outout Pc/ アンドロイド/MCU - Aliexpress.com | Alibaba グループ上の 家電製品 からの スマート アクティビティー トラッカー の中

amazonでも売ってる

購入して届いたものがこれです。

f:id:sgrsn1711:20180204230214j:plain

センサ本体と、ピンヘッダ、あとUSB-TTLまでついてますね。


少し前にmbedで使用してライブラリまで書いてました。ただ、関数が数個、未確認です。。。よく使うような関数は確認済みです。

JY901 | Mbed



このライブラリを使用してyaw角を得るんだったらコードは以下の感じで

#include "mbed.h"
#include "JY901.h"

JY901 i2c(dp5, dp27);

int main()
{
    float angle = 0;
    i2c.calibrateAll(5000);
    while(1)
    {
        angle = i2c.getZaxisAngle();
        printf("%f\r\n", angle);
    }
}


tera termを使って画像の感じで値を確認できると思います。

f:id:sgrsn1711:20180204225409p:plain


おまけ
これのBluetooth版みたいなものを使用してみました。
BWT901CLをpythonで受信する - 元高専生のロボット作り

ラズベリーパイとmbedでI2C通信する

ラズパイとmbedでi2c通信してみました。

ラズパイはI2C Slaveにはなれないようなので、マスターになっています。

mbedがスレーブで受信割り込みで自動で値を受け取るようにしました。

レジスタ指定で、４バイトの送受信が可能です。

使用したものはRaspberry Pi 2 model Bと、mbedLPC1768です。

はいコードです。


ラズパイ側

import smbus

addr = 0x1D

class I2CMaster():
    
    def __init__(self):
        
        self.i2c = smbus.SMBus(1)
        
    def writeSomeData(self, addr, reg, data, size):
        
        DATA = [size, 1]
        tmp = [(data>>i*8)&0xFF for i in range(size)]
        self.i2c.write_i2c_block_data(addr, reg, DATA)
        self.i2c.write_i2c_block_data(addr, tmp[0], tmp[1::])
        
    def getSlaveRegistarData(self, addr, reg, size):

        DATA = [size, 2]
        self.i2c.write_i2c_block_data(addr, reg, DATA)
        val = self.i2c.read_byte_data(addr, 1)
        return val
    
    def getSlaveData(self, addr, reg, size):
        val = 0
        for i in range(size):
            val |= self.getSlaveRegistarData(addr, reg*4+i, 1) << i*8
        return val
        
i2c = I2CMaster()
#レジスタ0に12345(4バイト送信)
i2c.writeSomeData(addr, 0, 12345, 4)
#レジスタ0の値4バイトを受信
data = i2c.getSlaveData(addr, 0, 4)
print(data)
#レジスタ1に6543(4バイト送信)
i2c.writeSomeData(addr, 1, 6543, 4)
#レジスタ1の値4バイトを受信
data = i2c.getSlaveData(addr, 1, 4)
print(data)

mbedはライブラリをあげときました。cppファイル中のサンプルコードで動きます。

os.mbed.com

まあそんな解説することもないです。

mbedの方のレジスタ登録は

union U{
    int8_t _Registar[0x40];
    int32_t Registar[0x10];
};
union U u = { 0 };
i2cslave slave(p28, p27, u._Registar);

としておいて、4バイトの方のレジスタを使用するといいです。

基本的にラズパイ側で使用する関数は、

writeSomeData(self, addr, reg, data, size)
getSlaveData(self, addr, reg, size)

だけです。

mbedとラズパイではI2Cアドレスの表記の仕方が下位1bitを書くか書かないかで異なるので、そこはmbedの方で修正するようにしてます。

こんな感じ

#define ADDR 0x1D
#define _ADDR ADDR<<1   //on raspi

あとは、スレーブからマスターにデータ送信するときの

case WriteRequired:
                I2CSlave::read(DATA, 1);    //捨て受信
                
                int8_t tmp = *(Registar+reg);
                I2CSlave::write(tmp);
                I2CSlave::write(tmp);
                I2CSlave::write(tmp);

あたりがこだわりポイントですかね。。。

I2CSlave::write(tmp);

を２つとかにすると動かないんですよ。

これmbedとラズパイの遅延時間とかの問題だと思うんですけど、waitを入れてもやはり動かなくて、

やけくそでI2CSlave::write(tmp);を連打してみたら動いたみたいな感じです。

すごく気持ち悪いので直したらすぐ修正しときます。