ネット通販で8,000円程度で入手できる６自由度ロボットアーム組立キット(模型or玩具品的な物)を購入しました。このロボットアームの制御に関して紹介します。(このロボットアームキットの概要と組立に関してはこちらの記事を参考にして下さい）

このロボットアーム組立キットは、ロボットアームのみの製品で、コントローラは付属していません。

今回は、コントローラとしてRaspberry Pi Pico Wを使用しました。プログラム言語はMicroPythonを使用しました。

ロボットアームの動作動画

下は、Raspberry Pi Pico Wをコントローラとして使用して、このロボットアームを制御した時の動画です。
グリッパーでクリップをつかみ箱に入れる動作をしています。

Raspberry Pi Pico Wに関して

今回ロボットアームのコントローラとして、マイコンボード　Raspberry Pi Pico Wを使用しました。

使用した理由は、たまたま手元にあったのと、小型で安価でPWMが16チャンネルあるので、ロボットアームの６個のサーボモータをPWM制御するのに適切であると思ったからです。
また、Wi-FiやBluetooth機能があるので、将来的にロボットアームを遠隔操作することも出来るかなと思いました。

図２にRaspberry Pi Pico Wの外観と特徴に関して記載します。

MicroPythonに関して

Raspberry Pi Pico Wのプログラム言語としてMicroPythonを使用しました。
MicroPythonをRaspberry Pi Pico Wで使用するための手順を以下に説明します。

１．Raspberry Pi Pico WとパソコンをBOOTSELボタンを押しながらUSBケーブルを使用して接続します。Raspberry Pi Pico Wがディスクドライブとして認識されます。

２．エクスプローラで見て、「RPI-RP2」がRaspberry Pi Pico Wのディスクドライブです。
MicroPythonのダウンロードページ(https://MicroPython.org/download/)
から、Raspberry Pi Pico W用のファームウェアをダウンロードします。

今回ダウンロードしたファームウェアはバージョン1.25.0でファイル名称は、
RPI_PICO_W-20250415-v1.25.0.uf2
でした。
このファイルを「RPI-RP2」ディスクにコピーします。これで、Raspberry Pi Pico WでMicroPythonが動作するようになります。

３．MicroPythonのプログラム作成ツールとしてThonnyをパソコンにインストールします。

Thonnyのダウンロードページは、
https://thonny.org/
です。

Raspberry Pi Pico WとパソコンをUSBケーブルで接続して、Thonnyを起動することによって、プログラムの編集や実行を行うことが出来ます。

図４はThonnyの画面です。

サーボモータ制御に関して

本ロボットアームキットには、図５のサーボモータが６個付属していました。MG 996Rと書いてありますが、MG 996RはTower Pro製なので、互換品と思われます。

サーボモータはPWM信号で制御します。サーボモータから出ている線の、橙線がPWM信号で、赤線が電源線(4.8V～7.2V)で、茶線がGND線です。
PWM信号は、図６の様にパルスのデューティ比が変化する信号です。

サーボモータは、PWM信号のデューティ比を変更することによって、軸の角度が変化します。
サーボモータは50HzのPWM信号で制御します。50Hzの周期は20msなので20ms内のパルスの幅によってサーボモータの軸の角度が変わります。
例えば図７は、サーボモータのPWM信号波形の例ですが、パルス幅1ms(デューティ比5%)の場合は軸の角度が0°、パルス幅2ms(デューティ比10%)の時は軸の角度が１８０°となります。

MG996Rサーボモータの資料には、パルス幅1msで-90°、1.5msで0°、2msで+90°と書いてありますが、ロボットアーム付属のサーボモータでは、そのパルス幅の範囲では全部で90°程度しか動作しませんでした。そこで簡単な回路とプログラムを作成して、デューティ比を色々変更して見て180°動作する値を捜しました。

パルス幅0.575ms(デューティ比2.875％)～2.425ms(デューティ比7.5％)とした場合にトータル180°動作したのでその値を使用することにしました。

サーボモータは、パルス幅0.575ms (デューティ比2.875％)時に-90°、パルス幅1.5ms(デューティ比7.5％)時に0°、パルス幅2.425ms(デューティ比12.125%)時に90°の位置に回転しました。また、回転方向は、-90°が時計方向、90°が反時計方向で通常のサーボモータと逆回転でした。

下はテスト時の動画で、パルス幅0.575ms～2.425msを使用して、0°→90°→0°→-90°→0°を繰り返す様子です。0°はパルス幅1.5msです。

サーボモータテスト回路

図８がサーボモータの回転をテストするために作成した回路です。Raspberry Pi Pico WのGP0のI/OにサーボモータのPWM信号線(橙色線)を接続しています。サーボモータの電源は電流が大きいので、USBから供給される電源とは別の電源を使用しました。

サーボモータテストプログラム

以下にサーボモータの回転角度をテストするMicroPythonのプログラムを示します。
0°→1秒待ち→90°→1秒待ち→0°→1秒待ち→-90°→1秒待ち→0°の繰り返しを行います。
PWMの周波数は50Hzで周期20msで、パルス幅2.425ms時に+90°、パルス幅1.5ms時に0°、パルス幅0.575ms時に-90°位置に回転します。

from machine import PWM, Pin
import time

#サーボモータの設定
servo = PWM(Pin(0), freq = 50)	#GPIO 0がサーボ接続

plu90_duty = float(2.425 / 20.0)	#90deg時のデユーティ比
min90_duty = float(0.575 / 20.0)	#-90deg時のデユーティ比   

#指定角度にサーボモータを動かす関数
#deg: 移動角度　-90～90
def sv_mov(deg):
    duty = int(65535 * (min90_duty + (plu90_duty - min90_duty) * (deg + 90) /180))
    servo.duty_u16(duty)
            
#---------メインプログラム----------
while True:
    sv_mov(0.0)
    time.sleep(1.0)
    sv_mov(90.0)
    time.sleep(1.0)
    sv_mov(0.0)
    time.sleep(1.0)
    sv_mov(-90.0)
    time.sleep(1.0)

【プログラム説明】
５行目：
servo = PWM(Pin(0), freq = 50)
Raspberry Pi Pico WのGP0のI/OにサーボモータのPWM信号を割り当てています。PWM周波数50Hzに設定しています。

７，８行目：
plu90_duty = float(2.425 / 20.0)
min90_duty = float(0.575 / 20.0)

サーボモータ軸が90°の時のPWMのパルス幅を2.425ms、-90°の時のPWMのパルス幅を0.575msに指定してそれぞれのデューティ比を計算しています。割り算の分母の20はPWM周波数50Hzの周期20msです。

１３，１４行目：
duty = int(65535 * (min90_duty + (plu90_duty – min90_duty) * (deg + 90) /180))
servo.duty_u16(duty)

PWM信号のデューティ比を、servo.duty_u16(duty)のdutyで指定し、サーボモータを動作させます。
dutyは100％を65535とする数値(int)で指定します。例えば20％の時は、65535 * 20/ 100 = 13107 となります。
ここでは、+90°の時のPWMのデューティ比(plu90_duty)と-90°の時のデューティ比(min90_duty)から指定角度に対するデューティ比を比例計算しています。

サーボモータテストプログラムの実行手順

MicroPythonで作成したサーボモータテストプログラムをRaspberry Pi Pico Wで実行する手順に関して以下で説明します。

１．パソコンでThonnyを起動して、Raspberry Pi Pico WをUSBケーブルで接続します。

２．Thonny画面の右下部をクリックして、「MicroPython (Raspberry Pi Pico)—」を選択します。

３．プログラムを書きます。

４．画面左上のをクリックするとプログラムがRaspberry Pi Pico Wで実行されます。

ロボットアームのコントローラの回路

図９はロボットアームのコントローラの回路図です。６個のサーボモータの名称をJ1～J6と名付けました。
それぞれのサーボモータのPWM信号線をGP0～GP5のI/Oに接続しました。
動作スタート用のスイッチをGP16のI/Oに接続しました。

ロボットアームの制御プログラムの要点

１．サーボモータを少しずつ動かす。
サーボモータを一気に動作させると、グリッパーで物を持っている場合に落としたりする場合があるので、サーボモータの角度を少しずつ変化させます。
本プログラムでは、10ms毎に各サーボモータの回転角度を変更しています。

２．複数のサーボモータを同時に動作させる。
本プログラムでは、10ms毎にサーボモータの動作処理を行いますが、６個のサーボモータを同時に動作させる必要があるので、10msの時間を取るのにtime.sleep()関数は使用していません。
time.sleep()関数は設定時間が来るまで抜け出ないので処理が止まってしまうからです。
変わりに以下、time.ticks_ms()関数とtime.ticks_diff()関数を使用しています。

time.ticks_ms()関数で前回サーボモータ動作処理を行った時間を記憶しておき、現在の時間から引き算し、その差が10ms以上となった時に、再びサーボモータの動作処理を行っています。

以下にその部分のコードを示します。

start_tim = time.ticks_ms() #現在時間初期セット
while True:
    #10msec経過毎に処理を行う
    if time.ticks_diff(time.ticks_ms(), start_tim) >= 10:
　      start_tim = time.ticks_ms() #現在時間記憶
        #------以下に10msec毎に行うサーボモータの動作処理を書く----

上記コードの４行目の
if time.ticks_diff(time.ticks_ms(), start_tim) >= 10:
で前回の処理から10ms経過したかを判断しています。10ms経過した場合は、現在時刻を記憶して今回のサーボモータの動作処理を行います。

time.ticks_ms()関数で得た値同士を直接引き算すると、ラップアラウンド(関数の内部変数がオーバーフローして０からカウントし直すこと)の関係で正しい値が得られない場合があります。time.ticks_diff()関数はラップアラウンドに関係なしに２つの時間の差を求めることが出来ます。

ロボットアーム制御プログラム全体

以下にロボットアームの制御プログラム全体を示します。
このプログラムでは、ロボットアームの各サーボモータ(J1,J2,—,J6)の移動角度が２次元配列のリスト(mov_list)に記載されていて、そのリストの順に動作します。
このリストは、J1,J2,—-J6の順に角度が記載されているリストを、更に要素とするリストとなっており２次元の配列となっています。

mov_list = [[-16.0, 90.0, 25.0, 0.0, 90.0, -24.0], #第1番目の動作角度
[19.0, 73.0, 11.0, 36.0, 90.0, -24.0], #第2番目の動作角度
[19.0, 41.0, 10.0, 63.6, 90.0, -24.0], #第3番目の動作角度
[19.0, 41.0, 10.0, 63.6, 90.0, 14.0], #第4番目の動作角度
[19.0, 74.0, -8.0, 63.6, 90.0, 14.0], #第5番目の動作角度
[-58.0, 74.0, -8.0, 63.6, 90.0, 14.0], #第6番目の動作角度
[-65.0, 31.0, -5.0, 63.6, -62.6, 14.0], #第7番目の動作角度
[-65.0, 31.0, -5.0, 63.6, -62.6, -24.0], #第8番目の動作角度
[-65.0, 83.0, 2.6, 63.6, -62.6, -24.0], #第9番目の動作角度
[-3.0, 83.0, 2.6, 63.6, 90.0, -24.0], #第10番目の動作角度
[-16.0, 90.0, 25.0, 0.0, 90.0, -24.0]] #第11番目の動作角度

上記例では最初にJ1～J6の各サーボモータが、J1:-16.0°、J2:90.0°、J3:25.0°、J4:0.0°、J5:90.0°、J6:-24.0°の角度に動作する。
その次にJ1:19.0°、J2:73.0°、J3:11.0°、J4:36.0°、J5:90.0°、J6:-24.0°の角度に動作する。
その次にJ1:19.0°、J2:41.0°、J3:10.0°、J4:63.0°、J5:90.0°、J6:-24.0°の角度に動作する。
この様に順次にサーボモータが動作します。

プログラムでは、最初のリストのJ1,J2,—,J6軸の角度は、mov_list[0][0], mov_list[0][1], —–, mov_list[0][5]で得ています。２番目のリストのJ1,J2,—,J6軸の角度は、mov_list[1][0], mov_list[1][1], —–, mov_list[1][5]で得ています。

このmov_listの内容を書き換えればロボットアームの動作を変更することが出来ます。

【全プログラム】

from machine import PWM, Pin
import time

#サーボモータの設定
servo1 = PWM(Pin(0), freq = 50)	#GPIO 0がサーボ1
servo2 = PWM(Pin(1), freq = 50)	#GPIO 1がサーボ2
servo3 = PWM(Pin(2), freq = 50)	#GPIO 2がサーボ3
servo4 = PWM(Pin(3), freq = 50)	#GPIO 3がサーボ4
servo5 = PWM(Pin(4), freq = 50)	#GPIO 4がサーボ5
servo6 = PWM(Pin(5), freq = 50)	#GPIO 5がサーボ6

plu90_duty = float(2.425 / 20.0)	#90deg時のデユーティ比
min90_duty = float(0.575 / 20.0)	#-90deg時のデユーティ比

#6軸分の現在角度リスト
cur_deg_list = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]	#J1軸、J2軸----,J6軸の順

#6軸分の移動時の10msec毎の移動角度リスト
inc_deg_list = [0.8, 0.2, 0.2, 0.2, 0.6, 0.2]	#J1軸、J2軸----,J6軸の順

#6軸分の移動目標角度リスト
mov_pos_list = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]	#J1軸、J2軸----,J6軸の順

