電子工作と制御ソフト

電子工作と、それに伴う制御ソフトに関するブログです

楽しい工作シリーズ(タミヤ)の部材を使用して、スカラ型のリンクロボットを作成した（制御編：コントローラRaspberry Pi Pico W使用）

管理者

,

タミヤの楽しい工作シリーズの部材を組み合わせて、スカラ型のリンクロボットを作成してみました。
このロボットは、３個の模型用サーボモータを使用して、先端部の水平面上の直線補間動作と、上下動が可能です。先端にボールペンを取り付けて、図形を描いてみました。

コントローラはRaspberry Pi Pico Wを使用して、プログラムはMicroPythonを使用して作成しました。
本記事では、このロボットの制御に関して記載します。ロボットの組立に関しては、以前の記事を参照してください。

本ロボット組み立てに関する記事はこちら

図1．完成したスカラ型のリンクロボット

１．ロボットの動作動画

下は、このロボットを動作した時の動画です。
ロボットの先端に接続したボールペンで直線や円の図形を描いています。

２．Raspberry Pi Pico Wに関して

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

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

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

図２.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がディスクドライブとして認識されます。

図３．Pico WのBOOTSELボタンとUSBコネクタ

２．エクスプローラで見て、「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の画面です。

図4.Thonny画面

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

本ロボットは、各アームの動作にサーボモータMG 996Rを使用しました。

図５．使用サーボモータ

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

図６．PWM信号に関して



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

図７．サーボモータのPWM信号

今回使用したサーボモータMG 996Rは、パルス幅0.5ms (デューティ比2.5％)時に-90°、パルス幅1.5ms(デューティ比7.5％)時に0°、パルス幅2.4ms(デューティ比12%)時に90°の位置に回転します。また、回転方向は、-90°が時計方向、90°が反時計方向です。

５．スカラ型リンクロボットの制御

５－１．原理

図８に今回作成したロボットの各部の寸法を示します。
２個のサーボモータ軸間の長さをB1、サーボモータに接続される最初のアームの長さがL1、次のアームの長さがL2です。
L1とL2アームの長さの関係は、サーボモータ１とサーボモータ２で同様です。

図８．ロボット各部の寸法

ロボットの座標系(X-Y座標系)は、図９に示した様にサーボモータ１とサーボモータ２の真ん中の位置を原点(0, 0)とし、右側がX軸のプラス方向、上側がY軸のプラス方向とします。

サーボモータ１の軸の角度Θ1とサーボモータ２の軸の角度Θ2が決まると先端部のX-Y座標値が決まります。

実際の制御に関しては、ロボット先端部の座標値(X, Y)からΘ1とΘ2を決めサーボモータを制御する必用があります。

図９．先端部座標値からサーボモータ軸角度を得る方法

ロボット先端部の座標値(X, Y)からサーボモータ軸の角度値Θ1とΘ2を求める方法は以下の通りです。

【Θ1を求める方法】
①先端部の座標値(X, Y)を中心とした半径L2の円と、サーボモータ１の軸位置の座標値(-B1/2, 0)を中心とした半径L1の円の２個の交点を求め、その内X座標値が小さい方がリンク部分の座標なので、それを(X1, Y1)とします。

②リンク部分の座標(X1, Y1)と、サーボモータ１の軸の座標値(-B1/2, 0)を使用して、arctan計算をして、サーボモータ１軸の角度値Θ1を求めます。

【Θ2を求める方法】
①先端部の座標値(X, Y)を中心とした半径L2の円と、サーボモータ２の軸位置の座標値(B1/2, 0)を中心とした半径L1の円の２個の交点を求め、その内X座標値が大きい方がリンク部分の座標なので、それを(X2, Y2)とします。

②リンク部分の座標(X2, Y2)と、サーボモータ２の軸の座標値(B1/2, 0)を使用して、arctan計算をして、サーボモータ２軸の角度値Θ2を求めます。

尚、２個の円の交わる点の座標は以下の様に計算することができます。

５－２．直線補間動作

ロボットの先端部分を直線状に動作させることを、直線補間動作と呼びます。
ロボット先端部分のX-Y座標位置が分れば、前記した様に２個のサーボモータ軸の角度が計算できるので、先端部分の座標位置を逐次直線状になるように計算して、そこからその時々のサーボモータ軸の角度を求めて動作させます。

