Translate

2019年3月14日木曜日

超音波センサHC-SR04をDonkey Carで使う


前方障害物との距離をはかりたくて、HC-SR04をRaspberry Piに接続し、Donkey Carのパーツクラス化してみた。

[GitHub] coolerking/donkeypart_hc_sr40
※インストール方法や使い方は上記リポジトリのREADME.mdを参照のこと。

このセンサ、5VとGND以外は、デジタルピンのままで使用できるのが良いのだけど、以下の仕様通りにGPIOを操作してやらないといけない。以下の記述は、購入元のスイッチサイエンス社サイトに掲載されていた。以下、引用する。

  • トリガ端子を10 us以上Highにしてください。
  • このセンサモジュールが40 kHzのパルスを8回送信して受信します。
  • 受信すると、出力端子がHighになります。
  • 出力端子がHighになっている時間がパルスを送信してから受信するまでの時間です。
  • 出力端子がHighになっている時間の半分を音速で割った数値が距離です。

トリガはTrigピン、出力端子というのがEchoピンです。

引用を要約すると、Trigピンに所定の値でOUTPUTして、Echo受信状態(HIGH)になったらLOWになるまでの時間を計測して、往復しているので2で割り、最後音速を使って自分で距離にしてね、ということ。

このような場合こそpigpioパッケージの指定ピンに対するコールバック関数機能が有効です。

最初に記述したリポジトリのコードは
pigpio python サンプルコードの「SonarRanger」をもとに書いたものです。

この超音波距離計即センサ、pigpioパッケージでコールバック関数をどう使うかを学習するのに最適。





この系統の処理なら、pigpioのほうがコード数が少ないと思いますよ。

2019年3月12日火曜日

DCモータをDonkey Carで使う

■DCモータを Donkey Carで使う


模型などでよく使用される マブチモータFC130RA-2270 をDonkey Carの駆動輪用に使用する際のパーツクラスをつくってみました。

Donkey Carを知らない人でも、Raspberry PiにDCモータを接続し、動作をPythonプログラムで操作したい人も参考になると思います。


■TA7291P DCモータドライバ



DCモータをRaspberry Piに直接つないでも操作することはできません。3.3VとGNDにつないで一定速度で回す事はできますが、速度を変えたり、逆転したりすることはできません。

このような場合、モータドライバというICを間にはさむ方法が一般的です。
このICにもいろんな種類があって、用途に合わせて選定しなくてはなりません。

・モータの種類(DCモータ、ステッピングモータ、サーボモータ)
・モータの個数
・モータ以外にGPIOピンをどのくらい使用するか

GPIOピンをほかのセンサやデバイスに使用する場合、26本もあるとはいえ、できるだけ使用ピンをへらしたいと考えるはずです。なれていない回路設計も1ピンへるだけでバグ発生原因が減るわけですし..

モータ数が多数などGPIOが足りない場合は、I2CやSPI対応しているICやマルチチャネルをもつICを選択します。

今回ここで使用するDCモータドライバ TA7291P は、I2CやSPIを使用しない、PWMピンを併用して回転速度調整をおこなう仕様となっています。

以下の図は、単一のDCモータの場合の結線例です。

 
GPIO番号接続先備考
5VTA7291P 7pin(Vcc), 10kΩ経由でTA7291P 4pin(Vref) 
GNDTA7291P 1pin(GND), DCモータGND
GPIO19TA7291P 5pin(IN1)PWM OUTPUT
GPIO26TA7291P 6pin(IN2)PWM OUTPUT

実は、Raspberry PiのハードウェアPWMピンもGPIO15/GPIO12か、GPIO13を使用しなくてはなりませんが、上図のGPIO19とGPIO26は通常のGPIOピンです。ハードウェアPWMではないGPIOピンをPWM化させると疑似PWMという品質の低いPWMになってしまいます。

しかし、これをハードウェアPWM並みの性能に上げる方法があります。

■ pigpio


pigpioはPythonパッケージの一つで、GPIOを操作することのできるライブラリを提供します。このライブラリをつかうと、疑似PWMの性能を向上させてくれる機能をもっているそうです。


pigpioはPythonプログラムからGPIOを操作するライブラリとしては後発で、実行前提としてpigpiodというデーモンプロセスをあらかじめRaspberry Pi上であげておかなくてはなりません。

デーモンが常時存在している恩恵もあります。別のノードからコマンドでGPIOを操作することができますし、疎結合となっているためテスト時にモッククラスを比較的簡単に実現することができます。


pigpioを使用可能にするには、以下のコマンドをRaspberry Pi上で実行します。

   $ sudo apt install -y pigpio
   $ sudo pigpiod
パーツクラスは以下のGitHubリポジトリにあります。

