M5stackで2足歩行ロボットの作成④「micropythonでPCA9685を制御してロボットのモーションを作成」

今回は以前に作成したmicropythonでのPCA9685制御プログラムで2足歩行ロボットのモーションを作成したいと思います。

それでは初めに、ロボット（サーボモーター）、pca9685,M5stack,pcを繋いで、モーションの確認が出来るようにしておきます。

モーションテスト環境構築

テンプレのように準備ができれば、第2回で作成した「mtest.py」のプログラムにモーションのプログラムを追加していきます。

mtest.py

from machine import Pin,I2C
import time
import servo
import pca9685

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

def set():
    s.position(0, 97)
    time.sleep_ms(1000)
    print('a')
    s.position(1, 90)
    time.sleep_ms(1000)
    print('b')
    s.position(2, 100)
    time.sleep_ms(1000)
    print('c')
    s.position(3, 90)
    time.sleep_ms(1000)
    print('c')
    
def fw():
    s.position(3, 70)
    s.position(1, 70)
    time.sleep_ms(500)
    s.position(0, 77)
    s.position(2, 80)
    time.sleep_ms(500)
    s.position(3, 90)
    s.position(1, 90)
    time.sleep_ms(500)
    s.position(3, 110)
    s.position(1, 110)
    time.sleep_ms(500)
    s.position(0,117)
    s.position(2, 120)
    time.sleep_ms(500)
    s.position(3, 90)
    s.position(1, 90)
    time.sleep_ms(500)
    
def bk():
    s.position(3, 110)
    s.position(1, 110)
    time.sleep_ms(500)
    s.position(0, 77)
    s.position(2, 80)
    time.sleep_ms(500)
    s.position(3, 90)
    s.position(1, 90)
    time.sleep_ms(500)
    s.position(3, 70)
    s.position(1, 70)
    time.sleep_ms(500)
    s.position(0,117)
    s.position(2, 120)
    time.sleep_ms(500)
    s.position(3, 90)
    s.position(1, 90)
    time.sleep_ms(500)
    
def lt():
    s.position(3, 40)
    s.position(1, 60)
    time.sleep_ms(500)
    s.position(0, 117)
    time.sleep_ms(500)
    s.position(3, 90)
    time.sleep_ms(500)
    s.position(1, 90)
    time.sleep_ms(500)
    s.position(0, 97)
    time.sleep_ms(500)
    
def rt():
    s.position(3, 120)
    s.position(1, 140)
    time.sleep_ms(500)
    s.position(2, 80)
    time.sleep_ms(500)
    s.position(1, 90)
    time.sleep_ms(500)
    s.position(3, 90)
    time.sleep_ms(500)
    s.position(2, 100)
    time.sleep_ms(500)
    

while(True):
    print('0/set 1/fw 2/lt 3/rt 4/bk')
    move = int(input('set No please:'))

    if move == 0:
        set()
    elif move == 1:
        fw()
    elif move == 2:
        lt()
    elif move == 3:
        rt()
    elif move == 4:
        bk()
    else:
        exit

関数にそれぞれ前進、後退、左旋回、右旋回、セットポジション、のモーションを作成しました。

第二引数の角度については、使用するサーボモーターで誤差が生じるため、微調整を繰り返しながらいい感じの数値に修正していきます。

このプログラムを実行すると、set No please:　の表示の後でシェルに番号を入力出来るので、動作させたい箇所の関数の番号を入力すればそれぞれのモーションの確認が可能になっています。

実行すると、下のような感じになります。↓

次回はこのプログラムを、前回作成したWEBサーバーのプログラムに埋め込みたいと思います。

コメントを残す

コメント一覧