本プログラムでは、20msec毎にロボットの先端部分の座標位置を計算して、そこから必用とするサーボモータ軸の角度を求めて駆動しています。

20msec毎のロボットの先端部分の座標値の計算は、現在のロボット先端部のX,Yの現在座標値に、それぞれ予め計算した20msecの間の移動量を足し算することによって求めています。（図１０)

図１０．直線補間説明図

５－３．円弧補間動作

今回作成したプログラムでは、前記の直線補間の動作を細かく組み合わせることによって、ロボット先端部を円弧上に動作させることを行っています。

６．ロボットのコントローラの回路

図１１はロボットのコントローラの回路図です。３個のサーボモータをRaspberry Pi Pico Wに接続しました。
それぞれのサーボモータのPWM信号線をGP0～GP2のI/Oに接続しました。
動作スタート用のスイッチをGP16のI/Oに接続しました。

図１1．ロボットコントローラ回路

７．制御プログラムに関して

ロボットを制御するRaspberry Pi Pico WのプログラムはMicroPythonを使用しました。
作成したプログラムの主要な部分を以下に説明します。

７－１．ロボット先端の座標から、サーボモータ軸の角度を求める

図１２に示すようなロボットアームの先端部のX,Y座標値から、サーボモータ１とサーボモータ２の軸の角度を求める部分のプログラムコードを以下に示します。

図１２.ロボット先端部座標値とサーボモータ軸の角度
#ロボット諸元
BL = 35	#ベース部(サーボモータ軸間)の長さ(mm)
L1 = 55	#リンク１部の長さ(mm)
L2 = 75	#リンク２部の長さ(mm)


#アームの先端座標から、各サーボ軸の回転角度を求める
#引き数
#x: アーム先端のX座標値、y: アーム先端のY座標値
#リターン値
#deg1: サーボ１の角度(度単位)、deg2: サーボ２の角度(度単位)、結果 OK: True
def cal_ang(x: float, y: float):
    #X軸マイナス側の中間リンク点位置検出(サーボ１側)
    x1, y1, res = circle_intersection(x, y , L2, -BL/2, 0, L1, False)
    if res == False:
        return 0.0, 0.0, False	#エラー終了
        
    #X軸プラス側の中間リンク点位置検出(サーボ２側)
    x2, y2, res = circle_intersection(x, y, L2, BL/2, 0, L1, True)
    if res == False:
        return 0.0, 0.0, False	#エラー終了
    
    #---サーボ１側の角度計算(CCW回転方向正)
    if -(BL / 2) < x1:	#サーボ角異常(±90°を越えている)
        print("サーボ１の軸角度が±90degを越えている")
        return 0.0, 0.0, False	#エラー終了
    elif -(BL / 2) == x1:
        if y1 > 0.0:
            ang1 = -math.pi / 2.0	#-π/2
        else:
            ang1 = math.pi / 2.0	#π/2
    else:
        ang1 = -math.atan(y1 / (-(BL / 2) - x1))#サーボ１角度(CCW回転が正)
    
    #---サーボ２側の角度計算(CCW回転方向正)
    if x2 < (BL / 2):	#サーボ角異常(±90°を越えるている)
        print("サーボ２の軸角度が±90degを越えている")
        return 0.0, 0.0, False	#エラー終了
    elif x2 == (BL / 2):
        if y > 0.0:
            ang2 = math.pi / 2.0	#π/2
        else:
            ang2 = -math.pi / 2.0	#-π/2
    else:
        ang2 = math.atan(y2 / (x2 - (BL / 2)))	#サーボ２角度(CCW回転が正)
                
    deg1 = 180.0 * ang1 / math.pi	#度に変換
    deg2 = 180.0 * ang2 / math.pi	#度に変換
 
    return deg1, deg2, True		#正常終了

【プログラム説明】
２行目～４行目：
BL = 35 #ベース部(サーボモータ軸間)の長さ(mm)
L1 = 55 #リンク１部の長さ(mm)
L2 = 75 #リンク２部の長さ(mm)
は、ロボットの諸元を定義しています。