coolerking/donkeypart_dcmotor
https://github.com/coolerking/donkeypart_dcmotor

   $ cd ~/
   $ git clone https://github.com/coolerking/donkeypart_dcmotor.git
   $ cd donkeypart_dcmotor
   $ pip install -e .

■Donkey Carアプリの修正

まず~/mycar/config.pyに以下の設定値を記述します。

変数名設定する変数値
MOTOR_IN1_GPIOIN1と接続されているGPIOの番号
MOTOR_IN2_GPIOIN2と接続されているGPIOの番号

~/mycar/manage.pyを編集して、DCMotorパーツを追加します。
以下修正サンプルです。

:
# ダミーデータ
V.mem['user/motor/value'] = 0.5
V.mem['user/motor/status'] = 'move'
:

:
from pigpio
pi = pigpio.pi()

from donkeypart_dcmotor import DCMotor
motor = DCMotor(pi, cfg.MONITOR_IN1_GPIO, cfg.MOTOR_IN2_GPIO)
V.add(motor, inputs=['user/motor/value', 'user/motor/status'])
:
上記の通り、出力値がvalue(float型)とstatus(string型)の2種類に増えているので、Tubデータフォーマットも変わってきます。

引数名範囲説明推奨キー名
motor_value[-1.0, 1.0]のfloat値正値:正転、負値:逆転となる。最大値は、モータにVsピンと同等の電圧が加わる。本リポジトリのコードではアナログパッドなどの遊びを鑑み、(-0.1, 0.1)の範囲はゼロとして扱っている。user/motor/value
motor_statusmove/free/brakeのいずれか'move':モータ駆動、 'free':モータ駆動なし、'brake':制動停止user/motor/brake

このためmanage.pyTubWriter引数も合わせて変更する必要があります。以下は一例です。

    :
    # recording ダミー入力
    V.mem['recording'] = True
    :
    # Tubデータ・フォーマットも変更しなくてはならない
    inputs = ['cam/image_array', 'user/motor/value', 'user/motor/status', 'timestamp']
    types = ['image_array', 'float', 'str', 'str']

    # single tub
    tub = TubWriter(path=cfg.TUB_PATH, inputs=inputs, types=types)
    V.add(tub, inputs=inputs, run_condition='recording')
    :
donkeycar パッケージのデフォルトmanage.pyではdonkeycar.partモジュールにデフォルトのオートパイロットパーツクラスKerasLinearが格納されていますが、Tubデータの項目を変更したので、デフォルトのオートパイロットでは動作しなくなってしまいました。

このため、以下のようにKerasLinearの代替となるクラスを作成する必要があります。

MOTOR_STATUS = ['move', 'free', 'brake']
class MyPilot(KerasPilot):
    def __init__(self, model=None, num_outputs=None, *args, **kwargs):
        super(KerasLinear, self).__init__(*args, **kwargs)
        if model:
            self.model = model
        elif num_outputs is not None:
            self.model = my_default_linear()
        else:
            self.model = my_default_linear()

    def run(self, img_arr):
        img_arr = img_arr.reshape((1,) + img_arr.shape)
        outputs = self.model.predict(img_arr)
        # モータ値:回帰
        left_value = outputs[0][0][0]
        # モータステータス:分類
        left_status = MOTOR_STATUS[np.argmax(outputs[1][0][0])]

        # 操作データを返却
        return motor_value, motor_status


