クアッド チュートリアル 10

■フレーム間推移時間

現状フレーム間の経過時間が divide × 30ミリ秒 で固定となっているので、これをそれぞれのフレームに固有の値に設定できるように変更します。動作リストのフレーム行の最後尾に「divide数」の列を追加します。設定した値は、4脚着地での体重移動はゆっくり、2脚での遊脚移動は素早く、という動きになるように決めました。
また、angleの行数が変わった時にも対応できるように変数「rows」を用意してangleの行数を格納しています。len()はリストの要素数を獲得できるPythonの関数です。

List16 walk2

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 = [14,-14,4, 0,0,6, -25,-12,4, 20,-4,-8]
servo = []
temp_angle = [90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90]

angle = [\
[90,105, 70, 90, 86, 95, 90, 94, 85, 90, 75,110, 3],\
[90,100,110, 90, 82,100, 90, 98, 80, 90, 80, 70, 3],\
[90, 90, 90, 90, 78,105, 90,102, 75, 90, 90, 90, 8],\
[90, 94, 85, 90, 75,110, 90,105, 70, 90, 86, 95, 3],\
[90, 98, 80, 90, 80, 70, 90,100,110, 90, 82,100, 3],\
[90,102, 75, 90, 90, 90, 90, 90, 90, 90, 78,105, 8]\
]

#divide = 6 削除
div_counter = 0
key_frame = 0
next_key_frame = 1
rows = len(angle)

#パルス幅を計算する関数
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

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

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

クアッド チュートリアル 11 >>

コメント

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