LoginSignup
5
3

More than 5 years have passed since last update.

@K-Ponta

Raspberry Pi 3 Mobel B+とカムプログラムロボットでロボット作成

Last updated at Posted at 2019-02-12

はじめに

Raspberry Pi 3 Model B+をタミアのカムプログラムロボット(ガンメタ／オレンジ)に組み込んでロボットを作成しました。外観と動作はこんな感じです。
なるべく外観を崩さないようにしたかったのですが、いろいろと詰め込んでゴテゴテしてます（笑）
P_20190213_001414_vHDR_Auto.jpg

ComeRob動作の様子(Youtubeへ移動します)

機能一覧

現在は下記の機能があります。

  1. TA7291Pを使ったモータ制御を行い、前進、後退、左旋回、右旋回、ブレーキ、ストップを行う
  2. モータ動作モードや出力(%)と作動時間(秒)を記述したCSVファイルを読み込み、その内容で動作させる
  3. MPU9250による加速度、ジャイロ、地磁気の取得し、取得した加速度などに時間と動作モードと追加してCSVファイルへの保存

動作は指定された出力と秒数で、前進、後退、左旋回、右旋回を行うだけです。
Webカムはいまのところ、動作はしていません(2019/02現在)。

今回はブレッドボードの配線や、GPIOの接続などの説明はしておらず、ソフトウェア関連の記述になっています。作ってソフト関連のところが一番苦労したので、こちらを先に公開しようと考えました。
これだけ見れば動作できるようには出来ないようにはならんだろうな。。と思います。
ハードウェア周りはまた別の機会に記述したいと思います。

参考にした本やサイト

実装の際、下記の本やサイトを参考にしました。

■ カラー図解 最新 Raspberry Piで学ぶ電子工作(ブルーバックス刊)
こちらの本、すごく参考になりました。
はじめは記述されたとおりに、LEDをチカチカさせて遊ぶのから初めて、段々と複数のLEDを光らせたり、ランダム間隔で点滅させたりと、各記述を色々と改良して試しました。
モータドライバの制御やGPIO配線はほぼこちらの書籍の内容になっています。

■Rasberry pi 3でストロベリー・リナックス社製の「MPU-9250 ９軸センサモジュール (メーカー品番：MPU-9250)」を使う
Qiitaのboyaki_machineさんのページです。
加速度センサやジャイロはデータシートなどみて何とかできたのですが、地磁気センサーの制御はこのページのコードを利用しております。
はじめは地磁気センサー周りだけ移植しようかとも思ったのですが、加速度などもそのまま利用させて頂いています。

RaspberryPiの設定について

RaspberryPiのOS設定などは、詳しく解説しているページが多々ありますので、そちらを参照してください。
特殊な設定はしていなく、

  • piユーザのパスワード変更と自動ログインの停止
  • CUIログインの設定
  • sshの有効化
  • GPIOの有効化

の設定を行っています。
利用OSはNOOBSです。Raspbianでも動作すると思います。
将来的にOpenCVをいれてWebカムによる画像認識と動作確認をしたいので、GUIも利用できるようフルパッケージ版にしました。今のところGUIの機能はいらないの、起動はCUIベースに変更しています。

動作コード

プログラムは仕事でやったことがあるのですが、pythonはほとんど使ったことがなく、ここまでのステップ数のプログラムを書いたのは初めてなので、問題がある部分もあると思います。
その点はご了承ください。

下記のコードに３つになります。

  • motor_sensor.py（メインプログラム）
  • mpu9250.py （MPU9250制御クラス）
  • ta7291p.py （TA7291P制御クラス）

メインプログラムや各制御クラスはRaspberry Piの下記のディレクトリに配置して動作させています。

/home/pi/Documents/RaspberryPi_DIY/CamRobo/

MPU9250やTA7291Pはクラスライブラリにしています。motor_sensor.pyと同じディレクトリに置いてください。

main code

motor_sensor.py
#!/usr/bin/python3
# -*- coding: utf_8 -*-
from time import sleep,time
from datetime import datetime
import sys
import threading

import ta7291p
import mpu9250