#6軸分の移動フラグリスト　(true: 移動中、 false:　移動停止)
mov_flag_list = [False, False, False, False, False, False]	#J1軸、J2軸----,J6軸の順

mov_step = 0		#移動位置ステップ番号
mov_stepping_flag = False	#移動中

#移動位置リスト(J1軸,J2軸,----J6軸の順)
mov_list = [[-16.0, 90.0, 25.0, 0.0, 90.0, -24.0],		#第1番目の動作角度
             [19.0, 73.0, 11.0, 36.0, 90.0, -24.0],		#第2番目の動作角度
             [19.0, 41.0, 10.0, 63.6, 90.0, -24.0],		#第3番目の動作角度
             [19.0, 41.0, 10.0, 63.6, 90.0, 14.0],		#第4番目の動作角度
             [19.0, 74.0, -8.0, 63.6, 90.0, 14.0],		#第5番目の動作角度
             [-58.0, 74.0, -8.0, 63.6, 90.0, 14.0],		#第6番目の動作角度
             [-65.0, 31.0, -5.0, 63.6, -62.6, 14.0],	#第7番目の動作角度
             [-65.0, 31.0, -5.0, 63.6, -62.6, -24.0],	#第8番目の動作角度
             [-65.0, 83.0, 2.6, 63.6, -62.6, -24.0],	#第9番目の動作角度
             [-3.0, 83.0, 2.6, 63.6, 90.0, -24.0],		#第10番目の動作角度
             [-16.0, 90.0, 25.0, 0.0, 90.0, -24.0]]		#第11番目の動作角度

