ロボットプログラミング
いよいよロボットのプログラム開発に入ります。アクションデータを作成して赤外線リモコンで操縦します。
■サーボの角度調整
ロボットを望みどおりに動かすためには、各関節が全体の中できちんと指定した角度になってくれる必要があります。このための調整を、ロボットの基準姿勢を使って行います。本ロボットの基準姿勢は股（肩）関節を後ろ側に45°曲げて膝（ひじ）関節を前側に直角に曲げた姿勢です。
サーボの出力軸をセンター角度（90°）にしたとき、サーボホーンは必ずしもサーボ容器に対して90°や0°の位置にはなりません。また取り付け角度を変えて調整しても一度にローレット（軸回りのギザギザ）の歯一つ分回ってしまいますので、微妙な調整には使えません。そこでソフト上での調整を行います。
●サーボ調整プログラム
チュートリアル4で作ったプログラムを元にしてサーボの角度調整をするためのプログラムを作ります。
Pythonのリスト（配列）というものを使って各サーボの角度補正値を保持することにします。左前肩関節から順番に右後ろ膝関節までとりあえず0°に設定します。
correction = [0,0,0, 0,0,0, 0,0,0, 0,0,0]
servo = []前回サーボは1個でしたが今回は12個動かしますので、こちらもリストで「servo」の集合を用意します。まずは中身のない空のリストをつくり、forループの中でひとつずつ実体を与えて角度を設定していきます。forループについては「python、 for」などで検索してください。「append」で新たな要素を一つ追加します。「11 – i」となっているのは基板上のPicoの向きの関係でサーボの番号とピン番号が逆になったためです。
サーボへの制御信号を発生している行で、設定角度に補正角度「correction[ i ]」を足しています。
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]))List6 servo_correct.py
from machine import Pin, PWM
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 = []
# パルス幅を計算する関数
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]))
「i」の値を変更しながらプログラムを書き込み、各サーボに対する必要な補正量を見つけて、「correction[ ]」のリストを完成させてください。サーボの回転方向は下図を参照してください。矢印の方向がプラスの回転方向です。この段階でもUSB電源で駆動できると思います。
45°や90°にはあまりこだわらず左右でなるべく対称になるように調整してください。
全関節調整完了し、このような感じになりました。それぞれのロボットで結果は異なるはずです。
correction = [14,-14,4, 0,0,6, -18,-12,4, 20,-4,-8]
コメント