Picot チュートリアル１７

2022.09.08

■リモコンで歩行のコントロール

リスト16-1のプログラムと歩行のリスト9-1などを合わせて、リモコンで動作をコントロールするプログラムを作ります。準備として歩行のプログラムを少し変更します。

現状歩行のタイミングをコントロールするためにtime.sleep()を使っていますが、これがあるとリモコン用の割り込みと両立ができません。このため歩行の側もタイマー割込みで30Hzの制御をすることにします。リスト9-1を改造します。

リモコンの時には端子の状態がトリガーとなって割り込みがかかりましたが、今回は自分で（マイコンが）時間を数えて定期的に割り込みをかけます。30Hzで呼び出される関数tick(timer)ではグローバル変数「servo_flag」をTrueに設定して0.03ms（30Hz）経過したことを知らせます。

whileループのなかでは、servo_flagがTrueだった場合にのみデータを更新してサーボを駆動します。Servo_flagは次に備えてFalseに戻しておきます。

リスト17-1　walk2.py（部分）

from machine import Pin, PWM, Timer
import time

servo_flag = False
tim = Timer()

# 30Hzのタイマー割り込み
def tick(timer):
    global servo_flag
    servo_flag = True

tim.init(freq=30, mode=Timer.PERIODIC, callback=tick)

while True: # 繰り返し
    if servo_flag == True:
        servo_flag = False
        # キーフレームを更新
        # 角度計算
        # サーボ駆動
        #time.sleep(0.03) # 0.03秒待ち　削除

それではリモコンのプログラムと合わせていきます。

リモコンのボタンの割り当てです。

表 17-1  リモコンボタン割り当て

0LED ONC LED OFF
1左旋回2前進3右旋回
4左横5停止6右横
  8後退  

動作モードを管理するために「action_mode」という文字変数を導入しています。イニシャルは action_mode = ‘ STOP ’で、リモコンの受信でaction_mode = ‘ FWRD ‘などに変わります。

歩行データはこれまでangle[]というリストで用意してきましたが、今回は複数ありますのでそれぞれの名前でリストとし、動作の切り替えの際に別途用意した空のリスト「action」にその動作のデータをコピーして使うようにします。これはそのあとのサーボの角度計算のところで場合分けをしなくて済むようにするためです。

さらに、これまでwhile True:ループの中に書いていたこの部分は「set_action(code)」という関数にしています。

if data_code == 22:
    led.value(1)
elif data_code == 13:
    led.value(0)
#********* 省略 **********
elif data_code == 24:
    action_mode = ‘FWRD’
    action.clear()
    action = fwrd.copy()
    rows = len(fwrd)

リスト17-2に全体のコードを示します。また今回もコードを見やすくするため、動作ごとの関節角度のリストを別ファイルmotion.pyに移動しました。

リスト17-2　remote_walk.py

from machine import Pin, PWM, Timer
import time
import remote
import motion

# ***** Action *****
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]

div_counter = 0
key_frame = 0
next_key_frame = 1
rows = 0
action = []
action_mode = ‘STOP’
servo_flag = False
tim = Timer()

# 30Hzのタイマー割り込み
def tick(timer):
    global servo_flag
    servo_flag = True

tim.init(freq=30, mode=Timer.PERIODIC, callback=tick)

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

# ***** LED *****
led = Pin(25, Pin.OUT)
led.value(0)

# ***** Sound *****
buz = PWM(Pin(14))

def set_action(code):
    global action_mode
    global action
    global rows
    global div_counter
    global key_frame
    global next_key_frame
    if data_code == 22:
        led.value(1)
    elif data_code == 13:
        led.value(0)
    elif data_code == 28:
        action_mode = ‘STOP‘
    elif data_code == 24:
        action_mode = ‘FWRD‘
        action.clear()
        action = motion.fwrd.copy()
        rows = len(fwrd)
    elif data_code == 82:
        action_mode = ‘BWRD‘
        action.clear()
        action = motion.bwrd.copy()
        rows = len(bwrd)
    elif data_code == 12:
        action_mode = ‘LTRN‘
        action.clear()
        action = motion.ltrn.copy()
        rows = len(ltrn)
    elif data_code == 94:
        action_mode = ‘RTRN‘
        action.clear()
        action = motion.rtrn.copy()
        rows = len(rtrn)
    elif data_code == 8:
        action_mode = ‘LEFT‘
        action.clear()
        action = motion.left.copy()
        rows = len(left)
    elif data_code == 90:
        action_mode = ‘RGHT‘
        action.clear()
        action = motion.rght.copy()
        rows = len(rght)
    div_counter = 0
    key_frame = 0
    next_key_frame = 1

