M5stackで2足歩行ロボットの作成

elchikaさんのハードウェアキャンペーンで頂きましたM5Stackにて、簡易的ではありますが個人的に非常に興味のある「2足歩行ロボット」を初めて作成しました。

本格的なロボットとは程遠い内容ですが、少しでも何かの役に立てたなら幸いです。

大まかな構成としては、M5Stackにwebサーバーを立てて、そこにブラウザでアクセスしてサーボモーターを操作する。です。

動作する流れとしては、下図のような感じです。

pythonだけで「WIFI AP」「webサーバー」「操作画面」「サーボモーター制御」を完結させます。

サーボモーターの制御には、ちょうど手元にpca9685があったので、それを使用する事にしました。

M5stackからはI2Cの半二重通信の接続を行って、pca9685ドライバを制御します。

ロボット自体は手乗りサイズ程度の大きさで、足は片方2個ずつ４つのサーボを制御で、手は無しで実装します。

適当な段ボールで見た目を設計します。

M5stackの画面は段ボールをくりぬいて見えるようにしてみます。

足のサーボモーターと胴体は結束バンドやボンド等で固定し、金具は、その辺にある使えそうな適当なものを見繕って実装します。（テキトーですいません。。）


main.py
import network
import machine
import time
import socket
import re
from machine import Pin,I2C
import servo
import pca9685
from m5stack import lcd

ESSID = 'M5stack' 
PASSWORD = '11223344'
IP = '192.168.5.1'

i2c = I2C(0, scl=Pin(22), sda=Pin(21))
sev = servo.Servos(i2c)

sv ="p"

#目と口の位置
posi = {"lx":90, "ly":90, "lr":10, "rx":230, "ry":90, "rr":10, "ux":110, "uy":170}

lcd.clear(lcd.BLACK)

#顔の表示
def nomale_lcd():
    lcd.circle(posi["lx"], posi["ly"], posi["lr"], lcd.WHITE, lcd.WHITE)
    lcd.circle(posi["rx"], posi["ry"], posi["rr"], lcd.WHITE, lcd.WHITE)
    lcd.rect(posi["ux"], posi["uy"], 100, 5, lcd.WHITE, lcd.WHITE)
    
#サーボモーター制御
def servoset():
    global sv
    if sv == "st":
        lcd.rect(posi["ux"], posi["uy"], 100, 30, lcd.WHITE, lcd.BLACK) #口空ける
        sev.position(0, 97)
        time.sleep_ms(1000)
        sev.position(1, 90)
        time.sleep_ms(1000)
        sev.position(2, 100)
        time.sleep_ms(1000)
        sev.position(3, 90)
        time.sleep_ms(1000)
    elif sv == "fw":
        lcd.rect(posi["ux"], posi["uy"], 100, 30, lcd.WHITE, lcd.BLACK)
        sev.position(3, 70)
        sev.position(1, 70)
        time.sleep_ms(500)
        sev.position(0, 77)
        sev.position(2, 80)
        time.sleep_ms(500)
        sev.position(3, 90)
        sev.position(1, 90)
        time.sleep_ms(500)
        sev.position(3, 110)
        sev.position(1, 110)
        time.sleep_ms(500)
        sev.position(0,117)
        sev.position(2, 120)
        time.sleep_ms(500)
        sev.position(3, 90)
        sev.position(1, 90)
        time.sleep_ms(500)
    elif sv == "lt":
        lcd.rect(posi["ux"], posi["uy"], 100, 30, lcd.WHITE, lcd.BLACK)
        sev.position(3, 40)
        sev.position(1, 60)
        time.sleep_ms(500)
        sev.position(0, 117)
        time.sleep_ms(500)
        sev.position(3, 90)
        time.sleep_ms(500)
        sev.position(1, 90)
        time.sleep_ms(500)
        sev.position(0, 97)
        time.sleep_ms(500)
    elif sv == "bk":
        lcd.rect(posi["ux"], posi["uy"], 100, 30, lcd.WHITE, lcd.BLACK)
        sev.position(3, 110)
        sev.position(1, 110)
        time.sleep_ms(500)
        sev.position(0, 77)
        sev.position(2, 80)
        time.sleep_ms(500)
        sev.position(3, 90)
        sev.position(1, 90)
        time.sleep_ms(500)
        sev.position(3, 70)
        sev.position(1, 70)
        time.sleep_ms(500)
        sev.position(0,117)
        sev.position(2, 120)
        time.sleep_ms(500)
        sev.position(3, 90)
        sev.position(1, 90)
        time.sleep_ms(500)
    elif sv == "rt":
        lcd.rect(posi["ux"], posi["uy"], 100, 30, lcd.WHITE, lcd.BLACK)
        sev.position(3, 120)
        sev.position(1, 140)
        time.sleep_ms(500)
        sev.position(2, 80)
        time.sleep_ms(500)
        sev.position(1, 90)
        time.sleep_ms(500)
        sev.position(3, 90)
        time.sleep_ms(500)
        sev.position(2, 100)
        time.sleep_ms(500)