# MPU9250 センサクラス
class mpu9250_sesor(threading.Thread):
    def __init__(self, mode):
        threading.Thread.__init__(self)
        # mpu9250センサオブジェクト
        self.sensor = mpu9250.SL_MPU9250(0x68,1)
        # 動作モード
        self.mode = mode

    def run(self):
        try:
            # センサ初期化
            self.sensor.resetRegister()
            self.sensor.powerWakeUp()
            self.sensor.setAccelRange(8,True)
            self.sensor.setGyroRange(1000,True)
            self.sensor.setMagRegister('100Hz','16bit')

            # 静止モード's'に設定
            self.mode = 's'
            # センセデータ出力ファイル
            filename = '/home/pi/Documents/RaspberryPi_DIY/CamRobo/data/gryo_accel_{!s}.csv'.format(datetime.now().strftime("%Y%m%d%H%M%S"))
            with open(filename,'w') as f:
                f.write('tm,ax,ay,az,gx,gy,gz,mx,my,mz,md\n')
                while True:
                    # 動作モードが終了以外の場合
                    if self.mode != 'd':
                        now = time()
                        acc = self.sensor.getAccel()
                        gyr = self.sensor.getGyro()
                        mag = self.sensor.getMag()
                        f.write('{0:.7f},'
                                '{1[0]:.7f},{1[1]:.7f},{1[2]:.7f},'
                                '{2[0]:.7f},{2[1]:.7f},{2[2]:.7f},'
                                '{3[0]:.7f},{3[1]:.7f},{3[2]:.7f},'
                                '{4}\n'
                                .format(now,acc,gyr,mag,self.mode))
                        sleep(0.1)
                    # 動作モードが終了の場合は whileを抜ける
                    else:
                        break
        except KeyboardInterrupt:
            pass
        finally:
            self.sensor.finallySensor()

# モータ制御オブジェクト PWMを 50Hzに指定
motor = ta7291p.TB_TA7291P(50)

# 動作ファイルの指定がない場合は、move_list.csvを読み込む
if len(sys.argv) < 2:
    file_path = './move_list.csv'
else:
    file_path = sys.argv[1]

try:
    # MPU9205センサスレッド生成、モードを初期化(i)で設定
    acc_gyr_mag_senser = mpu9250_sesor('i')
    # MPU9250 センサスタート
    acc_gyr_mag_senser.start()

    # MPU9250センサの初期が終わるまでwait
    while acc_gyr_mag_senser.mode == 'i':
        sleep(0.1)

    # 静止モードに設定
    acc_gyr_mag_senser.mode = 's'

    # モーター電源On
    motor.power_on()
    with open(file_path) as f:

        for line in f:
            move_data = line.rstrip('\n').split(',')

            # MPU9250にセンサモードを設定
            acc_gyr_mag_senser.mode = move_data[0]

            # 動作モードごとに呼び出し先を変更
            if move_data[0] == 'fw':
                # 前進
                motor.mv_forward(float(move_data[1]), float(move_data[2]))
            elif move_data[0] == 'bw':
                # 後退
                motor.mv_backword(float(move_data[1]), float(move_data[2]))
            elif move_data[0] == 'rt':
                # 右旋回
                motor.mv_right_turn(float(move_data[1]), float(move_data[2]))
            elif move_data[0] == 'lt':
                # 左旋回
                motor.mv_left_turn(float(move_data[1]), float(move_data[2]))
            elif move_data[0] == 'b':
                # ブレーキ
                motor.mv_brake(float(move_data[2]))
            elif move_data[0] == 's':
                # Stop
                motor.mv_stop(float(move_data[2]))
        # MPU9250センサを終了モートに設定
        acc_gyr_mag_senser.mode = 'd'
except KeyboardInterrupt:
    pass
finally:
    # モーター電源off
    motor.power_down()

mpu9250 センサ制御

MPU9250とTA7291Pの制御クラスは下記になります。
MPU9250のコードはboyaki_machineさんのコードについて次の部分を修正しています。
1. python3で動作するようにする
2. Ctrl+Cで停止させた場合なので終了処理の追加
3. 単体動作時にセンサから取得したデータを表示するprint文を変更しています。

mpu9250.py
#!/usr/bin/python3 -u
# -*- coding: utf-8 -*-

import smbus
import time