while True:
    # *** Remote ***
    if remote.rm_received == True:    #リモコン受信した
        remote.rm_received = False    #初期化
        remote.rm_state = 0      #初期化
        #図とは左右が逆であることに注意
        custom_code = remote.rm_code & 0xffff   #下16bitがcustomCode
        data_code = (remote.rm_code & 0xff0000) >> 16   #下16bitを捨てたあとの下8bitがdataCode
        inv_data_code = (remote.rm_code & 0xff000000) >> 24    #下24bitを捨てたあとの下8bitがinvDataCode
        if (data_code + inv_data_code) == 0xff:    #反転確認
            print("data_code="+str(data_code))
            set_action(data_code)
    # *** Servo ***
    if servo_flag == True:
        servo_flag = False
        if action_mode != ‘STOP‘:
            # キーフレームを更新
            div_counter += 1
            if div_counter >= action[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
                if action[next_key_frame][13] == 0: #無音
                    buz.duty_u16(0)
                elif action[next_key_frame][13] == 1: #「ド」
                    buz.duty_u16(int(MAX_DUTY/2))
                    buz.freq(262)
                elif action[next_key_frame][13] == 2: #「ソ」
                    buz.duty_u16(int(MAX_DUTY/2))
                    buz.freq(392)
            # 角度計算
            for i in range(12):
                temp_angle[i] = action[key_frame][i] +\
(action[next_key_frame][i] - action[key_frame][i])\
* div_counter / action[key_frame][12]
        else: #STOPモード
            buz.duty_u16(0) #音を止める
            for i in range(12):
                temp_angle[i] = 90
        # サーボ駆動
        for i in range(12):
            servo[i].duty_u16(get_pulse_width(int(temp_angle[i]) + correction[i]))

リスト17-3　motion.py

fwrd = [
    [75, 95,  90,110,110, 90,  90,110,110, 90,  95, 95, 8, 0],
    [80, 90,  90,110,110, 80,  80,100,100, 90,  90,100, 8, 0],
    [85, 85,  90, 90, 90, 90,  80, 90, 90, 90,  85,105, 4, 1],
    [90, 80,  90, 70, 70, 90,  80, 80, 80, 90,  80,110, 4, 1],
    [85, 85,  90, 70, 70, 90,  90, 70, 70, 90,  85,105, 8, 0],
    [80, 90,  90, 80, 80,100, 100, 70, 70, 90,  90,100, 8, 0],
    [75, 95,  90, 90, 90,100,  90, 90, 90, 90,  95, 95, 4, 2],
    [70,100,  90,100,100,100,  90,110,110, 90, 100, 90, 4, 2]
]
bwrd = [
    [75, 95,  90,110,110, 90,  90,110,110, 90,  95, 95, 8, 0],
    [70,100,  90,100,100,100,  90,110,110, 90, 100, 90, 4, 1],
    [75, 95,  90, 90, 90,100,  90, 90, 90, 90,  95, 95, 4, 1],
    [80, 90,  90, 80, 80,100, 100, 70, 70, 90,  90,100, 8, 0],
    [85, 85,  90, 70, 70, 90,  90, 70, 70, 90,  85,105, 8, 0],
    [90, 80,  90, 70, 70, 90,  80, 80, 80, 90,  80,110, 4, 2],
    [85, 85,  90, 90, 90, 90,  80, 90, 90, 90,  85,105, 4, 2],
    [80, 90,  90,110,110, 80,  80,100,100, 90,  90,100, 8, 0]
]
ltrn = [
    [75, 95,  90,110,110, 90,  90,110,110, 90,  95, 95, 8, 0],
    [80, 90,  90,110,110, 80,  80,100,100, 90,  90,100, 8, 0],
    [85, 85,  90, 90, 90, 90,  80, 90, 90, 90,  85,105, 4, 1],
    [90, 80,  90, 70, 70, 90,  80, 80, 80, 90,  80,110, 4, 1],
    [85, 85,  90, 70, 70, 90,  90, 70, 70, 90,  85,105, 8, 0],
    [80, 90,  90, 80, 80, 90,  90, 70, 70, 90,  90,100, 8, 0],
    [75, 95,  90, 90, 90, 90,  90, 90, 90, 90,  95, 95, 4, 2],
    [70,100,  90,100,100, 90,  90,110,110, 90, 100, 90, 4, 2]
]
rtrn = [
    [75, 95,  90,110,110, 90,  90,110,110, 90,  95, 95, 8, 0],
    [80, 90,  90,110,110, 90,  90,100,100, 90,  90,100, 8, 0], 
    [85, 85,  90, 90, 90, 90,  90, 90, 90, 90,  85,105, 4, 1],
    [90, 80,  90, 70, 70, 90,  90, 80, 80, 90,  80,110, 4, 1],
    [85, 85,  90, 70, 70, 90,  90, 70, 70, 90,  85,105, 8, 0],
    [80, 90,  90, 80, 80,100, 100, 70, 70, 90,  90,100, 8, 0],
    [75, 95,  90, 90, 90,100,  90, 90, 90, 90,  95, 95, 4, 2],
    [70,100,  90,100,100,100,  90,110,110, 90, 100, 90, 4, 2]
]
left = [
    [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90, 6, 0],
    [90, 90,  90, 90, 90,100, 100, 90, 90, 90,  90, 90, 6, 0], 
    [90, 90,  80, 90, 90,100, 100, 90, 90,100,  90, 90, 6, 1],
    [90, 90,  80, 90, 90, 80,  80, 90, 90, 90,  90, 90, 6, 0], 
    [90, 90,  90, 90, 90, 80,  80, 90, 90, 90,  90, 90, 6, 0],
    [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90, 6, 2]
]
rght = [
    [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90, 6, 0],
    [90, 90,  90, 90, 90, 80,  80, 90, 90, 90,  90, 90, 6, 0], 
    [90, 90,  80, 90, 90, 80,  80, 90, 90,100,  90, 90, 6, 1],
    [90, 90,  90, 90, 90,100, 100, 90, 90,100,  90, 90, 6, 0], 
    [90, 90,  90, 90, 90,100, 100, 90, 90, 90,  90, 90, 6, 0],
    [90, 90,  90, 90, 90, 90,  90, 90, 90, 90,  90, 90, 6, 2]
]

歩行から停止（または他の歩行動作）への姿勢移行は特に何も行わず、どの姿勢にいても瞬時に次の動作に移行します。

プログラムをロボットに書き込んで実機での動作を確認してください。

Picot　チュートリアル　インデックス >>
Picot　チュートリアル　18 単発動作 >>

コメント

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