■ロボットを動かすプログラム
続いてロボットを動かします。簡単な動作から徐々に進めていきます。まずは関節を往復運動させますが、現在「90」と固定で書いている設定角度を変更できるようにリストデータとして与えることにします。設定している角度については図5-2を参照し、足先を上方に上げようとすると膝は股関節の2倍の角度曲げる必要があることを確認してください。
angle = [\
[90, 90, 90, 90,90,90, 90, 90, 90, 90,90,90],\
[90,100,110, 90,80,70, 90,100,110, 90,80,70]\
]「correction[ ]」と同様に行（横）方向が各関節で、列（縦）方向が時間軸です。フレームと呼びます。股（肩）関節を10°、膝（ひじ）を20°曲げます。「＼（バックスラッシュ）」は「文の途中で、改行ではありません」という意味です。見やすさの都合で改行したい場合はこれをつけてください。
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秒間の休止を入れています。
List7 bend_knee.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 = [14,-14,4, 0,0,6, -18,-12,4, 20,-4,-8]
servo = []
angle = [\
[90, 90, 90, 90,90,90, 90, 90, 90, 90,90,90],\
[90,100,110, 90,80,70, 90,100,110, 90,80,70]\
]
# パルス幅を計算する関数
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: # 繰り返し
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( )の時間を小さくして、フレーム間の角度を比例計算で細かく指定してあげます。
List8 bend_knee2.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 = [14,-14,4, 0,0,6, -18,-12,4, 20,-4,-8]
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],\
[90,100,110, 90, 80, 70, 90,100,110, 90, 80, 70]\
]
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
# 全てのサーボを順番に駆動
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 >= divide:
div_counter = 0
key_frame = next_key_frame
next_key_frame += 1
if next_key_frame > 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 / 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プログラムを書き込んでロボットが滑らかに動くことを確認してください。
コメント