１４行目：
x1, y1, res = circle_intersection(x, y , L2, -BL/2, 0, L1, False)
は、ロボットの先端部を中心として、リンク２のアームの長さを半径とする円と、サーボモータ１の軸部分の座標を中心として、リンク１のアームの長さを半径とする円の交点の座標を求めています。
これは、サーボモータ１側のアーム間のジョイント部分の座標です。

１９行目：
x2, y2, res = circle_intersection(x, y, L2, BL/2, 0, L1, True)
は、ロボットの先端部を中心として、リンク２のアームの長さを半径とする円と、サーボモータ２の軸部分の座標を中心として、リンク１のアームの長さを半径とする円の交点の座標を求めています。
これは、サーボモータ２側のアーム間のジョイント部分の座標です。

２円の交点を求めるcircle_intersection()関数のコードは、次項(7-2)を参照して下さい。

３３行目：
ang1 = -math.atan(y1 / (-(BL / 2) – x1))#サーボ１側角度(CCW回転が正)
は、arctan 計算をしてサーボモータ１の軸の角度を計算しています。

４５行目：
ang2 = math.atan(y2 / (x2 – (BL / 2))) #サーボ２側角度(CCW回転が正)
は、arctan 計算をしてサーボモータ２の軸の角度を計算しています。

７－２．２個の円の交点の座標を求める

以下は、５－１項に示した計算に従って、２個の円の交点を求める関数のプログラムコードです。

#交わる２円の交点の座標を求める
#パラメータ
#X1	円1中心X座標
#Y2	円1中心Y座標
#R1	円1半径
#X2	円2中心X座標
#Y2	円2心Y座標
#R2	円2半径
#plus_flag	x座標で大きい方の交点を検出 True、小さい方の交点検出 False
#リターン値
#X	検出交点X座標
#Y	検出交点Y座標
#結果　OK: True, NG: False
def circle_intersection(x1: float, y1: float, r1: float, x2: float, y2: float, r2: float, plus_flag: bool):
    x = 0.0			#検出座標X
    y = 0.0			#検出座用Y
    
    dx = x2 - x1
    dy = y2 - y1
    d = math.sqrt(dx * dx + dy * dy)	#中心間の距離
    
    if d < 0.000001:
        if abs(r1 - r2) < 0.000001:
            print("同一円なので交点無")
        else:
            print("同心円で交点無")
        return x, y, False	#エラー終了
    if d > r1 + r2 + 0.000001:
        print("円が離れていて交点無")
        return x, y, False	#エラー終了
    if d < abs(r1 -  r2) - 0.000001:
        print("一方が他方に内包され交点無")
        return x, y, False	#エラー終了
    
    #交点計算
    a = (r1**2 - r2**2 + d**2) / (2.0 *  d)
    h2 = r1**2 - a**2
    
    if h2 < 0:
        if h2 > -0.000001:
            h2 = 0
        else:
            print("数値誤差で交点なし")
            return x, y, False	#エラー終了
        
    h = math.sqrt(h2)
    
    #規準点 p3
    x3 = x1 + a * dx / d
    y3 = y1 + a * dy / d
    
    #垂直方向オフセット
    rx = -dy * (h / d)
    ry = dx * (h / d)
    
    #交点
    if plus_flag == True:	#X軸がプラス側の交点検出
        x = x3 + rx
        y = y3 + ry
    else:					#X軸がマイナス側の交点検出
        x = x3 - rx
        y = y3 - ry

    return x, y, True		#正常終了

７－３．直線補間

ロボットの先端部を直線状に動作させる直線補間を行うためのプログラムコードを以下に示します。

x_cur_pos = 0.0		#アーム先端部の現在位置X座標
y_cur_pos = 80.0	#アーム先端部の現在位置Y座標
speed = 200.0		  #アーム先端部の動作スピード(mm/sec)
delay_time = 20		#処理待ち時間(20msec)


