Picot チュートリアル６

2022.09.08

■ロボット動作プログラム

続いてロボットを動かします。簡単な動作から徐々に進めていきます。まずは関節を往復運動させますが、現在「90」と固定で書いている設定角度を変更できるようにリストデータとして与えることにします。今回は縦横の2次元リストになります。

angle = [
    [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90],
    [70, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90]
]

「correction[ ]」と同様に行（横）方向が各関節で、列（縦）方向が時間軸です。フレームと呼びます。右肩ロール軸関節を20°曲げ伸ばしします。1行目の最後には「,」（カンマ）あり、2行目はなしです。

while True: # 繰り返し
    for i in range(12):
        servo[i].duty_u16(get_pulse_width(angle[0][i] + correction[i]))
    time.sleep(0.5) # 0.5秒待ち
    for i in range(12):
        servo[i].duty_u16(get_pulse_width(angle[1][i] + correction[i]))
    time.sleep(0.5) # 0.5秒待ち

繰り返しのプログラムはLEDでやったのと同じ方式です。angleの1行目と2行目の角度を設定する間に0.5秒間の休止を入れています。

リスト6-1　swing_arm.py

from machine import Pin, PWM
import time

SV_FREQ = 50.0  # サーボ信号周波数
MAX_DUTY = 65025.0 # 周期内の分割数
MIN_SV_PULSE = 0.6  # 最小パルス幅　0°
MAX_SV_PULSE = 2.4  # 最大パルス幅 180°

correction = [0,0, 0,0,0,0, 0,0,0,0, 0,0]
servo = []
angle = [
    [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90],
    [70, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90]
]

# パルス幅を計算する関数
def get_pulse_width(angle):
    pulse_ms = MIN_SV_PULSE + (MAX_SV_PULSE - MIN_SV_PULSE) * angle / 180.0
    x = (int)(MAX_DUTY * (pulse_ms * SV_FREQ /1000.0))
    return x

# 12個のservoを追加
servo.append(PWM(Pin(0))) #Right Shoulder Roll
servo.append(PWM(Pin(1))) #Right Shoulder Pitch
servo.append(PWM(Pin(2))) #Right Hip Roll
servo.append(PWM(Pin(3))) #Right Hip Pitch
servo.append(PWM(Pin(8))) #Right Ankle Pitch
servo.append(PWM(Pin(9))) #Right Ankle Roll
servo.append(PWM(Pin(20))) #Left Ankle Roll
servo.append(PWM(Pin(21))) #Left Ankle Pitch
servo.append(PWM(Pin(22))) #Left Hip Pitch
servo.append(PWM(Pin(26))) #Left Hip Roll
servo.append(PWM(Pin(27))) #Left Shoulder Pitch
servo.append(PWM(Pin(28))) #Left Shoulder Roll

# 全てのサーボを順番に駆動
for i in range(12):
    servo[i].freq(50)
    servo[i].duty_u16(get_pulse_width(90 + correction[i]))

while True: # 繰り返し
    for i in range(12):
        servo[i].duty_u16(get_pulse_width(angle[0][i] + correction[i]))
    time.sleep(0.5) # 0.5秒待ち
    for i in range(12):
        servo[i].duty_u16(get_pulse_width(angle[1][i] + correction[i]))
    time.sleep(0.5) # 0.5秒待ち

ちなみに現在のプログラムで待ち時間を2秒などに変更して実行してみてください。同じように往復動作はするものの、動きがカクカクして滑らかではありません。サーボは指定された角度に最高速で到達しようとしますので動いて停止、また動いて停止というアクションになってしまいます。

これを改善するためにプログラムを改造してみます。time.sleep( )の時間を小さくして、フレーム間の角度を比例計算で細かく指定してあげます。「\（バックスラッシュ）」は「文の途中で、改行ではありません」という意味です。見やすさの都合で改行したい場合はこれをつけてください。

リスト6-2　swing_arm2.py

from machine import Pin, PWM
import time