#指定角度にサーボモータを動かす関数
#パラメータ
#axNo: サーボモータ番号 1～6
#deg: 移動角度　-90～90
def sv_mov(axNo, deg):
    duty = int(65535 * (min90_duty + (plu90_duty - min90_duty) * (deg + 90) /180))
    if axNo == 1:
        servo1.duty_u16(duty)
    elif axNo == 2:
        servo2.duty_u16(duty)
    elif axNo == 3:
        servo3.duty_u16(duty)
    elif axNo == 4:
        servo4.duty_u16(duty)
    elif axNo == 5:
        servo5.duty_u16(duty)
    elif axNo == 6:
        servo6.duty_u16(duty)

#移動動作実行処理関数
def mov_manage():
    global mov_flag_list
    global cur_deg_list
    global mov_stepping_flag
    
    moving_flag = False		#軸動作中フラグ
    
    for i in range(6):		#6軸分繰り返し
        if mov_flag_list[i] == True:	#サーボモータ[i+1]　動作中？
            moving_flag = True	#軸動作中フラグセット
            if cur_deg_list[i] < mov_pos_list[i]:
                cur_deg_list[i] = cur_deg_list[i] + inc_deg_list[i]	#位置インクリメント
                if cur_deg_list[i] >= mov_pos_list[i]:	#目標位置到達
                    cur_deg_list[i] = mov_pos_list[i]
                    mov_flag_list[i] = False	#位置決め完了
            else:
                cur_deg_list[i] = cur_deg_list[i] - inc_deg_list[i]	#位置デクリメント
                if cur_deg_list[i] <= mov_pos_list[i]:	#目標位置到達
                    cur_deg_list[i] = mov_pos_list[i]
                    mov_flag_list[i] = False	#位置決め完了
 
            sv_mov(i + 1, cur_deg_list[i])		#サーボモータ[i+1]　目標位置移動
        
    if moving_flag == True:	#軸動作中？
        #各軸の現在角度をprint
        pos_list = [0, 0, 0, 0, 0, 0]
        for i in range(6):
            pos_list[i] = str(float(int(cur_deg_list[i] * 10.0)) / 10.0)	#→int→str変換
            print("J1:" + pos_list[0] +
                ",J2:" + pos_list[1] +
                ",J3:" + pos_list[2] +
                ",J4:" + pos_list[3] +
                ",J5:" + pos_list[4] +
                ",J6:" + pos_list[5] +
                ",  STEP:" + str(mov_step))
    else:
        mov_stepping_flag = False	#移動完了
        