#直線補間動作
#パラメータ
#x: X軸補間終了点の座標
#y: Y軸補間終了点の座標
def line_interpolation(x: float,  y: float):
    global x_cur_pos
    global y_cur_pos
        
    def_x = x - x_cur_pos
    def_y = y - y_cur_pos
    length = math.sqrt(def_x * def_x + def_y * def_y)
    if length == 0:
        return
    
    #処理時間毎の各軸移動量の計算
    inc_x = speed * def_x * delay_time / (length * 1000.0)
    inc_y = speed * def_y * delay_time / (length * 1000.0)
    
    endFlag_x = False	#X軸側終了フラグ
    endFlag_y = False	#Y軸側終了フラグ
    
    while True:
        x_cur_pos = x_cur_pos + inc_x
        if inc_x > 0:
            if x_cur_pos >= x:
                x_cur_pos = x
                endFlag_x = True
        else:
            if x_cur_pos <= x:
                x_cur_pos = x
                endFlag_x = True
        
        y_cur_pos = y_cur_pos + inc_y
        if inc_y > 0:
            if y_cur_pos >= y:
                y_cur_pos = y
                endFlag_y = True
        else:
            if y_cur_pos <= y:
                y_cur_pos = y
                endFlag_y = True
                
        #サーボモータの角度値計算
        deg1, deg2, res = cal_ang(x_cur_pos, y_cur_pos)
        if res == False:
            print("位置設定異常")
            break
        
        sv_mov(1, deg1)	#サーボ１回転
        sv_mov(2, deg2)	#サーボ２回転
        print("deg1 = " + str(deg1) + ", deg2 = " + str(deg2) +
              ", x=" + str(x_cur_pos) + ", y=" + str(y_cur_pos))
       
        #位置決め終了
        if ((endFlag_x == True) and (endFlag_y == True)):
            break
        
        time.sleep_ms(delay_time)	#次回処理時間待ち

【プログラム説明】
１５行目～１７行目：
def_x = x – x_cur_pos
def_y = y – y_cur_pos
length = math.sqrt(def_x * def_x + def_y * def_y)
は、直線補間の終点から始点を引き算して、Ｘ軸方向とＹ軸方向のそれぞれの移動量(def_x, def_y)と進行方向の移動量(length)を求めています。

２２行目、２３行目：
inc_x = speed * def_x * delay_time / (length * 1000.0)
inc_y = speed * def_y * delay_time / (length * 1000.0)
直線補間では、20msec毎にX, Y軸方向の移動位置を計算しています。
ここでは、20msecの間のX方向の移動量(inx_x)とY軸方向(inc_y)の移動量を計算しています。

２９行目：
x_cur_pos = x_cur_pos + inc_x
は、前回のロボット先端部のＸ座標値に20msecの移動量を加算して、今回のX座標上の位置を求めています。

３９行目：
y_cur_pos = y_cur_pos + inc_y
は、前回のロボット先端部のＹ軸座値に20msecの移動量を加算して、今回のＹ座標上の位置を求めています。

５０行目：
deg1, deg2, res = cal_ang(x_cur_pos, y_cur_pos)
は、ロボット先端の座標値からサーボモータ１とサーボモータ２の軸の角度を求めています。

５５行目、５６行目：
sv_mov(1, deg1) #サーボ１回転
sv_mov(2, deg2) #サーボ２回転
は、５０行目で得られたサーボモータ軸の角度から、実際にサーボモータを動作させています。
関数sv_mov()のプログラムコードは、7-5項を参照してください。

６４行目：
time.sleep_ms(delay_time) #次回処理時間待ち
20msec毎にロボット先端の位置を計算していますので、次の計算まで待ちます。(delay_time = 20です)

７－４．上下駆動

ロボットアームを上下動させる部分のプログラムを以下に示します。
上下動は図１３のように、サーボモータ３を動作させることによって行います。
上下動は一気に動作させると、ロボット先端に振動を発生するので、20msec毎に少しずつ動かして最終位置に移動させるようにしています。

図１３．ロボットの上下機構
#上下用サーボモータの位置角度
up_deg = -60.0		#上昇時のサーボモータ角度(度単位)
down_deg = -40.0	#下降時のサーボモータ角度(度単位)

z_cur_deg = up_deg	#上下軸のサーボモータ角度

#アーム上昇
def arm_up():
    global z_cur_deg
    end_flag = False
    while True:
        z_cur_deg = z_cur_deg - 1.0
        if z_cur_deg < up_deg:
            z_cur_deg = up_deg
            end_flag = True		#終了フラグセット
        sv_mov(3, z_cur_deg)
        if end_flag == True:	#終了？
            break
        time.sleep_ms(delay_time)	#次回処理時間待ち 
    