# Strawberry Linux社の「MPU-9250」からI2Cでデータを取得するクラス(python 2)
# https://strawberry-linux.com/catalog/items?code=12250
#
# 2016-05-03 Boyaki Machine
#   オリジナル
# 2019-02-09 K-ponta
#   Python3 に対応。
#   MPU9250 終了処理を追加。finallySensor()
# 
class SL_MPU9250:
    # 定数宣言
    REG_PWR_MGMT_1      = 0x6B
    REG_INT_PIN_CFG     = 0x37
    REG_GYRO_CONFIG     = 0x1B
    REG_ACCEL_CONFIG1   = 0x1C
    REG_ACCEL_CONFIG2   = 0x1D

    MAG_MODE_POWERDOWN  = 0         # 磁気センサpower down
    MAG_MODE_SERIAL_1   = 1         # 磁気センサ8Hz連続測定モード
    MAG_MODE_SERIAL_2   = 2         # 磁気センサ100Hz連続測定モード
    MAG_MODE_SINGLE     = 3         # 磁気センサ単発測定モード
    MAG_MODE_EX_TRIGER  = 4         # 磁気センサ外部トリガ測定モード
    MAG_MODE_SELF_TEST  = 5         # 磁気センサセルフテストモード

    MAG_ACCESS          = False     # 磁気センサへのアクセス可否
    MAG_MODE            = 0         # 磁気センサモード
    MAG_BIT             = 14        # 磁気センサが出力するbit数

    offsetRoomTemp      = 0
    tempSensitivity     = 333.87
    gyroRange           = 250       # 'dps' 00:250, 01:500, 10:1000, 11:2000
    accelRange          = 2         # 'g' 00:±2, 01:±4, 10:±8, 11:±16
    magRange            = 4912      # 'μT'  

    offsetAccelX        = 0.0
    offsetAccelY        = 0.0
    offsetAccelZ        = 0.0
    offsetGyroX         = 0.0
    offsetGyroY         = 0.0
    offsetGyroZ         = 0.0

    # コンストラクタ
    def __init__(self, address, channel):
        self.address    = address
        self.channel    = channel
        self.bus        = smbus.SMBus(self.channel)
        self.addrAK8963 = 0x0C

        # Sensor initialization
        self.resetRegister()
        self.powerWakeUp()

        self.gyroCoefficient    = self.gyroRange  / float(0x8000)   # センシングされたDecimal値をdpsに変換する係数
        self.accelCoefficient   = self.accelRange / float(0x8000)   # センシングされたDecimal値をgに変換する係数
        self.magCoefficient16   = self.magRange   / 32760.0         # センシングされたDecimal値をμTに変換する係数(16bit時)
        self.magCoefficient14   = self.magRange   / 8190.0          # センシングされたDecimal値をμTに変換する係数(14bit時)

    # レジスタを初期設定に戻します。
    def resetRegister(self):
        if self.MAG_ACCESS == True:
            self.bus.write_i2c_block_data(self.addrAK8963, 0x0B, [0x01])    
        self.bus.write_i2c_block_data(self.address, 0x6B, [0x80])
        time.sleep(0.1)
        # DLPF設定
        self.bus.write_i2c_block_data(self.address, 0x1A, [0x00])
        self.MAG_ACCESS = False
        time.sleep(0.1)

    # レジスタをセンシング可能な状態にします。
    def powerWakeUp(self):
        # PWR_MGMT_1をクリア
        self.bus.write_i2c_block_data(self.address, self.REG_PWR_MGMT_1, [0x00])
        time.sleep(0.1)
        # I2Cで磁気センサ機能(AK8963)へアクセスできるようにする(BYPASS_EN=1)
        self.bus.write_i2c_block_data(self.address, self.REG_INT_PIN_CFG, [0x02])
        self.MAG_ACCESS = True
        time.sleep(0.1)

    # センサの終了処理
    def finallySensor(self):
        self.bus.write_i2c_block_data(self.address, self.REG_PWR_MGMT_1, [0x80])
        time.sleep(0.1)
        self.bus.write_i2c_block_data(self.address, self.REG_PWR_MGMT_1, [0x40])
        time.sleep(0.1)

    # 磁気センサのレジスタを設定する
    def setMagRegister(self, _mode, _bit):      
        if self.MAG_ACCESS == False:
            # 磁気センサへのアクセスが有効になっていないので例外を上げる
            raise Exception('001 Access to a sensor is invalid.')

        _writeData  = 0x00
        # 測定モードの設定
        if _mode=='8Hz':            # 連続測定モード１
            _writeData      = 0x02
            self.MAG_MODE   = self.MAG_MODE_SERIAL_1
        elif _mode=='100Hz':        # 連続測定モード２
            _writeData      = 0x06
            self.MAG_MODE   = self.MAG_MODE_SERIAL_2
        elif _mode=='POWER_DOWN':   # パワーダウンモード
            _writeData      = 0x00
            self.MAG_MODE   = self.MAG_MODE_POWERDOWN
        elif _mode=='EX_TRIGER':    # 外部トリガ測定モード
            _writeData      = 0x04
            self.MAG_MODE   = self.MAG_MODE_EX_TRIGER
        elif _mode=='SELF_TEST':    # セルフテストモード
            _writeData      = 0x08
            self.MAG_MODE   = self.MAG_MODE_SELF_TEST
        else:   # _mode='SINGLE'    # 単発測定モード
            _writeData      = 0x01
            self.MAG_MODE   = self.MAG_MODE_SINGLE

        #　出力するbit数 
        if _bit=='14bit':           # 14bit出力
            _writeData      = _writeData | 0x00
            self.MAG_BIT    = 14
        else:   # _bit='16bit'      # 16bit 出力
            _writeData      = _writeData | 0x10
            self.MAG_BIT    = 16

        self.bus.write_i2c_block_data(self.addrAK8963, 0x0A, [_writeData])

    # 加速度の測定レンジを設定します。広レンジでは測定粒度が荒くなります。
    # val = 16, 8, 4, 2(default)
    def setAccelRange(self, val, _calibration=False):
        # ±2g (00), ±4g (01), ±8g (10), ±16g (11)
        if val==16 :
            self.accelRange     = 16
            _data               = 0x18
        elif val==8 :
            self.accelRange     = 8
            _data               = 0x10
        elif val==4 :
            self.accelRange     = 4
            _data               = 0x08
        else:
            self.accelRange     = 2
            _data               = 0x00

        self.bus.write_i2c_block_data(self.address, self.REG_ACCEL_CONFIG1, [_data])
        self.accelCoefficient   = self.accelRange / float(0x8000)
        time.sleep(0.1)

        # オフセット値をリセット(過去のオフセット値が引き継がれないように)
        self.offsetAccelX       = 0
        self.offsetAccelY       = 0
        self.offsetAccelZ       = 0

        # 本当はCalibrationしたほうが良いと思うけれど、時間もかかるし。
        if _calibration == True:
            self.calibAccel(1000)
        return

    # ジャイロの測定レンジを設定します。広レンジでは測定粒度が荒くなります。
    # val= 2000, 1000, 500, 250(default)
    def setGyroRange(self, val, _calibration=False):
        if val==2000:
            self.gyroRange      = 2000
            _data               = 0x18
        elif val==1000:
            self.gyroRange      = 1000
            _data               = 0x10
        elif val==500:
            self.gyroRange      = 500
            _data               = 0x08
        else:
            self.gyroRange      = 250
            _data               = 0x00

        self.bus.write_i2c_block_data(self.address, self.REG_GYRO_CONFIG, [_data])
        self.gyroCoefficient    = self.gyroRange / float(0x8000)
        time.sleep(0.1)

        # オフセット値をリセット(過去のオフセット値が引き継がれないように)
        self.offsetGyroX        = 0
        self.offsetGyroY        = 0
        self.offsetGyroZ        = 0

        # 本当はCalibrationしたほうが良いのだが、時間もかかるし。
        if _calibration == True:
            self.calibGyro(1000)
        return

    # 加速度センサのLowPassFilterを設定します。
    # def setAccelLowPassFilter(self,val):      

    #センサからのデータはそのまま使おうとするとunsignedとして扱われるため、signedに変換(16ビット限定）
    def u2s(self,unsigneddata):
        if unsigneddata & (0x01 << 15) : 
            return -1 * ((unsigneddata ^ 0xffff) + 1)
        return unsigneddata

    # 加速度値を取得します
    def getAccel(self):
        data    = self.bus.read_i2c_block_data(self.address, 0x3B ,6)
        rawX    = self.accelCoefficient * self.u2s(data[0] << 8 | data[1]) + self.offsetAccelX
        rawY    = self.accelCoefficient * self.u2s(data[2] << 8 | data[3]) + self.offsetAccelY
        rawZ    = self.accelCoefficient * self.u2s(data[4] << 8 | data[5]) + self.offsetAccelZ
        return rawX, rawY, rawZ

    # ジャイロ値を取得します。
    def getGyro(self):
        data    = self.bus.read_i2c_block_data(self.address, 0x43 ,6)
        rawX    = self.gyroCoefficient * self.u2s(data[0] << 8 | data[1]) + self.offsetGyroX
        rawY    = self.gyroCoefficient * self.u2s(data[2] << 8 | data[3]) + self.offsetGyroY
        rawZ    = self.gyroCoefficient * self.u2s(data[4] << 8 | data[5]) + self.offsetGyroZ
        return rawX, rawY, rawZ

    def getMag(self):
        if self.MAG_ACCESS == False:
            # 磁気センサが有効ではない。
            raise Exception('002 Access to a sensor is invalid.')

        # 事前処理
        if self.MAG_MODE==self.MAG_MODE_SINGLE:
            # 単発測定モードは測定終了と同時にPower Downになるので、もう一度モードを変更する
            if self.MAG_BIT==14:                # 14bit出力
                _writeData      = 0x01
            else:                               # 16bit 出力
                _writeData      = 0x11
            self.bus.write_i2c_block_data(self.addrAK8963, 0x0A, [_writeData])
            time.sleep(0.01)

        elif self.MAG_MODE==self.MAG_MODE_SERIAL_1 or self.MAG_MODE==self.MAG_MODE_SERIAL_2:
            status  = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
            if (status[0] & 0x02) == 0x02:
                # データオーバーランがあるので再度センシング
                self.bus.read_i2c_block_data(self.addrAK8963, 0x09 ,1)

        elif self.MAG_MODE==self.MAG_MODE_EX_TRIGER:
            # 未実装
            return

        elif self.MAG_MODE==self.MAG_MODE_POWERDOWN:
            raise Exception('003 Mag sensor power down')

        # ST1レジスタを確認してデータ読み出しが可能か確認する。
        status  = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)
        while (status[0] & 0x01) != 0x01:
            # データレディ状態まで待つ
            time.sleep(0.01)
            status  = self.bus.read_i2c_block_data(self.addrAK8963, 0x02 ,1)

        # データ読み出し
        data    = self.bus.read_i2c_block_data(self.addrAK8963, 0x03 ,7)
        rawX    = self.u2s(data[1] << 8 | data[0])  # 下位bitが先
        rawY    = self.u2s(data[3] << 8 | data[2])  # 下位bitが先
        rawZ    = self.u2s(data[5] << 8 | data[4])  # 下位bitが先
        st2     = data[6]

        # オーバーフローチェック
        if (st2 & 0x08) == 0x08:
            # オーバーフローのため正しい値が得られていない
            raise Exception('004 Mag sensor over flow')

        # μTへの変換
        if self.MAG_BIT==16:    # 16bit出力の時
            rawX    = rawX * self.magCoefficient16
            rawY    = rawY * self.magCoefficient16
            rawZ    = rawZ * self.magCoefficient16
        else:                   # 14bit出力の時
            rawX    = rawX * self.magCoefficient14
            rawY    = rawY * self.magCoefficient14
            rawZ    = rawZ * self.magCoefficient14

        return rawX, rawY, rawZ

    def getTemp(self):
        data    = self.bus.read_i2c_block_data(self.address, 0x65 ,2)
        raw     = data[0] << 8 | data[1]
        return ((raw - self.offsetRoomTemp) / self.tempSensitivity) + 21

    def selfTestMag(self):
        print ("start mag sensor self test")
        self.setMagRegister('SELF_TEST','16bit')
        self.bus.write_i2c_block_data(self.addrAK8963, 0x0C, [0x40])
        time.sleep(1.0)
        data = self.getMag()

        print (data)

        self.bus.write_i2c_block_data(self.addrAK8963, 0x0C, [0x00])
        self.setMagRegister('POWER_DOWN','16bit')
        time.sleep(1.0)
        print ("end mag sensor self test")
        return

    # 加速度センサを較正する
    # 本当は緯度、高度、地形なども考慮する必要があるとは思うが、簡略で。
    # z軸方向に正しく重力がかかっており、重力以外の加速度が発生していない前提
    def calibAccel(self, _count=1000):
        print ("Accel calibration start")
        _sum    = [0,0,0]

        # 実データのサンプルを取る
        for _i in range(_count):
            _data   = self.getAccel()
            _sum[0] += _data[0]
            _sum[1] += _data[1]
            _sum[2] += _data[2]

        # 平均値をオフセットにする
        self.offsetAccelX   = -1.0 * _sum[0] / _count
        self.offsetAccelY   = -1.0 * _sum[1] / _count
        self.offsetAccelZ   = -1.0 * ((_sum[2] / _count ) - 1.0)    # 重力分を差し引く

        # オフセット値をレジスタに登録したいけれど、動作がわからないので実装保留

        print ("Accel calibration complete")
        return self.offsetAccelX, self.offsetAccelY, self.offsetAccelZ

    # ジャイロセンサを較正する
    # 各軸に回転が発生していない前提
    def calibGyro(self, _count=1000):
        print ("Gyro calibration start")
        _sum    = [0,0,0]

        # 実データのサンプルを取る
        for _i in range(_count):
            _data   = self.getGyro()
            _sum[0] += _data[0]
            _sum[1] += _data[1]
            _sum[2] += _data[2]

        # 平均値をオフセットにする
        self.offsetGyroX    = -1.0 * _sum[0] / _count
        self.offsetGyroY    = -1.0 * _sum[1] / _count
        self.offsetGyroZ    = -1.0 * _sum[2] / _count

        # オフセット値をレジスタに登録したいけれど、動作がわからないので実装保留

        print ("Gyro calibration complete")
        return self.offsetGyroX, self.offsetGyroY, self.offsetGyroZ