def my_default_linear():
    img_in = Input(shape=(120, 160, 3), name='img_in')
    x = img_in

    # Convolution2D class name is an alias for Conv2D
    x = Convolution2D(filters=24, kernel_size=(5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(filters=32, kernel_size=(5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(filters=64, kernel_size=(5, 5), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(filters=64, kernel_size=(3, 3), strides=(2, 2), activation='relu')(x)
    x = Convolution2D(filters=64, kernel_size=(3, 3), strides=(1, 1), activation='relu')(x)

    x = Flatten(name='flattened')(x)
    x = Dense(units=100, activation='linear')(x)
    x = Dropout(rate=.1)(x)
    x = Dense(units=50, activation='linear')(x)
    x = Dropout(rate=.1)(x)

    # 最終全結合層と活性化関数のみ差し替え
    # categorical output of the angle
    #angle_out = Dense(units=1, activation='linear', name='angle_out')(x)

    # continous output of throttle
    #throttle_out = Dense(units=1, activation='linear', name='throttle_out')(x)
   
    # モータ値(回帰)
    value_out  = Dense(units=1, activation='linear', name='value_out')(x)
    # モータ値(分類)
    status_out = Dense(units=len(MOTOR_STATUS), activation='softmax', name='left_status_out')(x)


    #model = Model(inputs=[img_in], outputs=[angle_out, #throttle_out])
    model = Model(inputs=[img_in], outputs=[value_out, status_out])

    #model.compile(optimizer='adam',
    #              loss={'angle_out': 'mean_squared_error',
    #                    'throttle_out': 'mean_squared_error'},
    #              loss_weights={'angle_out': 0.5, 'throttle_out': .5})
    # 回帰、分類別に最適化関数を使い分け
    model.compile(optimizer='adam',
                  loss={'value_out': 'mean_squared_error',
                        'status_out':  'categorical_crossentropy'},
                  loss_weights={'value_out': 0.5,
                        'status_out': 0.5})

    return model

上記モデルのアウトプット層は以下の通り。


変数名範囲説明
value_out[-1.0, 1.0]の範囲のfloat値DCモータの電圧値をあらしており、1.0/-1.0の場合、用意されたDCモータ用電圧の最大値が投入される。
status_out0,1,2のいずれかのint値配列MOTOR_STATUSのindex値に相当。

manage.pyのトレーニング関数train()も修正する必要があります。

まず先頭部分のTubデータのキーを修正します。

    :
    X_keys = ['cam/image_array']
    y_keys = ['user/motor/value', 'user/motor/status']
    :
そして、Tubデータのmotor_statusが文字列であるのに対し、モデルのアウトプット層の所定の変数は0,1,2のint値となっているため、このあたりの変更にも対応し無くてはならない。本来は大改修が必要なのだけど、donkeycarパッケージはその点うまいかんじにできている(これに気づいているユーザは少ないと思うけど..)。

TubGroup のトレーニングバッチと評価用バッチデータを取得するための関数TubGroup.get_traun_val_genの引数train_record_transformval_record_transformに修正する関数motor_status_transformを与えることで対応できてしまうのだ。

    :
    tubgroup = TubGroup(tub_names)
    train_gen, val_gen = tubgroup.get_train_val_gen(
        X_keys, y_keys,
        batch_size=cfg.BATCH_SIZE,
        train_frac=cfg.TRAIN_TEST_SPLIT,
        train_record_transform=motor_status_transform,
        val_record_transform=motor_status_transform)
    :
関数`motor_status_transform`の定義例は、以下の通り。

MOTOR_STATUS = ['move', 'free', 'brake']
:
def agent_record_transform(record_dict):
    """
    TubGroupクラスのメソッドget_train_val_genの引数として渡し、
    Agent用Tubデータ仕様のものでも学習データ化できるようにする
    マッピング関数。

    引数
        record_dict     もとのレコードデータ(Agent用Tubデータ、辞書型)
    戻り値
        record_dict     AIのinput/output層にあったマッピングが終わったレコードデータ(辞書型)
    例外
        ValueError      モータステータスキーが1件も存在しない場合
    """
    status_val = record_dict['user/motor/status']
    if status_val is not None:
        record_dict['user/motor/status'] = motor_status_to_list(status_val)
    else:
        raise ValueError('no value of key=\"user/motor/status\" in loaded record')
    return record_dict

def motor_status_to_list(val):
    """
    モータステータス値を数値リスト化する。

    引数
        モータステータス値
    戻り値
        数値リスト
    """
    classes = np.zeros(len(MOTOR_STATUS)).tolist()
    classes[MOTOR_STATUS.index(val)] = 1
    return classes

■まとめ

・pigpioパッケージで実装すれば疑似PWMはハードウェアPWMに近い性能になる
・I2C/SPIによる回路短縮をえらぶと、コードが増えるし、データシートが読めないと難しい
・PWMのみの場合はコードは簡単だが、ピン数は複雑になる


ちなみに、I2Cを使う場合はDRV8830があります。SPI化は更にADC/DACをつかうことで実現できます。


<後日追記>
実はこのTA7291P、現在では使用していません。
理由はモータ電源の電圧と実際にモータへ割り当てられる電圧差が大きいこと。
一度実測した値だと、2.6Vの入力に対して2.1~1.8Vくらい。

今はTB6612(実際は秋月のモータ不ドライブキット)を使っている。
こちらだと実測2.4Vが2.3V~2.0Vくらい。
キットの足とPiをジャンパ線でつないで使ってます。


はんだでジャンパするとピン1本管理しなくていいのだが、
手が滑ってこてをチップにあててしまい1個無駄にしてしまった。
不器用な方はご注意を。


Raspberry Pi 4B(Raspbean buster lite) へ pyrealsense2 をインストールする

DonkeycarでIntel RealSense T265 トラッキングカメラを使おうと思ったのだけど、ドキュメントにUSB3.0推奨とある記述をみつけだ。 どうもUSB2.0でも動作するのだけど、イメージストリームを使う場合などは3.0のほうが良いのだろう。白黒の800...