8速歩行ロボットの歩行プログラム

WebIOPiで動作させるpythonのスクリプトについてメモ。

使用サーボはSG90
使用サーボドライバはPCA9685
足は8本、各足2自由度（水平）

基本的なコード、特にPCA9685のセットとWebIOPiとの連携に関しては、以下の書籍の6足歩行ロボットを作る項のコードを参考にさせていただきました。
連携に関する設定等から初心者向けに詳しく書かれているので、毎度のことながら有難み極まる。

実例で学ぶRaspberry Pi電子工作作りながら応用力を身につける (ブルーバックス)

作者: 金丸隆志
出版社/メーカー: 講談社
発売日: 2015/12/18
メディア: 新書
この商品を含むブログを見る

自分はプログラムには不慣れなため、感覚的にわかりやすいよう各足ごとに動作させるようにしました。
また、パルスではなく±90で動作角度を指定できるように関数を追加。
少し記述が冗長になってしまうけれど、後から調整もしやすいのでいいかな。

出来上がったコードは以下のような感じ。
これをWebIOPiが起動したときに動作するスクリプトとして設定する。
あとで自分が見返す時のためにコメントましましで。

import webiopi
from time import sleep
import smbus
import math
import threading

def resetPCA9685():
    bus.write_byte_data(address_pca9685, 0x00, 0x00)

# setPCA9685Freq、setPCA9685Duty関数は書籍（別記）より
def setPCA9685Freq(freq):
    ・・・

def setPCA9685Duty(channel, on, off):
    ・・・

# 角度指定でサーボを動かす関数
def setPos(channel, zeroOffset, pos):
    #角度をパルスに変換
    #+90〜-90度がパルス143〜410に相当するようなので、1度あたりのパルスは(410-143)/180
    #角度指定の±90に90を足して0〜180に直し、各サーボごとの補正値zeroOffsetを加えて、最終的な角度に相当するパルスを算出する
    pulse = int(round((410-143)/180*(pos+90+zeroOffset)))+143
    setPCA9685Duty(channel, 0, pulse)

# 動作させる足と動かし方を指定するための関数
def setLegServo(leg, order):
    if leg['Side'] == 'right':
        if order == 'f':
            setPos(leg['Channel_h'], leg['ZeroOffset_h'], -pos_f)
        elif order == 'b':
            setPos(leg['Channel_h'], leg['ZeroOffset_h'], pos_b)
        elif order == 'n':
            setPos(leg['Channel_h'], leg['ZeroOffset_h'], pos_n)
        elif order == 'u':
            setPos(leg['Channel_v'], leg['ZeroOffset_v'], -pos_u)
        elif order == 'd':
            setPos(leg['Channel_v'], leg['ZeroOffset_v'], pos_d)
    elif leg['Side'] == 'left':
        if order == 'f':
            setPos(leg['Channel_h'], leg['ZeroOffset_h'], pos_f)
        elif order == 'b':
            setPos(leg['Channel_h'], leg['ZeroOffset_h'], -pos_b)
        elif order == 'n':
            setPos(leg['Channel_h'], leg['ZeroOffset_h'], pos_n)
        elif order == 'u':
            setPos(leg['Channel_v'], leg['ZeroOffset_v'], pos_u)
        elif order == 'd':
            setPos(leg['Channel_v'], leg['ZeroOffset_v'], -pos_d)

# 足一本ごとに動作を指定するための関数
# 一本の足につき複数のサーボを動かす場合は、第二引数に配列で渡す。
def setLeg(leg, orderList):
    if type(orderList) is str:
        setLegServo(leg, orderList)
    elif type(orderList) is list:
        for order in orderList:
            setLegServo(leg, order)

# モーション設定
def forwardPhase1(delay):
    setLeg(leg_r1, ['u', 'f'])
    setLeg(leg_r3, ['u', 'f'])
    setLeg(leg_l1, ['u', 'f'])
    setLeg(leg_l3, ['u', 'f'])
    setLeg(leg_r2, ['b'])
    setLeg(leg_r4, ['b'])
    setLeg(leg_l2, ['b'])
    setLeg(leg_l4, ['b'])
    sleep(delay)
    setLeg(leg_r1, ['d'])
    setLeg(leg_r3, ['d'])
    setLeg(leg_l1, ['d'])
    setLeg(leg_l3, ['d'])
    sleep(delay)

