Rover C ProをRaspberryPiとObnizで動かしてみる(その1)
- 2021.05.05
- IoT

アメコミ好きな方ならその傍に出てくるガジェットに一度はワクワクした人も多いんじゃないでしょうか.
私もその一人です.
そこで現在、小型で自由自在に遠隔操作できるラジコンを制作しております.
狭かった門も開かれ手軽にガジェットが作れるこの時代、先人の知恵なども駆使していけばどうにか作ることができるんじゃないかと奮闘した過程をこれからこの場を使ってまとめていく予定です.(作業中のメモを備忘録としてブログにまとめていきます.)
仕様
スマホからRover C Proを遠隔操作します.
具体的にはサーバー経由でObnizを動かし、Rover C Proを接続したラズパイとBluetoothで接続します.
ラズパイとRover C ProはI2C通信でやり取りを行い、Obnizから受け取ったデータをラズパイ側で書込用に変換した値を元にRoverCが動き回ります.
また、屋外からだと状況が分からないのでカメラをラズパイかRover C Proに直接装備させ、FPS視点で操作できるようにもする予定です.
今回すべてやると長くなりすぎるので、まずはラズパイからRoverCを動かすとこまでをまとめていきます.
機材
今回の記事で扱うものを載せます.
Obniz:https://obniz.com/ja/products/obnizboard
Raspberry pi:zero:https://www.switch-science.com/catalog/3200/
Rover C Pro:https://www.switch-science.com/catalog/6617/
Rover C Proセット内容
本体、アーム(サーボモーター)、取り付け用ネジ等細かいパーツが付属.
箱も小さいが、内容もかなりシンプルです.