if __name__ == "__main__":
    sensor  = SL_MPU9250(0x68,1)
    try:
        sensor.resetRegister()
        sensor.powerWakeUp()
        sensor.setAccelRange(8,True)
        #sensor.setAccelRange(8,False)
        sensor.setGyroRange(1000,True)
        sensor.setMagRegister('100Hz','16bit')
        # sensor.selfTestMag()
        while True:
            now     = time.time()
            acc     = sensor.getAccel()
            gyr     = sensor.getGyro()
            mag     = sensor.getMag()
            print ('{0[0]:.7f},{0[1]:.7f},{0[2]:.7f},'
                   '{1[0]:.7f},{1[1]:.7f},{1[2]:.7f},'
                   '{2[0]:.7f},{2[1]:.7f},{2[2]:.7f}'.format(acc,gyr,mag))

            sleepTime       = 0.1 - (time.time() - now)
            if sleepTime < 0.0:
                continue
            time.sleep(sleepTime)
    except KeyboardInterrupt:
        pass
    finally:
        sensor.finallySensor()

TA7291P モータドライバ制御

TA7291Pの制御クラスは下記のコードになります。
Raspberry Piで学ぶ電子工作のコードを参考に、Python3化や各動作を関数化したりしています。

ta7291p.py
#!/usr/bin/python3 -u
# -*- coding: utf_8 -*-
import RPi.GPIO as GPIO
from time import sleep