def forwardPhase2(delay):
    setLeg(leg_r2, ['u', 'f'])
    setLeg(leg_r4, ['u', 'f'])
    setLeg(leg_l2, ['u', 'f'])
    setLeg(leg_l4, ['u', 'f'])
    setLeg(leg_r1, ['b'])
    setLeg(leg_r3, ['b'])
    setLeg(leg_l1, ['b'])
    setLeg(leg_l3, ['b'])
    sleep(delay)
    setLeg(leg_r2, ['d'])
    setLeg(leg_r4, ['d'])
    setLeg(leg_l2, ['d'])
    setLeg(leg_l4, ['d'])
    sleep(delay)

# 前進
def forwardMove(delay):
    forwardPhase1(delay)
    forwardPhase2(delay)

def backwardPhase1(delay):
    setLeg(leg_r1, ['u', 'b'])
    setLeg(leg_r3, ['u', 'b'])
    setLeg(leg_l1, ['u', 'b'])
    setLeg(leg_l3, ['u', 'b'])
    setLeg(leg_r2, ['f'])
    setLeg(leg_r4, ['f'])
    setLeg(leg_l2, ['f'])
    setLeg(leg_l4, ['f'])
    sleep(delay)
    setLeg(leg_r1, ['d'])
    setLeg(leg_r3, ['d'])
    setLeg(leg_l1, ['d'])
    setLeg(leg_l3, ['d'])
    sleep(delay)

def backwardPhase2(delay):
    setLeg(leg_r2, ['u', 'b'])
    setLeg(leg_r4, ['u', 'b'])
    setLeg(leg_l2, ['u', 'b'])
    setLeg(leg_l4, ['u', 'b'])
    setLeg(leg_r1, ['f'])
    setLeg(leg_r3, ['f'])
    setLeg(leg_l1, ['f'])
    setLeg(leg_l3, ['f'])
    sleep(delay)
    setLeg(leg_r2, ['d'])
    setLeg(leg_r4, ['d'])
    setLeg(leg_l2, ['d'])
    setLeg(leg_l4, ['d'])
    sleep(delay)

# 後退
def backwardMove(delay):
    backwardPhase1(delay)
    backwardPhase2(delay)

def rotateRightPhase1(delay):
    setLeg(leg_r1, ['u', 'b'])
    setLeg(leg_r3, ['u', 'b'])
    setLeg(leg_l1, ['u', 'f'])
    setLeg(leg_l3, ['u', 'f'])
    setLeg(leg_r2, ['f'])
    setLeg(leg_r4, ['f'])
    setLeg(leg_l2, ['b'])
    setLeg(leg_l4, ['b'])
    sleep(delay)
    setLeg(leg_r1, ['d'])
    setLeg(leg_r3, ['d'])
    setLeg(leg_l1, ['d'])
    setLeg(leg_l3, ['d'])
    sleep(delay)

def rotateRightPhase2(delay):
    setLeg(leg_r2, ['u', 'b'])
    setLeg(leg_r4, ['u', 'b'])
    setLeg(leg_l2, ['u', 'f'])
    setLeg(leg_l4, ['u', 'f'])
    setLeg(leg_r1, ['f'])
    setLeg(leg_r3, ['f'])
    setLeg(leg_l1, ['b'])
    setLeg(leg_l3, ['b'])
    sleep(delay)
    setLeg(leg_r2, ['d'])
    setLeg(leg_r4, ['d'])
    setLeg(leg_l2, ['d'])
    setLeg(leg_l4, ['d'])
    sleep(delay)

# 右旋回
def rotateRightMove(delay):
    rotateRightPhase1(delay)
    rotateRightPhase2(delay)

def rotateLeftPhase1(delay):
    setLeg(leg_r1, ['u', 'f'])
    setLeg(leg_r3, ['u', 'f'])
    setLeg(leg_l1, ['u', 'b'])
    setLeg(leg_l3, ['u', 'b'])
    setLeg(leg_r2, ['b'])
    setLeg(leg_r4, ['b'])
    setLeg(leg_l2, ['f'])
    setLeg(leg_l4, ['f'])
    sleep(delay)
    setLeg(leg_r1, ['d'])
    setLeg(leg_r3, ['d'])
    setLeg(leg_l1, ['d'])
    setLeg(leg_l3, ['d'])
    sleep(delay)

def rotateLeftPhase2(delay):
    setLeg(leg_r2, ['u', 'f'])
    setLeg(leg_r4, ['u', 'f'])
    setLeg(leg_l2, ['u', 'b'])
    setLeg(leg_l4, ['u', 'b'])
    setLeg(leg_r1, ['b'])
    setLeg(leg_r3, ['b'])
    setLeg(leg_l1, ['f'])
    setLeg(leg_l3, ['f'])
    sleep(delay)
    setLeg(leg_r2, ['d'])
    setLeg(leg_r4, ['d'])
    setLeg(leg_l2, ['d'])
    setLeg(leg_l4, ['d'])
    sleep(delay)

