Picot チュートリアル5

ロボットプログラミング

いよいよロボットのプログラム開発に入ります。アクションデータを作成して赤外線リモコンで操縦します。

サーボの角度調整

ロボットを望みどおりに動かすためには、各関節が全体の中できちんと指定した角度になってくれる必要があります。このための調整を、ロボットの基準姿勢を使って行います。本ロボットの基準姿勢は直立で、腕関節ロール軸(ひじ)を下側に45°曲げた姿勢です。

サーボの出力軸をセンター角度(90°)にしたとき、サーボホーンは必ずしもサーボ容器に対して90°や0°の位置にはなりません。また取り付け角度を変えて調整しても一度にローレット(軸回りのギザギザ)の歯一つ分回ってしまいますので、微妙な調整には使えません。そこでソフト上での調整を行います。

■サーボ調整プログラム

前ページで作ったプログラムを元にしてサーボの角度調整をするためのプログラムを作ります。

Pythonのリスト(配列)というものを使って各サーボの角度補正値を保持することにします。右腕関節から表4-4の順番でとりあえず0°に設定します。

correction = [0,0, 0,0,0,0, 0,0,0,0, 0,0]
servo = []

前回サーボは1個でしたが今回は12個動かしますので、こちらもリストで「servo」の集合を用意します。まずは中身のない空のリストをつくり、「append」で新たな要素を一つ追加して実体を与えていきます。 forループの中ではひとつずつ周波数と角度を設定しています。forループについては「python、 for」などで検索してください。

サーボへの制御信号を発生している行で、設定角度に補正角度「correction[ i ]」を足しています。

servo.append(PWM(Pin(0))) #Right Shoulder Roll
servo.append(PWM(Pin(1))) #Right Shoulder Pitch
・・省略・・
for i in range(12):
    servo[i].freq(50)
    servo[i].duty_u16(get_pulse_width(90 + correction[i]))

リスト5-1 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

# 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]))

「correction」の値を変更しながらプログラムを書き込み、各サーボに対する必要な補正量を見つけて、「correction[ ]」のリストを完成させてください。サーボの回転方向は図5-1を参照してください。矢印の方向がプラスの回転方向です。この段階でもUSB電源で駆動できると思います。

45°や90°にはあまりこだわらず左右でなるべく対称になるように調整してください。

図5-1

全関節調整完了し、このような感じになりました。それぞれのロボットで結果は異なるはずです。

correction = [0,-12, -4,0,4,0, 4,-6,4,-12, 0,0]

Picot チュートリアル インデックス >>
Picot チュートリアル 6 ロボット動作プログラム >>

コメント

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