#アーム下降
def arm_down():
    global z_cur_deg
    end_flag = False
    while True:
        z_cur_deg = z_cur_deg + 1.0
        if z_cur_deg > down_deg:
            z_cur_deg = down_deg
            end_flag = True		#終了フラグセット
        sv_mov(3, z_cur_deg)
        if end_flag == True:	#終了？
            break
        time.sleep_ms(delay_time)	#次回処理時間待ち

【プログラム説明】
２行目、３行目：
up_deg = -60.0 #上昇時のサーボモータ角度(度単位)
down_deg = -40.0 #下降時のサーボモータ角度(度単位)
は、上昇時のサーボモータ軸の角度(up_deg)と、下降時のサーボモータ軸の角度(down_deg)を定義しています。

１２行目：
z_cur_deg = z_cur_deg – 1.0
は、上昇の処理で20msec毎にサーボモータの軸角度を1.0°減らしています。

１９行目：
time.sleep_ms(delay_time) #次回処理時間待ち
は、次の1.0°軸角度を減らす処理まで20msec待っています。(delay_time = 20です)

２６行目：
z_cur_deg = z_cur_deg + 1.0
は、下降の処理で20msec毎にサーボモータの軸角度を1.0°増やしています。

３３行目：
time.sleep_ms(delay_time) #次回処理時間待ち
は、次の1.0°軸角度を増やす処理まで20msecまっています。(delay_time = 20です)

７－５．サーボモータの駆動

下記のsv_mov()関数はサーボモータ軸を指定角度(deg)に回転させる関数です。
パラメータaxNo (1～3)の指定で、対象とするサーボモータ (サーボモータ１～サーボモータ３)を指定することができます。

#サーボモータの設定
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(上下)

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

#サーボモータの0度のオフセット
sv1_offset = 23.0	#サーボモータ１の0度のオフセット
sv2_offset = 0.0	#サーボモータ２の0度のオフセット

#指定角度にサーボモータを動かす関数
#パラメータ
#axNo: サーボモータ番号 1～3
#deg: 移動角度　-90～90
def sv_mov(axNo, deg):
    if axNo == 1:
        deg_val = deg - sv1_offset	#0°点のオフセット値加味
    elif axNo == 2:
        deg_val = deg - sv2_offset	#0°点のオフセット値加味
    elif axNo == 3:
        deg_val = deg
        
    duty = int(65535 * (min90_duty + (plu90_duty - min90_duty) * (deg_val + 90) /180))
    
    if axNo == 1:
        servo1.duty_u16(duty)
    elif axNo == 2:
        servo2.duty_u16(duty)
    elif axNo == 3:
        servo3.duty_u16(duty)

【プログラム説明】
２行目～４行目：
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(上下)
は、Raspberry Pi Pico WのI/OのGP0～GP2に各サーボモータのPWM信号を割り当てています。PWM周波数50Hzに設定しています。

６行目，７行目：
plu90_duty = float(2.4 / 20.0) #90deg時のデユーティ比
min90_duty = float(0.5 / 20.0) #-90deg時のデユーティ比

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

１０行目、１１行目：
sv1_offset = 23.0 #サーボモータ１の0度のオフセット
sv2_offset = 0.0 #サーボモータ２の0度のオフセット

sv1_offsetとsv2_offsetはsv_mov()関数のパラメータdegでサーボモータ軸の角度を０°に指定した時に、サーボモータに接続したアームが図１２で示した０°の位置になるように調整するための変数です。実態に合わせて指定しています。

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

２８行目：
servo1.duty_u16(duty)
サーボモータ１のPWM信号のデューティ比を、servo1.duty_u16(duty)のdutyで指定し、サーボモータ１を動作させます。
dutyは100％を65535とする数値(int)で指定します。例えば20％の時は、65535 * 20/ 100 = 13107 となります。

３０行目：
servo2.duty_u16(duty)
サーボモータ２のPWM信号のデューティ比を、servo2.duty_u16(duty)のdutyで指定し、サーボモータ２を動作させます。
dutyは100％を65535とする数値(int)で指定します。例えば20％の時は、65535 * 20/ 100 = 13107 となります。

３２行目：
servo3.duty_u16(duty)
サーボモータ３のPWM信号のデューティ比を、servo3.duty_u16(duty)のdutyで指定し、サーボモータ３を動作させます。
dutyは100％を65535とする数値(int)で指定します。例えば20％の時は、65535 * 20/ 100 = 13107 となります。