class TB_TA7291P:
    # 定数宣言
    GPIO_L1 = 24    # 左モーター Line1
    GPIO_L2 = 25    # 左モーター Line2
    GPIO_R1 = 22    # 右モーター Line1
    GPIO_R2 = 23    # 右モーター Line2
    MAX_POWER = 75    # PWM最大値 これ以上の値が入力されて場合はこの値に変更

    # コンストラクタ
    def __init__(self, pwm_hz = 50):
        GPIO.setmode(GPIO.BCM)

        # Left Sied
        GPIO.setup(self.GPIO_L1, GPIO.OUT)
        GPIO.setup(self.GPIO_L2, GPIO.OUT)
        # Right Side
        GPIO.setup(self.GPIO_R1, GPIO.OUT)
        GPIO.setup(self.GPIO_R2, GPIO.OUT)

        # 
        self.L_p0 = GPIO.PWM(self.GPIO_L1, pwm_hz)  # GPIO=24、周波数50Hz
        self.L_p1 = GPIO.PWM(self.GPIO_L2, pwm_hz)  # GPIO=25、周波数50Hz
        # 
        self.R_p0 = GPIO.PWM(self.GPIO_R1, pwm_hz)  # GPIO=24、周波数50Hz
        self.R_p1 = GPIO.PWM(self.GPIO_R2, pwm_hz)  # GPIO=25、周波数50Hz

    # Power ON
    def power_on(self):
        self.L_p0.start(100)
        self.L_p1.start(100)
        self.R_p0.start(100)
        self.R_p1.start(100)

    # Power Down
    def power_down(self):
        ### 終了処理
        self.L_p0.stop(0)
        self.L_p1.stop(0)
        self.R_p0.stop(0)
        self.R_p1.stop(0)

        GPIO.cleanup()

    # 前進
    def mv_forward(self, power_gain, op_time):
        # 出力が100% を超えていた場合は 100% にする
        if ( power_gain > 100 ):
            power_gain = 100

        # 左右前進
        self.L_p0.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)
        self.L_p1.ChangeDutyCycle(0)
        self.R_p0.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)
        self.R_p1.ChangeDutyCycle(0)

        # op_time秒維持
        sleep(op_time)

        return

    # 後退
    def mv_backword(self, power_gain, op_time):
        # 出力が100% を超えていた場合は 100% にする
        if ( power_gain > 100 ):
            power_gain = 100

        # 左右 back
        self.L_p0.ChangeDutyCycle(0)
        self.L_p1.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)
        self.R_p0.ChangeDutyCycle(0)
        self.R_p1.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)

        # op_time秒維持
        sleep(op_time)

        return

    # 左旋回
    def mv_left_turn(self, power_gain, op_time):
        # 出力が100% を超えていた場合は 100% にする
        if ( power_gain > 100 ):
            power_gain = 100

        # 左後退、右前進
        self.L_p0.ChangeDutyCycle(0)
        self.L_p1.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)
        self.R_p0.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)
        self.R_p1.ChangeDutyCycle(0)

        # op_time秒維持
        sleep(op_time)

        return

    # 右旋回
    def mv_right_turn(self, power_gain, op_time):
        # 出力が100% を超えていた場合は 100% にする
        if ( power_gain > 100 ):
            power_gain = 100

        # 左前進、右後退
        self.L_p0.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)
        self.L_p1.ChangeDutyCycle(0)
        self.R_p0.ChangeDutyCycle(0)
        self.R_p1.ChangeDutyCycle(self.MAX_POWER * power_gain / 100)

        # op_time秒維持
        sleep(op_time)

        return

    # Brake
    def mv_brake(self, op_time = 0.1):
        # Brake
        self.L_p0.ChangeDutyCycle(self.MAX_POWER)
        self.L_p1.ChangeDutyCycle(self.MAX_POWER)
        self.R_p0.ChangeDutyCycle(self.MAX_POWER)
        self.R_p1.ChangeDutyCycle(self.MAX_POWER)

        # op_time秒維持、標準は0.1秒
        sleep(op_time)

        return

    # Stop
    def mv_stop(self, op_time = 0.1):
        # Stop
        self.L_p0.ChangeDutyCycle(0)
        self.L_p1.ChangeDutyCycle(0)
        self.R_p0.ChangeDutyCycle(0)
        self.R_p1.ChangeDutyCycle(0)

        # op_time秒維持、省略した場合は0.1秒
        sleep(op_time)

        return

