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本もあるとはいえ、できるだけ使用ピンをへらしたいと考えるはずです。なれていない回路設計も１ピンへるだけでバグ発生原因が減るわけですし..

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

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

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

　

GPIO番号	接続先	備考
5V	TA7291P 7pin(Vcc), 10kΩ経由でTA7291P 4pin(Vref)
GND	TA7291P 1pin(GND), DCモータGND
GPIO19	TA7291P 5pin(IN1)	PWM OUTPUT
GPIO26	TA7291P 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_GPIO	IN1と接続されているGPIOの番号
MOTOR_IN2_GPIO	IN2と接続されている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型)の２種類に増えているので、Tubデータフォーマットも変わってきます。

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

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

    :
    # 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_out	0,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_transformとval_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      モータステータスキーが１件も存在しない場合
    """
    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をジャンパ線でつないで使ってます。


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