本来はM5Stickを取り付けて動かすものですが、製造元ページ(https://docs.m5stack.com/#/en/hat/hat_roverc_pro)をみる感じ、I2Cで動かせるっぽいのでラズパイでも何とかなるはず.
ということでラズパイとワイヤーで無理やり繋いでみた↓

Rover Cのピン配置はM5Stickと同じ(下記画像上部)です.
(以下から拝借)
https://lang-ship.com/reference/unofficial/M5StickC/

I2C(アイスクエアドシー)
デバイスを制御するマスター(今回の場合だとラズパイ)と、マスターからの指示で制御される側をスレーブ(RoverCPro)を繋ぐことでお互いにデータのやり取りが可能となるシリアル通信方式の一種.
データのやり取りを行うSDA(シリアルデータ)、通信のタイミングを測るSCL (シリアルクロック)、そして5V、GNDの4本を繋ぐことが基本となる.
ラズパイからスレーブ側のレジスタにデータを書き込むことでモーターを回したり、また、別の端末の話になるが温度センサーから温度を読み取ったりとできるみたい.概要だけ知ると簡単にできそうに思えてしまう.
また、繋ぐ際には識別用のI2Cアドレスも必要となる.ここのアドレスを指定することで対象の機器と通信ができる.
これはRover Cの製造元ページから「0x38」であることを確認したが、ラズパイからコマンドでも確認できるみたいなので、せっかくならこちらでも確認したいところ.

ラズパイからスレーブ側のI2Cアドレスを調べる
製造元のページからでも確認できそうだけど興味本位でやってみます.
「i2cdetect 1」で接続されているI2Cデバイスのアドレスが表示されます.(引数の1はラズパイのI2Cのチャンネル番号で固定です)
下記が実行結果です.「0x38」と「0x1a」が見つかりました.
前述の通り、モーター制御は「0x38」から行えます.「0x1a」は現時点では分かりません…

I2Cアドレスから更にレジスタを調べる
こちらも興味本位.
「i2cdump 1 0x38」で調べてみます.

上図の赤枠を見て、その足した合計がアドレスになります(上記だと21).
モーターは「0〜3」のアドレスを書き換えることで制御できます.
ちなみにサーボモーターは「10」と「11」で制御可能です.
実際に通信してみる
まずはモーターを動かしてみます.
ここからはラズパイ側でPythonで行っていきます.
結論から申し上げますと以下のコードで直接モーターを回すことができました.(-127~127の範囲で16進数指定)
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
import wiringpi as pi #I2C通信用のインスタンスを生成 i2c = pi.I2C() # 通信対象のI2Cアドレスをセット roverC = i2c.setup(0x38) data = i2c.readReg8(roverC, 0x00) print("data :" + str(data)) # 左前 (7f = 127) i2c.writeReg8(roverC, 0x00, 0x7f) # 右前 (00 = 0) i2c.writeReg8(roverC, 0x01, 0x00) # 左後 (9c = -127) i2c.writeReg8(roverC, 0x02, 0x9c) # 右後 i2c.writeReg8(roverC, 0x03, 0x00) |
また、以下のように書き込むことでサーボが制御できました.(0~180(16進数で00~b4)を指定)
1 2 3 4 |
#左サーボ i2c.writeReg8(roverC, 0x10, 0x64) #右サーボ i2c.writeReg8(roverC, 0x11, 0x64) |
以下の赤丸の所が「0x10」レジスタで制御できる端子です.

とりあえずモーターの制御方法さえわかれば形にはできそうなので、ラズパイ4での検証はここまで.
あとはこちらのPC(vscode)でプログラムを書き、本番環境であるラズパイzeroにて動かし、まずは原型を作っていきたいです.(カメラなどはそのあと)
メカナムホイール
車輪に更に車輪がついています.
この4つのホイールの正転、逆転を組み合わせることで四方八方滑らかに動かすことが可能になります.

以下は旧型RoverC(https://docs.m5stack.com/#/en/hat/hat-roverc)の製造元ページからお借りしました.
各ホイールの動きに対する本体の進む方向がまとめられています.

その他参考にしたサイトを以下に載せます
・メカナムホイール制御方法
https://qiita.com/coppercele/items/1596b7b9904eb4403191
・RoverC(旧型)
https://docs.m5stack.com/#/en/hat/hat-roverc
ファイルをラズパイ側に転送する
ラズパイzero単体で開発をするのは面倒なのでPCでプログラムを作ってからラズパイに送る形式としたい.
Macからラズパイへ送る場合は以下のようにscpコマンドで送る.
$ scp ファイルパス pi@ラズパイのアドレス:/home/pi/
(/home/piは送り先)
PC側のテキストエディターでプログラムを編集→ラズパイにscpコマンドで送信→以後はコマンド履歴から簡単に送れるのであまり手間もなくラズパイ側へ変更が反映できるようになる.
※ちなみにWindowsから送りたい場合はTeratermでSSH接続をした後、そのコマンドプロンプトに向かってファイルをドラッグ&ドロップするだけなので非常に簡単.
ラズパイで動かしてみる
まずはコードから.
こちらは公式のGithub(https://github.com/m5stack/M5StickC/blob/master/examples/KIT/JoyC_%26_RoverC/Remote/Remote.ino)とMoke Nakamura様(https://qiita.com/coppercele/items/1596b7b9904eb4403191)のコードを参考にしてPython向けに書いてみました.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 |
import wiringpi as pi import time # 進む向きに対する左前、右前、左後、右後4つの車輪のエネルギーを配列として保持 FORWARD = [50, 50, 50, 50] LEFT = [-50, 50, 50, -50] BACKWARD = [-50, -50, -50, -50] RIGHT = [50, -50, -50, 50] ROTATE_R = [-25, 25, -25, 25] ROTATE_L = [25, -25, 25, -25] #左前、右前、左後、右後のホイールのスピードを保持 speed = [0,0,0,0] #コントローラーの入力係数 f = b = l = r = rr = rl = 0.0 #I2C通信用のインスタンスを生成 i2c = pi.I2C() # 通信対象のI2Cアドレスをセット roverC = i2c.setup(0x38) def main(): data = i2c.readReg8(roverC, 0x00) print("data :" + str(data)) testMove() #動作テスト def testMove(): global f global b global l global r global rl global rr # 前進 f = 1.0 setSpeed() time.sleep(2) # 左平行移動 f = 0.0 l = 1.0 setSpeed() time.sleep(2) # 後退 l = 0.0 b = 1.0 setSpeed() time.sleep(2) # 右平行移動 b = 0.0 r = 1.0 setSpeed() time.sleep(2) # 停止 r = 0.0 setSpeed() time.sleep(2) # 右ナナメ前移動 f = 0.5 r = 0.5 setSpeed() time.sleep(2) # 右斜め後ろ移動 f = 0.0 r = 0.5 b = 0.5 setSpeed() time.sleep(2) # 左斜め後ろ移動 r = 0.0 l = 0.5 b = 0.5 setSpeed() time.sleep(2) # 左斜め前移動 f = 0.5 l = 0.5 b = 0.0 setSpeed() time.sleep(2) # 右回転 f = 0.0 l = 0.0 rr = 1.0 setSpeed() time.sleep(2) # 左回転 rr = 0.0 rl = 1.0 setSpeed() time.sleep(2) # 停止 rl = 0.0 setSpeed() time.sleep(2) #i2c書き込み def i2cWrite(data): # 左前 (7f = 127) i2c.writeReg8(roverC, 0x00, data[0]) # 右前 (00 = 0) i2c.writeReg8(roverC, 0x01, data[1]) # 左後 (9c = -127) i2c.writeReg8(roverC, 0x02, data[2]) # 右後 i2c.writeReg8(roverC, 0x03, data[3]) print("i2c_data :" + str(data)) def setSpeed(): #各車輪のスピードを割り出す for index, item in enumerate(speed): print("FORWARD :" + str(FORWARD[index])) speed[index] = FORWARD[index] * f speed[index] += BACKWARD[index] * b speed[index] += RIGHT[index] * r speed[index] += LEFT[index] * l speed[index] += ROTATE_R[index] * rl speed[index] += ROTATE_L[index] * rr print("speed" + "[" + str(index) + "]:" + str(speed[index])) #スピードが100を越えないように丸め込む speed[index] = speedParameter(speed[index]) print("speed:" + str(speed)) speed_hex = [int(i) for i in speed] i2cWrite(speed_hex) #スピードを調整できるように10進数→16進数の範囲に丸め込む def speedParameter(speed): rawSpeed = 0 if (speed == 0): rawSpeed = 0; # stop motor elif (speed > 100): rawSpeed = 100 console.log("speed" + rawSpeed) elif (speed < -100): rawSpeed = -100 console.log("speed" + rawSpeed) else: rawSpeed = speed return rawSpeed #return parseInt32ToByte(rawSpeed, 1)[0] if __name__ == "__main__": main() |
前述したようにラズパイとRoverCを接続し、上記のコードをラズパイで実行することで動き始めます.
コード上のグローバル変数であるf,b,l,rが操作スティックの倒し具合(0.0〜1.0)をfloat型で持っています.
また、右左回転はrr,rlです.
これと 進む向きに対する左前、右前、左後、右後4つの車輪のエネルギーをまとめた「FORWARD」、「LEFT」、「BACKWARD」、[RIGHT」、「ROTATE_R」、[ROTATE_L」を組み合わせて進む向きとスピードを計算します.
以下に簡素ではありますが計算方法を図にしてみました。
図の通りだと、右前輪と左後輪を正転することで左斜め前に進めることがわかります.

前の方でも載せましたが、こちらの画像と比べると更にわかりやすいと思います.
(https://docs.m5stack.com/#/en/hat/hat-roverc)

そしてこちらが実行結果です.
前進、左平行、後進、右平行、停止、右斜前、右斜後、左斜後、左斜前、右回転、左回転、停止の順番で動作する.
今回はRoverC搭載のバッテリーでこのように(運良く?)動かすことができたものの、これ以降はラズパイへの給電不足が原因かうまく動かなくなった.
なのでラズパイ用に別途バッテリーを準備した方が良さそう.
終わりに
今回はラズパイとRoverCでの検証のみとした.
ラズパイ用の外部電源はすぐには準備できないのでしばらくはUSBからの供給で賄う.
次回からはObnizから操作できるようにする予定です.
-
前の記事
RETROFLAG Raspberry Pi 4 NESPi 4ケース 2020.12.12
-
次の記事
【Rover C ProをObnizで動かす】遠隔操作できる自宅監視カメラ(BLE) 2021.05.06