if __name__ == "__main__":
    moter = TB_TA7291P(50)

    try:
        # 動作テスト
        # 電源On
        moter.power_on()
        # 前進 出力100%で2秒間
        moter.mv_forward(100,2)
        # 停止 1秒間
        moter.mv_stop(1)
        # 後退 出力100%で2秒間
        moter.mv_backword(100,2)
        # 停止 1秒間
        moter.mv_stop(1)
        # 左旋回 出力100%で3秒間
        moter.mv_left_turn(100,3)
        # 停止 省略した場合は0.1秒間
        moter.mv_stop()
        # 右旋回 出力100%で3秒間
        moter.mv_right_turn(100,3)
        # 停止 省略した場合は0.1秒間
        moter.mv_stop()
        # 前進 出力100%で2秒間
        moter.mv_forward(100,2)
        # ブレーキ 1秒間
        moter.mv_brake(1)
        # 後退 出力100%で2秒間
        moter.mv_backword(100,2)
        # 停止 1秒間
        moter.mv_stop(1)
    except KeyboardInterrupt:
        pass
    finally:
        # 電源Off
        moter.power_down()

起動方法

motor_sensor.py に実行権限を与えておきます。
motor_sensor.py は、引数で動作指定CSVファイルを指定しないと、 motor_sensor.py と同じ場所の move_list.csv を読み込みに行くので、起動前に move_list.csv を次項目の「動作指定CSVファイル」の書式に作成してください。
MPU9250センサがファイルを出力するディレクトリも作成しておきます。