#移動位置インタープリター処理
#リターン値
#True: 動作継続、 False:　動作終了
def interpreter():
    global mov_flag_list
    global mov_step
    global mov_pos_list
    global mov_stepping_flag
    
    if mov_stepping_flag == False:
        if mov_step < len(mov_list):
            for i in range(6):
                mov_pos_list[i] = mov_list[mov_step][i]	#移動目標位置設定
                mov_flag_list[i] = True	#移動要求フラグセット
                
            mov_step = mov_step + 1
            mov_stepping_flag = True	#移動ステップ実施中フラグセット
            
            return True		#動作継続
        else:
            return False	#動作終了
            
            
#---------メインプログラム----------
start_tim = time.ticks_ms()	#現在時間初期セット
sw = Pin(16, Pin.IN, Pin.PULL_UP )			#"GP16をプルアップ付き入力とする

#初期スタート位置へ移動
sv_mov(2, 90.0)
cur_deg_list[1] =90.0
sv_mov(3, 25.0)
cur_deg_list[2] = 25.0
sv_mov(4, 0.0)
cur_deg_list[3] = 0.0
sv_mov(1, 0.0)
cur_deg_list[0] = 0.0
sv_mov(5, 90.0)
cur_deg_list[4] = 90.0
sv_mov(6, 0.0)
cur_deg_list[5] = 0.0

