Rover C ProをRaspberryPiとObnizで動かしてみる(その１)

2021.05.05
IoT

アメコミ好きな方ならその傍に出てくるガジェットに一度はワクワクした人も多いんじゃないでしょうか.
私もその一人です.
そこで現在、小型で自由自在に遠隔操作できるラジコンを制作しております.
狭かった門も開かれ手軽にガジェットが作れるこの時代、先人の知恵なども駆使していけばどうにか作ることができるんじゃないかと奮闘した過程をこれからこの場を使ってまとめていく予定です.(作業中のメモを備忘録としてブログにまとめていきます.)

1. 仕様
2. 機材
3. Rover C Proセット内容
4. I2C(アイスクエアドシー)
5. ラズパイからスレーブ側のI2Cアドレスを調べる
6. I2Cアドレスから更にレジスタを調べる
7. 実際に通信してみる
8. メカナムホイール
9. ファイルをラズパイ側に転送する
10. ラズパイで動かしてみる
11. 終わりに

仕様

スマホから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の４本を繋ぐことが基本となる.

ラズパイからスレーブ側のレジスタにデータを書き込むことでモーターを回したり、また、別の端末の話になるが温度センサーから温度を読み取ったりとできるみたい.概要だけ知ると簡単にできそうに思えてしまう.

また、繋ぐ際には識別用の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進数指定)

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)

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)を指定)

#左サーボ
i2c.writeReg8(roverC, 0x10, 0x64)
#右サーボ
i2c.writeReg8(roverC, 0x11, 0x64)

#左サーボ

i2c.writeReg8(roverC, 0x10, 0x64)

#右サーボ

i2c.writeReg8(roverC, 0x11, 0x64)

以下の赤丸の所が「0x10」レジスタで制御できる端子です.

とりあえずモーターの制御方法さえわかれば形にはできそうなので、ラズパイ４での検証はここまで.
あとはこちらのPC(vscode)でプログラムを書き、本番環境であるラズパイzeroにて動かし、まずは原型を作っていきたいです.(カメラなどはそのあと)

メカナムホイール

車輪に更に車輪がついています.
この４つのホイールの正転、逆転を組み合わせることで四方八方滑らかに動かすことが可能になります.

以下は旧型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向けに書いてみました.

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()

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」を組み合わせて進む向きとスピードを計算します.
以下に簡素ではありますが計算方法を図にしてみました。
図の通りだと、右前輪と左後輪を正転することで左斜め前に進めることがわかります.