AMD製品に関連する技術ブログ

Long Copy

KR260でロボットアームを動かしてみた！ (2)

皆さん、こんにちは。新人エンジニアのノリさんです！今回は前回作成したハードウェアデザインを元にしたソフトウェアによってクレーンゲーム風にロボットアームアームを動かしていきます。

2023-12-1

Long Copy - body

Step0：ハードウェア要件

KR260 Robotics Starter Kit (/japan/products/product-highlights/xilinx-kr260)
KR260用電源（KR260 Robotics Starter Kit に付属）
USB-A to micro-B ケーブル（KR260 Robotics Starter Kit に付属）
マイクロSDカード（最低16GB、KR260 Robotics Starter Kit に付属しているものでOK）
LANケーブル（KR260 Robotics Starter Kit に付属）
6軸ロボットアーム (https://www.amazon.co.jp/AMONIDA/dp/B08FBDNY5M)
サーボモーター (MG996R : 6個、ロボットアームに付属)
Pmod CON3 : 2個 (https://digilent.com/reference/pmod/pmodcon3/start)
3.3 V to 5 V レベルシフタ (https://www.amazon.co.jp/VKLSVAN-Bi-Directional-Conversion-Channel-Converter/dp/B086WWCJGQ/ または https://www.amazon.co.jp/Hailege-Converter-Bi-Directional-Interchange-Raspberry/dp/B08R5MLZH9/ 前者ははんだ付けが必要)
ジャンパー線 (https://www.amazon.co.jp/ELEGOO-120pcs-Multicolor-Dupont-arduino/dp/B06Y48V9DL/)
ブレッドボード (https://www.amazon.co.jp/ELEGOO-Tie-Point-Breadboard-Solderless-Arduino/dp/B06Y4KFCRJ/)
5V電源 (https://akizukidenshi.com/catalog/g/gM-14167/)
電源アダプタ (https://akizukidenshi.com/catalog/g/gC-08849/)

Step1：回路の作成

ここでは、ロボットアームを動作させるための回路を作っていきます

後述するプログラムによって回路に流れる信号を操作し、ロボットアームを動かしていきます

下の回路図を元に回路の作成を行います

回路作成時は事故防止のため、KR260には繋がずに作業を行います

Pmodの端子を■、ブレッドボードの穴を●、最終出力を▲で表現しています

S1～S6は信号線、V1～V6は電源、G1～G6はグランドを指しています

回路が出来上がったらKR260にPmodを差し込みます

電源プラグを奥（上）、Micro-USB差込口を手前（下）にして、一番左にPmod1、左から二番目にPmod2を差し込みます（どちらも上の穴が全て埋まるように差し込みます）

下の図に実際にKR260と回路を繋いだ様子を示します

Step2： Pynqのインストール

既にPynqがインストールされている場合は、スキップしてください

Pynqを使う目的：PynqはPythonを用いてFPGAのコンフィグレーションや操作を行うことのできるオープンソースパッケージです。HWに関する直接的な知識がなくとも、Pythonによる直観的な操作が行うことができます。今回は主にSWエンジニアをターゲットにしているので、HWに関する知識のない場合でも直観的にソフトウェア開発ができるようにPynqを使うことを決めました

Pynqに関しては次のページをご確認ください

Pynq公式ページ：http://www.pynq.io/
Acriブログ：https://www.acri.c.titech.ac.jp/wordpress/archives/29

ここではKR260にて操作を行うので、あらかじめ作成した回路をKR260に繋ぎ、

初めにaptのアップデートとアップグレードを行います

sudo apt update
sudo apt upgrade

Kria-PYNQ の githubから git cloneします

git clone https://github.com/Xilinx/Kria-PYNQ.git

Pynqをインストールします

cd Kria-PYNQ
sudo bash install.sh -b KR260

Step3：ロボットアームを動かす

ここからはKR260でロボットアームを操作するためのプログラムをJupyter Notebookで作成していきます

KR260のIPアドレスを確認します（次のコマンドでIPアドレスが表示されない場合はeth1をeth0に変更）

ifconfig eth1

次のように表示されるので、inetの横に書かれているIPアドレスをコピーしてPCまたはKR260上のWebブラウザに貼り付け、Jupyter Notebook のページに移動します（ログインパスワードはxilinxです）

eth1: flags=4163<UP,BROADCAST,RUNNING,MULTICAST> mtu 1500 inet 192.168.1.36 netmask 255.255.255.0 broadcast 192.168.1.255 ... （中略）

PCから操作する場合は、PCとKR260が同一のネットワークに繋がっている必要があります

無事にページの移動とログインができた場合、次のような画面が表示されます画面右上のNewからPython3 (ipykernel) を選択して新しいPythonカーネルを立ち上げてください

Jupyter Notebook上のコマンド及びプログラムはshift + enter で実行できます

セルに次に示すソースコードをコピー&ペーストして実行してください（参考：https://www.makarenalabs.com/pwm-on-pynq-how-to-control-a-stepper-motor/, https://ipywidgets.readthedocs.io/en/latest/examples/Widget%20List.html）

from pynq import Overlay
from ipywidgets import interact, interactive, HBox, VBox
import ipywidgets as widgets

print("手先に近い順でモーターに 1 ～ 6 の番号が振ってあります。")
print("Angle1 ～ Angle6 のスライドバーを動かして Motor1 ～ Motor6 の角度を調節できます。")

# load the bit file and configure the FPGA
ol = Overlay('kr260_pwm_pmod.bit')

# utility functions for bit manipulation (set, clear, get)
def set_bit(value, bit):
    return value | (1 << bit)

def clear_bit(value, bit):
    return value & ~(1 << bit)

def get_bit(value, bit):
    return (value >> bit) & 1

# extract register addresses (will be the same for every Axi Timer)
TCSR0 = ol.ip_dict['axi_timer_0']['registers']['TCSR0']
TCSR1 = ol.ip_dict['axi_timer_0']['registers']['TCSR1']
TCSR0_address = TCSR0['address_offset']
TCSR1_address = TCSR1['address_offset']
TCSR0_register = TCSR0['fields'] # bit_offset for address
TCSR1_register = TCSR1['fields']
TLR0 = ol.ip_dict['axi_timer_0']['registers']['TLR0']
TLR1 = ol.ip_dict['axi_timer_0']['registers']['TLR1']
TLR0_address = TLR0['address_offset']
TLR1_address = TLR1['address_offset']

# create the configuration values for the control register
temp_val_0 = 0
temp_val_1 = 0

# The PWMA0 bit in TCSR0 and PWMB0 bit in TCSR1 must be set to 1 to enable PWM mode
temp_val_0 = set_bit(temp_val_0, TCSR0_register['PWMA0']['bit_offset'])
temp_val_1 = set_bit(temp_val_1, TCSR1_register['PWMA1']['bit_offset'])

# The GenerateOut signals must be enabled in the TCSR (bit GENT set to 1). The PWM0
# signal is generated from the GenerateOut signals of Timer 0 and Timer 1, so these
# signals must be enabled in both timer/counters
temp_val_0 = set_bit(temp_val_0, TCSR0_register['GENT0']['bit_offset'])
temp_val_1 = set_bit(temp_val_1, TCSR1_register['GENT1']['bit_offset'])

# The counter can be set to count up or down. UDT
temp_val_0 = set_bit(temp_val_0, TCSR0_register['UDT0']['bit_offset'])
temp_val_1 = set_bit(temp_val_1, TCSR1_register['UDT1']['bit_offset'])

# set Autoreload (ARHT0 = 1)
temp_val_0 = set_bit(temp_val_0, TCSR0_register['ARHT0']['bit_offset'])
temp_val_1 = set_bit(temp_val_1, TCSR1_register['ARHT1']['bit_offset'])

# enable timer (ENT0 = 1)
temp_val_0 = set_bit(temp_val_0, TCSR0_register['ENT0']['bit_offset'])
temp_val_1 = set_bit(temp_val_1, TCSR1_register['ENT1']['bit_offset'])


Moter1 = ol.axi_timer_0
# function of angle control
def clicked1(Angle1):
    # period of PWM
    _period_ = 16000  
    period = int((_period_ & 0x0ffff) * 100); 
    
    # pulse width of PWM
    _pulse_ = int((3200 + 200.0/9.0 * Angle1))  
    pulse = int((_pulse_ & 0x0ffff) * 100);
   
    # control PWM waveform
    Moter1.write(TCSR0['address_offset'], temp_val_0)
    Moter1.write(TCSR1['address_offset'], temp_val_1)
    Moter1.write(TLR0['address_offset'], period)
    Moter1.write(TLR1['address_offset'], pulse)

w1 = interactive(clicked1, Angle1=widgets.IntSlider(min=-45, max=45, step=1, value=45))


Moter2 = ol.axi_timer_1
# function of angle control
def clicked2(Angle2):
    # period of PWM
    _period_ = 16000  
    period = int((_period_ & 0x0ffff) * 100); 
    
    # pulse width of PWM
    _pulse_ = int((3200 + 200.0/9.0 * Angle2))  
    pulse = int((_pulse_ & 0x0ffff) * 100);
   
    # control PWM waveform
    Moter2.write(TCSR0['address_offset'], temp_val_0)
    Moter2.write(TCSR1['address_offset'], temp_val_1)
    Moter2.write(TLR0['address_offset'], period)
    Moter2.write(TLR1['address_offset'], pulse)

w2 = interactive(clicked2, Angle2=widgets.IntSlider(min=-90, max=90, step=1, value=0))


Moter3 = ol.axi_timer_2
# function of angle control
def clicked3(Angle3):
    # period of PWM
    _period_ = 16000  
    period = int((_period_ & 0x0ffff) * 100); 
    
    # pulse width of PWM
    _pulse_ = int((3200 + 200.0/9.0 * Angle3))  
    pulse = int((_pulse_ & 0x0ffff) * 100);
   
    # control PWM waveform
    Moter3.write(TCSR0['address_offset'], temp_val_0)
    Moter3.write(TCSR1['address_offset'], temp_val_1)
    Moter3.write(TLR0['address_offset'], period)
    Moter3.write(TLR1['address_offset'], pulse)

w3 = interactive(clicked3, Angle3=widgets.IntSlider(min=-90, max=90, step=1, value=90))

Moter4 = ol.axi_timer_3
# function of angle control
def clicked4(Angle4):
    # period of PWM
    _period_ = 16000  
    period = int((_period_ & 0x0ffff) * 100); 
    
    # pulse width of PWM
    _pulse_ = int((3200 + 200.0/9.0 * Angle4))  
    pulse = int((_pulse_ & 0x0ffff) * 100);
   
    # control PWM waveform
    Moter4.write(TCSR0['address_offset'], temp_val_0)
    Moter4.write(TCSR1['address_offset'], temp_val_1)
    Moter4.write(TLR0['address_offset'], period)
    Moter4.write(TLR1['address_offset'], pulse)

w4 = interactive(clicked4, Angle4=widgets.IntSlider(min=-90, max=90, step=1, value=-90))


Moter5 = ol.axi_timer_4
# function of angle control
def clicked5(Angle5):
    # period of PWM
    _period_ = 16000  
    period = int((_period_ & 0x0ffff) * 100); 
    
    # pulse width of PWM
    _pulse_ = int((3200 + 200.0/9.0 * Angle5))  
    pulse = int((_pulse_ & 0x0ffff) * 100);
   
    # control PWM waveform
    Moter5.write(TCSR0['address_offset'], temp_val_0)
    Moter5.write(TCSR1['address_offset'], temp_val_1)
    Moter5.write(TLR0['address_offset'], period)
    Moter5.write(TLR1['address_offset'], pulse)

w5 = interactive(clicked5, Angle5=widgets.IntSlider(min=-90, max=90, step=1, value=-30))


Moter6 = ol.axi_timer_5
# function of angle control
def clicked6(Angle6):
    # period of PWM
    _period_ = 16000  
    period = int((_period_ & 0x0ffff) * 100); 
    
    # pulse width of PWM
    _pulse_ = int((3200 + 200.0/9.0 * Angle6))  
    pulse = int((_pulse_ & 0x0ffff) * 100);
   
    # control PWM waveform
    Moter6.write(TCSR0['address_offset'], temp_val_0)
    Moter6.write(TCSR1['address_offset'], temp_val_1)
    Moter6.write(TLR0['address_offset'], period)
    Moter6.write(TLR1['address_offset'], pulse)

w6 = interactive(clicked6, Angle6=widgets.IntSlider(min=-90, max=90, step=1, value=-30))

# display the slide bar
VBox([w1.children[0], w2.children[0], w3.children[0], w4.children[0], w5.children[0], w6.children[0]])

スライドバーを用いてモーターの角度を調節できます

実際に動かした様子を動画にしました

感想

今回は動画を撮るのに大変苦労しました。プログラムの作成や回路図を作るのも大変でしたが、ロボットアームを上手く動かそうと思うと大変で、なかなかモノを掴めませんでした。動画でもそれは見て取れると思います。また、動画が縦長なのはスマホで撮影したからです（手持ちのカメラの調子が悪かったので）。今は手持ちのカメラの調子も戻りましたが、二度とは撮影したくないですね(笑)。クレーンゲームとして遊ぶこと自体は楽しかったです。今回はできませんでしたが、今後は可能なら自動制御にも繋げていく予定です。それでは、また次回お会いしましょう！See you next time!