$ chmod +x ./motor_sensor.py
$ vi move_list.csv
$ mkdir data
$ ./motor_sensor.py

任意の動作指定CSVを指定する場合は下記にように指定してください。

$ ./motor_sensor.py ./move_list_2.csv

動作指定CSVファイル

motor_sensor.pyは、引数に動作ファイルを指定しないと、同じ場所に置かれた move_list.csv を読み込むようになっています。
move_list.csv の記述例は下記のとおりです。

move_list.csv
fw,100,3
s,,1
bw,100,3
s,,1
rt,100,5
s,,1
lt,100,5
s,,1
fw,100,3
s,,1
bw,100,3
s,,1
rt,100,5
s,,1
lt,100,5
s,,1
fw,100,3
s,,1
bw,100,3
s,,1
rt,100,5
s,,1
lt,100,5
s,,1

動作指定CSVファイルは下記のような書式で記述します。

動作モード 書式 記述例
前進 fw,出力(%),動作時間(秒) fw,100,5
後退 bw,出力(%),動作時間(秒) bw,80,10
左旋回 lt,出力(%),動作時間(秒) lt,100,15
右旋回 rt,出力(%),動作時間(秒) rt,70,10
ブレーキ b,,動作時間(秒) b,,1
ストップ s,,動作時間(秒) s,,0.1