８．ロボットプログラム全体

以下に今回作成したロボット制御プログラム全体を示します。

動作は、プログラムをスタートすると、ロボットアームが初期位置に移動します。
スタートボタン(sw)をONすると、直線補間動作を組み合わせて矩形と矩形内の対角線等を描き、最後に微少直線補間を繰り返し実行して円を描きます。

【全プログラム】

#---------スカラ型リンクロボット制御-------------
from machine import PWM, Pin
import time
import math

sw = Pin(16, Pin.IN, Pin.PULL_UP )	#"GP16をプルアップ付き入力とする

#サーボモータの設定
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(上下)

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

#ロボットの諸元
BL = 35	#ベース部の長さ(mm)
L1 = 55	#リンク１部の長さ(mm)
L2 = 75	#リンク２部の長さ(mm)

#上下用サーボモータの位置角度
up_deg = -60.0		#上昇時のサーボモータ角度(度単位)
down_deg = -40.0	#下降時のサーボモータ角度(度単位)

#サーボモータの0度のオフセット
sv1_offset = 23.0	#サーボモータ１の0度のオフセット
sv2_offset = 0.0	#サーボモータ２の0度のオフセット

x_cur_pos = 0.0		#アーム先端部の現在位置X座標
y_cur_pos = 80.0	#アーム先端部の現在位置Y座標
speed = 200.0		#アーム先端部の動作スピード(mm/sec)
delay_time = 20		#処理待ち時間(20msec)

z_cur_deg = up_deg	#上下軸のサーボモータ角度


#指定角度にサーボモータを動かす関数
#パラメータ
#axNo: サーボモータ番号 1～3
#deg: 移動角度　-90～90
def sv_mov(axNo, deg):
    if axNo == 1:
        deg_val = deg - sv1_offset	#0°点のオフセット値加味
    elif axNo == 2:
        deg_val = deg - sv2_offset	#0°点のオフセット値加味
    elif axNo == 3:
        deg_val = deg
        
    duty = int(65535 * (min90_duty + (plu90_duty - min90_duty) * (deg_val + 90) /180))
    
    if axNo == 1:
        servo1.duty_u16(duty)
    elif axNo == 2:
        servo2.duty_u16(duty)
    elif axNo == 3:
        servo3.duty_u16(duty)
    

#交わる２円の交点の座標を求める
#パラメータ
#X1	円1中心X座標
#Y2	円1中心Y座標
#R1	円1半径
#X2	円2中心X座標
#Y2	円2心Y座標
#R2	円2半径
#plus_flag	x座標で大きい方の交点を検出 True、小さい方の交点検出 False
#リターン値
#X	検出交点X座標
#Y	検出交点Y座標
#結果　OK: True, NG: False
def circle_intersection(x1: float, y1: float, r1: float, x2: float, y2: float, r2: float, plus_flag: bool):
    x = 0.0			#検出座標X
    y = 0.0			#検出座用Y
    
    dx = x2 - x1
    dy = y2 - y1
    d = math.sqrt(dx * dx + dy * dy)	#中心間の距離
    
    if d < 0.000001:
        if abs(r1 - r2) < 0.000001:
            print("同一円なので交点無")
        else:
            print("同心円で交点無")
        return x, y, False	#エラー終了
    if d > r1 + r2 + 0.000001:
        print("円が離れていて交点無")
        return x, y, False	#エラー終了
    if d < abs(r1 -  r2) - 0.000001:
        print("一方が他方に内包され交点無")
        return x, y, False	#エラー終了
    
    #交点計算
    a = (r1**2 - r2**2 + d**2) / (2.0 *  d)
    h2 = r1**2 - a**2
    
    if h2 < 0:
        if h2 > -0.000001:
            h2 = 0
        else:
            print("数値誤差で交点なし")
            return x, y, False	#エラー終了
        
    h = math.sqrt(h2)
    
    #規準点 p3
    x3 = x1 + a * dx / d
    y3 = y1 + a * dy / d
    
    #垂直方向オフセット
    rx = -dy * (h / d)
    ry = dx * (h / d)
    
    #交点
    if plus_flag == True:	#X軸がプラス側の交点検出
        x = x3 + rx
        y = y3 + ry
    else:					#X軸がマイナス側の交点検出
        x = x3 - rx
        y = y3 - ry

    return x, y, True		#正常終了