#html 
def home_page():
    html = """<html lang="ja">
    <head>
    <meta charset="utf-8">
    <title>サーボテスト</title>
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <style>
    * { 
    margin: 0px; 
    padding: 0px; 
    }
    body {
    max-width: 600px;
    font-size: 25px;
    width: 100%;
    }
    main {
    height: 40vh;
    background: skyblue;
    }
    ul {
    display: block;
    height: 40vh;
    list-style: none;
    padding-top: 10px;
    }
    .bc {
    display: flex;
    }
    .a, .bc, .d {
    height: 12vh;
    }
    li {
    width: 100px ;
    height: 90% ;
    margin-left: auto;
    margin-right: auto;
    background: yellow;    
    }
    .b {
    margin-left: auto;
    }
    .c {
    margin-right: auto;
    }
    ul li {
    text-align: center;
    }
    .a li, .b li, .c
    li, .d li {
    border: solid 1px;
    }
    .n li {
    background: yellow;
    }
    a:active {
    color: #ff2020;
    }
    .a li, .b li, .c li, .d li, .n li {
    border: solid 1px;
    }
    .n {
    margin-right: 3px;
    margin-left: 3px;
    }
    </style>
    </head>　　　　　　　　　　
    <body>
    <main>
    <ul>
    <div class="a">
    <a href="/svm_aa">
    <li id="forward">前進</li>
    </a></div>
    <div class="bc">
    <div class="b">
    <a href="/svm_bb">
    <li id="left">左旋回</li>
    </a></div>
    <div class="n">
    <a href="/svm_ee">
    <li id="set">SET</li>
    </a></div>
    <div class="c">
    <a href="/svm_cc">
    <li id="right">右旋回</li>
    </a></div>
    </div>
    <div class="d">
    <a href="/svm_dd">
    <li id="backward">後退</li>
    </a></div>
    </ul>
    </main>
    </body>
    </html>"""
    return html     #テキストとして値を返す

nomale_lcd()

#WIFI APの立ち上げ       
ap = network.WLAN(network.AP_IF)
ap.config(essid=ESSID, authmode=3, password=PASSWORD)
ap.ifconfig((IP,'255.255.255.0',IP,'8.8.8.8'))
ap.active(True)  
print("AP OK")

#WEBサーバーの立ち上げ
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('', 80))
s.listen(5)
 
#クライアント接続
while True:
    conn, addr = s.accept()
    request = str(conn.recv(1024))
    m = re.search(r'svm_(\D\D\s)', request)
    #print(request)
  
#ボタン操作反映
    if m != None :
        svm = m.group(0)       
        if svm == 'svm_aa ':
            sv = "fw"
        elif svm == 'svm_bb ':
            sv = "lt"    
        elif svm == 'svm_cc ':
            sv = "rt"
        elif svm == 'svm_dd ':
            sv = "bk"
        elif svm == 'svm_ee ':
            sv = "st"
            
    servoset()       #サーボ関数呼び出し
    sv ="p"
    lcd.clear(lcd.BLACK)
    nomale_lcd()
    response = home_page()    #html text格納
    conn.send(response)       #クライアントへhtmlデータ送信
    conn.close()              #scket破棄
    print("Data Received")

Thonny IDE等で、コーディング＋デバッグ＋M5Stackのフラッシュにファイルの書き込みが出来ます。

プログラムをさらっと順に説明していきます。

ネットワーク部分の記述は、まずM5stackのアクセスポイントのSSID,pass,IPを変数に格納しています。

html pageの関数は、テキストとしてhtmlの変数に格納し、それを返します。

WIFI APの立ち上げは、ap.configでssid,暗号方式,pass,を設定し、ipconfigでサーバ側のIP,サブネットマスク,デフォルトゲートウェイ,DNS,を設定できます。ap.activeで立ち上がります。

WEBサーバーの立ち上げのプログラムについては、socket.socketの第一引数は「ipv4」第二引数は「TCP接続」を表しています。s.bindの引数は、''が全てのクライアントを受け付ける意味で、第二引数はポート番号になります。s.listunは、クライアントの同時接続可能数になります。

クライアント接続については、s.acceptで待機し、接続後、while文に入ります。

クライアントがサーバーに接続すると、s.acceptでクライアント用のソケットとIPアドレスをconn,addrに返します。requestにはクライアントからのhttp接続のheader情報が入ります。search(r'svm_(\D\D\s)', request)でheader情報からsvm_(\D\D\s)の正規表現された文字列に該当するものを検索しています。

ボタン操作反映では、先ほど検索した文字列で上下左右、もしくは何もしない、の判定をしています。

最終的にソケットは破棄され、再度接続を行うと（今回で言えばボタンを押すと）s.acceptからの実行となります。

あとはコメント通りです。

サーボモーターが動く流れとしては、サーバー接続後、ブラウザで操作画面（html）の任意のボタンをクリックすると、URLにsvm_○○のパラメータが乗り、再度ループ処理を開始します。正規表現された文字列がボタン操作反映部のどこかの条件に当てはまるので、svの値が変更されます。サーボ関数が実効されると、svの値に応じた処理が発生、となります。

サーボモーターの速度調整をしていないので荒々しさに満ち溢れていますが、完成したらこんな感じになります。↓

ここにPOSTが表示されます