SV_FREQ = 50.0  # サーボ信号周波数
MAX_DUTY = 65025.0 # 周期内の分割数
MIN_SV_PULSE = 0.6  # 最小パルス幅　0°
MAX_SV_PULSE = 2.4  # 最大パルス幅 180°

correction = [0,0, 0,0,0,0, 0,0,0,0, 0,0]
servo = []
temp_angle = [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90]
angle = [
    [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90],
    [70, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90]
]

divide = 30 # フレーム間の分割数
div_counter = 0 # 分割を数える
key_frame = 0 # 現在のキーフレーム
next_key_frame = 1 # 次回のフレーム

# パルス幅を計算する関数
def get_pulse_width(angle):
    pulse_ms = MIN_SV_PULSE + (MAX_SV_PULSE - MIN_SV_PULSE) * angle / 180.0
    x = (int)(MAX_DUTY * (pulse_ms * SV_FREQ / 1000.0))
    return x

# 12個のservoを追加
servo.append(PWM(Pin(0))) #Right Shoulder Roll
servo.append(PWM(Pin(1))) #Right Shoulder Pitch
servo.append(PWM(Pin(2))) #Right Hip Roll
servo.append(PWM(Pin(3))) #Right Hip Pitch
servo.append(PWM(Pin(8))) #Right Ankle Pitch
servo.append(PWM(Pin(9))) #Right Ankle Roll
servo.append(PWM(Pin(20))) #Left Ankle Roll
servo.append(PWM(Pin(21))) #Left Ankle Pitch
servo.append(PWM(Pin(22))) #Left Hip Pitch
servo.append(PWM(Pin(26))) #Left Hip Roll
servo.append(PWM(Pin(27))) #Left Shoulder Pitch
servo.append(PWM(Pin(28))) #Left Shoulder Roll
# 全てのサーボを順番に駆動
for i in range(12):
    servo[i].freq(50)
    servo[i].duty_u16(get_pulse_width(90 + correction[i]))

while True: # 繰り返し
    # キーフレームを更新
    div_counter += 1
    if div_counter >= divide:
        div_counter = 0
        key_frame = next_key_frame
        next_key_frame += 1
        if next_key_frame > 1:
            next_key_frame = 0 #angle 0行に戻る
    # 角度計算
    for i in range(12):
        temp_angle[i] = angle[key_frame][i] +\
(angle[next_key_frame][i] - angle[key_frame][i])\
* div_counter / divide
    # サーボ駆動
    for i in range(12):
        servo[i].duty_u16(get_pulse_width(int(temp_angle[i]) + correction[i]))
    time.sleep(0.03) # 0.03秒待ち

●プログラムの説明

待ち時間を0.03秒として角度を1秒間に30回更新するようにします。

    time.sleep(0.03) # 0.03秒待ち

0.03秒ごとにカウンターを更新します。カウンターの値がdivide（この場合30なので0.03 × 30で約1秒）に達したらkey_frameとnext_key_frameを更新（次のangleの行に移行）します。今回は2行しかありませんので1回ごとに元に戻ります。

    div_counter += 1
    if div_counter >= divide:
        div_counter = 0
        key_frame = next_key_frame
        next_key_frame += 1
        if next_key_frame > 1:
            next_key_frame = 0

サーボの角度はdiv_counter / divideで比例計算です。

    for i in range(12):
        temp_angle[i] = angle[key_frame][i] +\　#前回角度
(angle[next_key_frame][i] - angle[key_frame][i])\ #次との差分
* div_counter / divide #割合

プログラムを書き込んでロボットがゆっくりと動くことを確認してください。

●練習問題

 リスト 5-3 を変更して独自の動作を作ってください。動かすのは腕だけで OK です。

例）

angle = [
    [90, 80,  90, 90, 90, 90,  90, 90, 90, 90,  80,110],
    [70,100,  90, 90, 90, 90,  90, 90, 90, 90, 100, 90]
]

Picot　チュートリアル　インデックス >>
Picot　チュートリアル　7 左右の体重移動 >>

コメント

タイトルとURLをコピーしました