#アームの先端座標から、各サーボ軸の回転角度を求める
#引き数
#x: アーム先端のX座標値、y: アーム先端のY座標値
#リターン値
#deg1: サーボ１の角度(度単位)、deg2: サーボ２の角度(度単位)、結果 OK: True
def cal_ang(x: float, y: float):
    #X軸マイナス側の中間リンク点位置検出(サーボ１側)
    x1, y1, res = circle_intersection(x, y , L2, -BL/2, 0, L1, False)
    if res == False:
        return 0.0, 0.0, False	#エラー終了
        
    #X軸プラス側の中間リンク点位置検出(サーボ２側)
    x2, y2, res = circle_intersection(x, y, L2, BL/2, 0, L1, True)
    if res == False:
        return 0.0, 0.0, False	#エラー終了
    
    #---サーボ１側の角度計算(CCW回転方向正)
    if -(BL / 2) < x1:	#サーボ角異常(±90°を越えている)
        print("サーボ１の軸角度が±90degを越えている")
        return 0.0, 0.0, False	#エラー終了
    elif -(BL / 2) == x1:
        if y1 > 0.0:
            ang1 = -math.pi / 2.0	#-π/2
        else:
            ang1 = math.pi / 2.0	#π/2
    else:
        ang1 = -math.atan(y1 / (-(BL / 2) - x1))#サーボ１角度(CCW回転が正)
    
    #---サーボ２側の角度計算(CCW回転方向正)
    if x2 < (BL / 2):	#サーボ角異常(±90°を越えるている)
        print("サーボ２の軸角度が±90degを越えている")
        return 0.0, 0.0, False	#エラー終了
    elif x2 == (BL / 2):
        if y > 0.0:
            ang2 = math.pi / 2.0	#π/2
        else:
            ang2 = -math.pi / 2.0	#-π/2
    else:
        ang2 = math.atan(y2 / (x2 - (BL / 2)))	#サーボ２角度(CCW回転が正)
                
    deg1 = 180.0 * ang1 / math.pi	#度に変換
    deg2 = 180.0 * ang2 / math.pi	#度に変換
 
    return deg1, deg2, True		#正常終了


#直線補間動作
#パラメータ
#x: X軸補間終了点の座標
#y: Y軸補間終了点の座標
def line_interpolation(x: float,  y: float):
    global x_cur_pos
    global y_cur_pos
        
    def_x = x - x_cur_pos
    def_y = y - y_cur_pos
    length = math.sqrt(def_x * def_x + def_y * def_y)
    if length == 0:
        return
    
    #処理時間毎の各軸移動量の計算
    inc_x = speed * def_x * delay_time / (length * 1000.0)
    inc_y = speed * def_y * delay_time / (length * 1000.0)
    
    endFlag_x = False	#X軸側終了フラグ
    endFlag_y = False	#Y軸側終了フラグ
    
    while True:
        x_cur_pos = x_cur_pos + inc_x
        if inc_x > 0:
            if x_cur_pos >= x:
                x_cur_pos = x
                endFlag_x = True
        else:
            if x_cur_pos <= x:
                x_cur_pos = x
                endFlag_x = True
        
        y_cur_pos = y_cur_pos + inc_y
        if inc_y > 0:
            if y_cur_pos >= y:
                y_cur_pos = y
                endFlag_y = True
        else:
            if y_cur_pos <= y:
                y_cur_pos = y
                endFlag_y = True
                
        #サーボモータの角度値計算
        deg1, deg2, res = cal_ang(x_cur_pos, y_cur_pos)
        if res == False:
            print("位置設定異常")
            break
        
        sv_mov(1, deg1)	#サーボ１回転
        sv_mov(2, deg2)	#サーボ２回転
        print("deg1 = " + str(deg1) + ", deg2 = " + str(deg2) +
              ", x=" + str(x_cur_pos) + ", y=" + str(y_cur_pos))
       
        #位置決め終了
        if ((endFlag_x == True) and (endFlag_y == True)):
            break
        
        time.sleep_ms(delay_time)	#次回処理時間待ち


