ロボットおもちゃフォロをRaspberry pi with ROSで遠隔操作する。

フォロは安価で手に入る、ロボットおもちゃです。
歩行の機構はよく考えられていてとてもおもしろいのですが、
実際の制御はモータ2個を動かしているだけなので、
今回は電子工作でROSによって動作するロボットに仕立て上げようと思います。
ROSで動かせると、ROSの様々なツールを利用して容易にロボットのシステムを拡張することができます。
今回はパソコン側につないだジョイスティックでフォロを遠隔操作をできるように設計したので、
備忘録に書いておくことにしました。

#必要なもの
- フォロ本体(https://www.biccamera.com/bc/item/5103296/)
- Raspberry pi zero一式(本体に加えて、micro SDとACアダプター)
- モータドライバ×2(https://toshiba.semicon-storage.com/jp/product/linear/motordriver/detail.TA7267BP.html)*-

#前提
パソコンROSがインストールされていて、raspberry piとROSによる通信ができる状態にある。

#Raspberry pi zeroのセットアップ

次を参照(https://www.shumi-tech.online/entry/2018/09/24/%E3%80%90%E3%83%A9%E3%82%BA%E3%83%91%E3%82%A4%E5%88%9D%E5%BF%83%E8%80%85%E5%90%91%E3%81%91%E3%80%91Raspberry_Pi_Zero_W_%E3%82%BB%E3%83%83%E3%83%88%E3%82%A2%E3%83%83%E3%83%97%E6%96%B9%E6%B3%95%EF%BC%8820) (http://robonchu.hatenablog.com/entry/2018/07/21/094208)

#電子回路のセットアップ
![foro_raspberry_pi_ブレッドボード.png](https://qiita-image-store.s3.amazonaws.com/0/310639/68649284-2db7-637a-96d6-4ab486ee7b54.png)

モータドライバのピン番号は文字が書いてある左側から順番に読みます。
1:入力1,2:入力2,3:出力1,4:GND,5:出力2,6:電源1,7:電源2となっています。(https://toshiba.semicon-storage.com/jp/product/linear/motordriver/detail.TA7267BP.html)
モータドライバの入力(In1,In2)×2合計4本にraspberry pi zeroの信号ピンをつなぎ、
出力はそれぞれ、モータの正負をつなげます。
電源部分は電源1をraspberry piの電源に、電源2を電池に、GNDをraspberry piと電池のGNDにつなぎます。
この電源周りの回路はモータドライバ2個が並列になるような構成になります。

このままだと、raspberry pi zero への電源供給が無いままなので、別電源でraspberry pi zeroに電源供給を行う必要があります。
動作を試すだけなら、ACアダプターから直接電源を供給し、完全コードレスで動かすときは、別にバッテリー用意してraspberry pi zeroを動作させてください。

#ソフトウェア側のセットアップ

今回はROSを利用して動かします。作成したプログラムは次のようなsubscriberのノードです。

```python:foloros.py
#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Joy
import time
import pigpio

class Folo:
    def __init__(self):
        rospy.init_node('folo',anonymous=True)
        self.subscriber = rospy.Subscriber("/joy",Joy,self.callback)
        self.pi = pigpio.pi()
    	self.pi.set_mode(14,pigpio.OUTPUT)
    	self.pi.set_mode(15,pigpio.OUTPUT)
    	self.pi.set_mode(17,pigpio.OUTPUT)
    	self.pi.set_mode(18,pigpio.OUTPUT)
        self.con=0
        rospy.spin()
    def callback(self,ros_data):
        if(ros_data.buttons[0]==1):
            self.pi.write(14,1)
            self.pi.write(15,0)
        elif(ros_data.buttons[1]==1):
            self.pi.write(14,0)
            self.pi.write(15,1)
        if(ros_data.buttons[2]==1):
            self.pi.write(17,1)
            self.pi.write(18,0)
        elif(ros_data.buttons[3]==1):
            self.pi.write(17,0)
            self.pi.write(18,1)
	if(ros_data.buttons[4]==1):
	    self.pi.write(14,0)
	    self.pi.write(15,0)
	    self.pi.write(17,0)
	    self.pi.write(18,0)
        time.sleep(0.1)
if __name__=='__main__':
    print("start")
    folo = Folo()
```

ボタン0~4で前後移動、左右旋回、停止が割り当てられています。

###動作確認
subscriberノードにジョイスティックのメッセージを送って、フォロを動作させます。
そのために、ホスト側で次のようにroscoreとjoyノードを動作させます。

```
roscore
rosrun joy joy_node
```
参考(http://wiki.ros.org/joy)

raspberry pi zero側では先ほど作成したsubscriberノードを動作させます。


```
sudo pigpiod
rosrun Folo foloros.py
```
参考(https://qiita.com/yuuri23/items/597fd1a40c63627e59c2)
事前にpigpiodを起動しておかないと、作成したノードが動かないので注意してください。
また最後のコマンドは

```
rosrun 作成したパッケージの名前 pythonファイルの名前
```
となっていて、今回は詳細を省いていますが、FoloはROSパッケージの名前です。

うまくいくと次のように動作するはずです。
<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">お安いロボットが売ってたので改造してraspberry pi with ROSで動かしてみた。 <a href="https://t.co/xjKl3onJfv">pic.twitter.com/xjKl3onJfv</a></p>&mdash; ろくろ (@colocolorobo) <a href="https://twitter.com/colocolorobo/status/1066599257426128897?ref_src=twsrc%5Etfw">2018年11月25日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>