# 左旋回
def rotateLeftMove(delay):
    rotateLeftPhase1(delay)
    rotateLeftPhase2(delay)

# 停止
def stopMove(delay):
    setLeg(leg_r1, ['n', 'd'])
    setLeg(leg_r2, ['n', 'd'])
    setLeg(leg_r3, ['n', 'd'])
    setLeg(leg_r4, ['n', 'd'])
    setLeg(leg_l1, ['n', 'd'])
    setLeg(leg_l2, ['n', 'd'])
    setLeg(leg_l3, ['n', 'd'])
    setLeg(leg_l4, ['n', 'd'])
    sleep(delay)

def processCommands():
    global status

    while True:
        if status == 'i':
            sleep(delay)
        elif status == 'f':
            forwardMove(delay)
        elif status == 'b':
            backwardMove(delay)
        elif status == 'r':
            rotateRightMove(delay)
        elif status == 'l':
            rotateLeftMove(delay)
        elif status == 's':
            stopMove(delay)
            status = 'i'
        elif status == 'k':
            break

# 各足に対応する変数に、向き、サーボのチャンネル、角度補正値を連想配列で格納
# サーボは取り付けるときに0度位置に合わせるが、少しずれるのでここで補正する
# 水平方向に動かすサーボはCannel_h, ZeroIffset_h, 垂直方向に動かすサーボはChannel_v, ZeroOffset_vをそれぞれキーにする
leg_r1 = {'Side' : 'right', 'Channel_h' : 0, 'ZeroOffset_h' : -5, 'Channel_v' : 1, 'ZeroOffset_v' : 5}
leg_r2 = {'Side' : 'right', 'Channel_h' : 2, 'ZeroOffset_h' : 10, 'Channel_v' : 3, 'ZeroOffset_v' : -5}
leg_r3 = {'Side' : 'right', 'Channel_h' : 4, 'ZeroOffset_h' : 0, 'Channel_v' :5 , 'ZeroOffset_v' :0 }
leg_r4 = {'Side' : 'right', 'Channel_h' : 6, 'ZeroOffset_h' :0 , 'Channel_v' :7 , 'ZeroOffset_v' :-5 }
leg_l1 = {'Side' : 'left', 'Channel_h' : 8, 'ZeroOffset_h' :0, 'Channel_v' :9 , 'ZeroOffset_v' :-10 }
leg_l2 = {'Side' : 'left', 'Channel_h' : 10, 'ZeroOffset_h' : -5, 'Channel_v' :11 , 'ZeroOffset_v' :-5 }
leg_l3 = {'Side' : 'left', 'Channel_h' : 12, 'ZeroOffset_h' : -5, 'Channel_v' :13 , 'ZeroOffset_v' :5 }
leg_l4 = {'Side' : 'left', 'Channel_h' : 14, 'ZeroOffset_h' : 0, 'Channel_v' :15 , 'ZeroOffset_v' :5 }

# 各動作に応じたサーボの動作角度設定
# 水平方向 n: neutral, f: forward, b: backward
# 垂直方向 u: up, d: down
pos_n = 0
pos_f = 18
pos_b = 18
pos_u = 30
pos_d = 0

delay = 0.15

# i:idle, f: forward, b: backward, r: right, l: left, s: stop, k: kill
status = 'i'

bus = smbus.SMBus(1)
address_pca9685 = 0x40

resetPCA9685()
setPCA9685Freq(50)

stopMove(delay)

t = threading.Thread(target=processCommands)
t.start()

# デバッグ出力を有効に
webiopi.setDebug()

# WebIOPiの起動時に呼ばれる関数
def setup():
    webiopi.debug("Script with macros - Setup")

# WebIOPiにより繰り返される関数
def loop():
    webiopi.sleep(5)

# WebIOPi終了時に呼ばれる関数
def destroy():
    webiopi.debug("Script with macros - Destroy")
    global status
    status = 'k'
    t.join()

@webiopi.macro
def setLegsAction(action, commandID):
    global status
    status = action

足を運ぶ順番や角度の指定を少し変えると、また違った動きになって楽しい。単なる前進でも足運びの方法が多数あるのも、多足ロボットならではの面白さかも。

なお、他の一般的なサーボは動作角度が+-逆らしい。Mk-Ⅱを作るときは気をつける。

つづく。