#アーム上昇
def arm_up():
    global z_cur_deg
    end_flag = False
    while True:
        z_cur_deg = z_cur_deg - 1.0
        if z_cur_deg < up_deg:
            z_cur_deg = up_deg
            end_flag = True		#終了フラグセット
        sv_mov(3, z_cur_deg)
        if end_flag == True:	#終了？
            break
        time.sleep_ms(delay_time)	#次回処理時間待ち 
    
#アーム下降
def arm_down():
    global z_cur_deg
    end_flag = False
    while True:
        z_cur_deg = z_cur_deg + 1.0
        if z_cur_deg > down_deg:
            z_cur_deg = down_deg
            end_flag = True		#終了フラグセット
        sv_mov(3, z_cur_deg)
        if end_flag == True:	#終了？
            break
        time.sleep_ms(delay_time)	#次回処理時間待ち 

#----メインプログラム--------------------------
#アーム上下の初期位置移動
z_cur_deg = up_deg 
sv_mov(3, z_cur_deg)
time.sleep_ms(100)

#サーボモータオフセット調整用
sv_mov(1, 0.0)
sv_mov(2, 45.0)
time.sleep_ms(1000)

sv_mov(1, -45.0)
sv_mov(2, 0.0)
time.sleep_ms(1000)


#アーム初期位置移動
x_cur_pos = 0.0
y_cur_pos = 70.0
deg1, deg2, res = cal_ang(x_cur_pos, y_cur_pos)
if res == True:
    sv_mov(1, deg1)
    sv_mov(2, deg2)

#---連続動作--------
while True:
    #スタートスイッチON待ち 
    while True:
        if sw.value() == 0:	#スイッチ値判断
            break
        time.sleep_ms(10)
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(-20.0,  70.0)
    
    time.sleep_ms(100)
    arm_down()			#アーム下降
    time.sleep_ms(200)
      
    #矩形の描画
    speed = 60.0	#速度60mm/sec
    line_interpolation(-20.0,  70.0)
    line_interpolation(-20.0,  110.0)
    line_interpolation(20.0,  110.0)
    line_interpolation(20.0,  70.0)
    line_interpolation(-20.0,  70.0)
    line_interpolation(20.0, 110.0)
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
     
    speed = 300.0	#速度30mm/sec
    line_interpolation(-20.0, 110.0)
        
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(200)
    
    #矩形内対角線描画
    speed = 60.0	#速度60mm/sec
    line_interpolation(20.0, 70.0)

    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
    
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(-20, 90)
    
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(200)
    
    #横線の描画
    speed = 60.0	#速度60mm/sec
    line_interpolation(20.0, 90.0)
    
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(0.0, 70.0)
    
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(200)
    
    #縦線の描画
    speed = 60.0	#速度60mm/sec
    line_interpolation(0.0, 110.0)
         
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(-20.0, 90)
    
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(100)
    
        
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(0.0, 70)
    
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(100)
       
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(20.0, 90)
    
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(100)
       
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(0.0, 110)
    
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(100)
       
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)
    
    speed = 300.0	#速度300mm/sec
    line_interpolation(-20.0, 70)
    
    time.sleep_ms(100)
    arm_down()	#アーム下降
    time.sleep_ms(200)
    
    speed = 60.0	#速度60mm/sec
    
    #円を描く(半径28.2mm,　CW方向)
    rad = math.pi * 5.0 / 4.0
    while True:
        x = 28.2 * math.cos(rad)
        y = 28.2 * math.sin(rad) + 90.0
        line_interpolation(x, y)
        rad = rad - 0.1	#CW方向移動で角度マイナス
        if rad < ((math.pi * 5.0 / 4.0) - math.pi * 2):
            break
        time.sleep_ms(10)
     
    time.sleep_ms(100)
    arm_up()	#アーム上昇
    time.sleep_ms(100)

９．最後に

タミヤの楽しい工作シリーズの部材を使用して作成したスカラ型リンクロボットをRaspberry Pi Pico Wで制御する方法を紹介しました。

使用したサーボモータの位置決め精度や、アーム等各部のガタのせいか、直線や円の描画が歪んでいます。
サーボモータを高精度の物に変更したり、各部のガタを無くすことによって、もう少し歪の少ない描画が出来るかもしれません。
今後改造を検討したいと思っています。

PAGE TOP