time.sleep(1.0)

while True:
    while True:	#スイッチ入力待ち
        if sw.value() == 0:
            break
        
    mov_step = 0
    while True:
        #10msec経過毎に移動動作実行処理を行う
        if time.ticks_diff(time.ticks_ms(), start_tim) >= 10:
            start_tim = time.ticks_ms()	#現在時間記憶
            mov_manage()				#移動動作実行処理
        
        if interpreter() == False:		#移動位置インタープリター処理
            break	
            

【プログラム説明】
主なグローバル変数説明：

◎プログラム行説明
４７行目：
def sv_mov(axNo, deg):
sv_mov()関数は、サーボモータの軸を指定した角度にする関数です。axNoはサーボ番号（1～6)、degは角度(度)を指定します。
+90°の時のPWMのデューティ比と-90°の時のデューティ比から指定角度に対するデューティ比を比例計算して、duty_u16()関数を使用して、サーボモータの軸の角度を変更しています。

６３行目：
def mov_manage():
mov_manage()関数は、全サーボモータの軸の角度変更を行う関数です。10msec毎に実行されます。各サーボモータの移動フラグmov_flag_list[サーボ番号-1]がTrueの時、inc_deg_list[サーボ番号-1]だけ各サーボモータの軸の角度を変更します。

また、角度変更後、全サーボモータ軸の角度値をprint文でプリントします。

８６行目～９７行目：
動作中に、各サーボモータの軸の角度をprintしています。printした内容は、Thonny画面の下側の欄に表示されます。

１０４行目：
def interpreter():
interpreter()関数は、mov_listの各サーボモータの角度リストを解釈して、各サーボモータの最終角度リスト(mov_pos_list)を設定し、動作フラグリスト(mov_flag_list)の各要素をTrueにします。
mov_flag_list[サーボ番号-1]がTrueになると、10ms毎に呼び出されるmov_manage()関数で対応するサーボモータの回転処理が実行されます。

１５２行目、１５４行目：
if time.ticks_diff(time.ticks_ms(), start_tim) >= 10:
if文で前回のサーボモータ動作処理から10msec経過したかを判断します。経過した場合はサーボモータ動作処理のmov_manage()関数を実効します。

最後に

Raspberry Pi Pico Wによるロボットアームの制御方法をサーボモータの制御方法を中心に紹介しました。
今後は、Wi-Fi使用によるスマホからのロボットアームの遠隔操作やインテリジェント化にトライしたいと思います。