MPU9250センサの出力結果

実行が終了すると、dataディレクトリに gryo_accel_YYYYMMDDHHMISS.csv というファイル名で実行結果が出力されます。

MPU9250センサの出力結果例

gryo_accel_20190211171249.csv
tm,ax,ay,az,gx,gy,gz,mx,my,mz,md
1549872769.3912337,-0.0016426,-0.0022119,0.9971360,0.1807556,-0.0335693,0.0632019,3.2986569,1.1995116,52.4786325,s
1549872769.5083649,-0.0275215,-0.0832666,0.9876145,2.8663025,2.0111084,0.6125183,4.1982906,2.3990232,50.3794872,fw
1549872769.6260414,0.0999199,-0.0019678,0.9761399,-1.5587463,5.7037354,1.0397644,3.2986569,2.9987790,50.9792430,fw
1549872769.7436359,0.0051934,-0.0881494,0.7981614,0.1807556,-3.3905029,2.3825378,2.6989011,1.4993895,51.8788767,fw
・
・
中略
・
・
1549872781.9190757,-0.0411934,-0.0073389,0.7908372,-6.6246643,4.6661377,5.2511902,3.1487179,1.9492063,52.0288156,bw
1549872782.0366840,-0.1910957,0.0590674,1.0276536,3.8733826,-3.8177490,4.7934265,2.2490842,3.1487179,51.1291819,bw
1549872782.1542869,0.0618340,0.1633154,0.9378098,-3.2982483,3.1402588,-0.1504211,2.6989011,3.2986569,51.5789988,bw
1549872782.2611101,-0.1989082,0.0949561,0.8975266,-2.0165100,2.9571533,2.5046082,2.6989011,1.7992674,51.2791209,bw
1549872782.3787096,0.0545098,-0.1186670,1.0347336,-6.3194885,6.6802979,3.9084167,1.7992674,2.0991453,50.9792430,bw

各値は下記のとおりです。

UNIX時間,X軸加速度,Y軸加速度,Z軸加速度,X角速度,Y角速度,Z角速度,X地磁気,Y地磁気,Z地磁気,動作モード

これのデータを元に機械学習して、自分がどのように動作しているのか認識できるよう出来ないかな〜、と漠然と考えています。
データにばらつきが多いので、果たしでできるかどうか分かりませんが。。:sweat_smile:

おわりに

取り急ぎ、自己満足的な備忘録的な内容になっていると思います。
当初の目的が、動作中の挙動データがほしかったので、しばらくはこちらで色々と試して見たいと思います。

5
3
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
3