Raspberry Pi Pico Wに模型用のサーボモータを接続して動作させる各種方法を紹介します。一気に目的角度に回転させる方法、一定速で目的の角度に回転させる方法、加減速して目的の角度に回転させる方法を紹介します。プログラム言語はMicroPythonを使用しました。
Raspberry Pi Pico Wに関して
今回サーボモータを制御するコントローラとして、マイコンボード 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がディスクドライブとして認識されます。
２．エクスプローラで見て、「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の画面です。
MicroPythonの実行手順
MicroPythonで作成したプログラムをRaspberry Pi Pico Wで実行する手順に関して以下で説明します。
１．パソコンでThonnyを起動して、Raspberry Pi Pico WをUSBケーブルで接続します。
２．Thonny画面の右下部をクリックして、「MicroPython (Raspberry Pi Pico)—」を選択します。
３．プログラムを書きます。
４．画面左上の
をクリックするとプログラムがRaspberry Pi Pico Wで実行されます。
サーボモータ制御に関して
図４は、今回の実験で使用したサーボモータTower ProのMG996Rです。
サーボモータはPWM信号で制御します。サーボモータから出ている線の、橙線がPWM信号で、赤線が電源線(4.8V～7.2V)で、茶線がGND線です。
PWM信号は、図５の様にパルスのデューティ比が変化する信号です。
サーボモータは、PWM信号のデューティ比を変更することによって、軸の角度が変化します。
また、PWM信号の周波数は50Hzです。50Hzの周期は20msなので20ms内のパルスの幅によってサーボモータの軸の角度が変わります。
例えば図６は、サーボモータのPWM信号波形の例ですが、パルス幅1ms(デューティ比5%)の場合は軸の角度が0°、パルス幅2ms(デューティ比10%)の時は軸の角度が１８０°となります。
MG996Rサーボモータの資料には、パルス幅1msで-90°、1.5msで0°、2msで+90°と書いてありますが、そのパルス幅の範囲では全部で90°程度しか動作しませんでした。
パルス幅0.5ms(デューティ比2.5％)～2.4ms(デューティ比12％)とした場合にトータル180°動作したのでその値を使用することにしました。
パルス幅0.5ms (デューティ比2.5％)時に-90°、パルス幅1.5ms(デューティ比7.5％)時に0°、パルス幅2.4ms(デューティ比12%)時に90°の位置に回転しました。また、回転方向は-90°が時計方向、90°が反時計方向です。
サーボモータ動作回路
図７がサーボモータを動作させるために作成した回路です。Raspberry Pi Pico WのI/OのGP0にサーボモータのPWM信号線(橙色線)を接続しています。サーボモータの電源は電流が大きいので、USBから供給される電源とは別の電源を使用しました。
I/OのGP16にサーボモータ回転スタート用のスイッチを接続しました。
サーボモータ動作プログラム
サーボモータを動作させるプログラムとして、一気に目的角度に回転させるプログラム、一定速で目的の角度に回転させるプログラム、加減速して目的の角度に回転させるプログラムの３通りのプログラムを紹介します。
１．一気に目的角度に回転させるプログラム
サーボモータをRaspberry Pi Pico WのMicroPythonで動作させるためには、PWMをimportして、PWM()関数でPico Wの対象I/OをPWM出力に割り当てます。
servo.duty_16(duty)関数を使用して、PWMのデューティ比を指定することによってサーボモータを指定角度に回転させることが出来ます。
デューティ比は100％を65535とする数値(int)で指定します。例えば20％の時は、65535 * 20/ 100 = 13107 となります。
PWMの周波数は50Hzで周期20msで、使用したサーボモータの場合、
パルス幅2.4ms時に+90°、パルス幅1.5ms時に0°、パルス幅0.5ms時に-90°位置に回転します。
servo.duty_16(duty)関数のdutyは、+90°の時は、
2.4 * 65535 / 20 = 7864となります。(分母の20は周波数50Hzの周期です)
-90°の時は、
0.5 * 65535 / 20 = 1638となります。
以下プログラムはサーボモータを一気に目的角度に回転させるMicroPythonのプログラムです。
スイッチを押すとサーボモータ軸が90°→1秒待ち→-90°→1秒待ち→0°→1秒待ち→-45°→1秒待ち→45°→1秒待ち→-90°の順に回転します。
PWMの周波数は50Hzで周期20msで、パルス幅2.4ms時に+90°、パルス幅1.5ms時に0°、パルス幅0.5ms時に-90°位置に回転します。
90°回転時のPWMのデューティ比と-90°回転時のPWMのデューティ比から、目的の角度のPWMのデューティ比を比例計算しています。
from machine import PWM, Pin
import time
#サーボモータの設定
servo = PWM(Pin(0), freq = 50) #GP0がサーボモータ用PWM出力
plu90_duty = float(2.4 / 20.0) #90deg時のデューティ比
min90_duty = float(0.5 / 20.0) #-90deg時のデューティ比
sw1 = Pin(16, Pin.IN, Pin.PULL_UP) #GP16がプルアップ付きスイッチ入力
#指定角度にサーボモータを動かす関数
#パラメータ
#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:
while True: #スイッチ入力待ち
if sw1.value() == 0:
break
time.sleep_ms(10) #10msec待ち
sv_mov(90)
time.sleep(1) #1秒待ち
sv_mov(-90)
time.sleep(1) #1秒待ち
sv_mov(0)
time.sleep(1) #1秒待ち
sv_mov(-45)
time.sleep(1) #1秒待ち
sv_mov(45)
time.sleep(1) #1秒待ち
sv_mov(-90)
time.sleep(1) #1秒待ち【プログラム説明】
５行目：
servo = PWM(Pin(0), freq = 50)
Raspberry Pi Pico WのI/OのGP0にサーボモータのPWM信号を割り当てています。PWM周波数50Hzに設定しています。
７，８行目：
plu90_duty = float(2.4 / 20.0)
min90_duty = float(0.5 / 20.0)
サーボモータ軸が90°の時のPWMのパルス幅を2.4ms、-90°の時のPWMのパルス幅を0.5msとして、それぞれのデューティ比を計算しています。割り算の分母の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)から指定角度に対するデューティ比を比例計算しています。
２２行目：
if sw1.value() == 0:
は、I/OのGP16に接続した、押し釦スイッチが押されているかを判断する条件判断文です。スイッチが押されると、２６行目以降のsv_mov()関数でサーボモータ軸の回転処理が行われます。
下記動画は上記プログラムを実行してサーボモータを動作させる様子です。
２．一定速度で目的の角度に回転させるプログラム
前記のPWMのパルス幅を一気に変えて、サーボモータ軸を目的の角度に回転させた場合、サーボモータは最高回転数で回ります。この場合に回転するのに振動が発生し問題が発生する場合があります。また、用途によっては遅い速度で回転させたい場合もあります。
このような場合は、PWMのパルス幅を目的の角度に相当する値まで徐々に変化させます。
以下は10ms毎にPWMパルス幅を変更し、サーボモータを定速(低速)で回転させたプログラムです。
スイッチを押すと、速度50°/sで90°に回転→速度100°/sで-90°に回転→速度100°/sで0°に回転→速度50°/sで-45°に回転→速度100°/sで45°に回転→速度25°/sで-90°に回転します。
from machine import PWM,Pin
import time
#サーボモータの設定
servo1 = PWM(Pin(0), freq = 50) #GP0がサーボモータ用PWM出力
plu90_duty = float(2.4 / 20.0) #90deg時のデューティ比
min90_duty = float(0.5 / 20.0) #-90deg時のデューティ比
sw1 = Pin(16, Pin.IN, Pin.PULL_UP ) #GP16がプルアップ付きスイッチ入力
#現在角度
deg_new = 0.0 #単位deg
#指定角度にサーボモータを動かす関数
#パラメータ
#deg: 移動角度 -90～90
def sv_mov(deg):
duty = int(65535 * (min90_duty + (plu90_duty - min90_duty) * (deg + 90) /180))
servo1.duty_u16(duty)
#サーボモータ定速運転処理
#パラメータ
#deg: 移動角度
#speed: 回転速度(deg/sec)
def servoMov(deg, speed):
global deg_new #現在角度
sp_per10ms = speed / 100.0 #10ms単位の目標速度(deg/10ms)
cw_flag = False #CW方向回転フラグ
if deg >= deg_new: #目標位置>=現在位置
cw_flag = False #CCW方向回転
else:
cw_flag = True #CW方向回転
all_length = abs(deg - deg_new) #全移動角度
length_now = 0.0 #現在移動角度
#-----定速回転実施-----------
while True:
if length_now >= all_length:
break #目標位置到達
length_now = length_now + sp_per10ms #移動距離インクリメント
if cw_flag == False: #cw方向回転？
deg_new = deg_new + sp_per10ms #移動角度計算
if deg_new >= deg:
deg_new = deg
else:
deg_new = deg_new - sp_per10ms #移動角度計算
if deg_new <= deg:
deg_new = deg
sv_mov(deg_new) #サーボモータ回転
time.sleep_ms(10) #10msec待ち
#-------------メインプログラム-----------------
sv_mov(-90.0) #サーボモータ初期位置移動
deg_new = -90.0
time.sleep(1) #１秒待ち
while True:
while True: #スイッチ入力待ち
if sw1.value() == 0:
break
time.sleep_ms(10) #10msec待ち
#90degに回転
servoMov(90, 50)
time.sleep(1) #１秒待ち
#-90degに回転
servoMov(-90, 100)
time.sleep(1) #１秒待ち
#0degに回転
servoMov(0, 100)
time.sleep(1) #１秒待ち
#-45degに回転
servoMov(-45, 50)
time.sleep(1) #１秒待ち
#45degに回転
servoMov(45, 100)
time.sleep(1) #１秒待ち
#-90degに回転
servoMov(-90, 25)
time.sleep(1) #１秒待ち【プログラム説明】
１２行目：
deg_new = 0.0
は、サーボモータの現在の角度を表す変数で、その初期設定です。deg_nowの値を10ms毎に変更することによって、サーボモータの定速回転を行っています。
２５行目：
def servoMov(deg, speed):
は、サーボモータを設定角度まで定速で回転させる関数です。関数の引数degは角度(deg)でspeedは回転相度(deg/s)です。
２８行目：
sp_per10ms = speed / 100.0
は、10ms毎にサーボモータの角度変更量の設定でservoMov()関数の引数speedから計算されます。
４７行目、５１行目
deg_new = deg_new + sp_per10ms
または、
deg_now = deg_now – sp_per10ms
は、10ms毎に、現在のサーボモータ軸の角度に10ms毎の角度変更量を足し算、または引き算して、次の回転角度を計算しています。
CW方向回転の場合は、角度変更量を引き算して、CCW方向回転の場合は足し算しています。
５６行目：
time.sleep_ms(10)
は、10ms毎にサーボモータの回転角度の変更処理を行うため、10msのsleep処理を行っています。
下記動画は上記プログラムを実行してサーボモータを動作させる様子です。
３．加減速して目的の角度に回転させるプログラム
振動を抑えるために、サーボモータ軸を目的角度まで低速で回転させた場合は目的角度に到達するのに時間がかかります。加減速を行ってサーボモータを回転させることによって、振動を抑えて短時間に目的角度に回転させることができます。
サーボモータを一定速度で回転させるためには、PWMのパルス幅を一定時間間隔毎に徐々に変化させますが、加減速時はパルス幅を変化させる量を徐々に変えます。
以下にプログラム例を示します。
このプログラムでは、10ms毎にサーボモータ軸の角度を変更しています。定速時は一定量で角度を変更していますが、加速時は10ms毎に角度の変更量を増やしています。また減速時は10ms毎に角度の変更量を減らしています。
スイッチを押すと回転速度300°/s、加速度5°/s2で、90°→-90°→０°→-45°→-90°→90°→-90°の順にサーボモータ軸が回転します。
from machine import PWM, Pin
import time
#サーボモータの設定
servo = PWM(Pin(0), freq = 50) #GP0がサーボモータ用PWM出力
plu90_duty = float(2.4 / 20.0) #90deg時のデューティ比
min90_duty = float(0.5 / 20.0) #-90deg時のデューティ比
sw1 = Pin(16, Pin.IN, Pin.PULL_UP ) #"GP16をプルアップ付き入力とする
#現在角度
deg_new = 0.0 #単位deg
#指定角度にサーボモータを動かす関数
#パラメータ
#deg: 移動角度 -90～90
def sv_mov(deg):
duty = int(65535 * (min90_duty + (plu90_duty - min90_duty) * (deg + 90) /180))
servo.duty_u16(duty)
#サーボモータ加減速運転処理
#パラメータ
#deg: 移動角度
#speed: 回転速度(deg/sec)
#acc: 回転加減速度(deg/sec**2)
def servoMov(deg, speed, acc):
global deg_new #現在角度
sp_per10ms = speed / 100.0 #10ms単位の目標速度(deg/10ms)
acc_per10ms = acc / 100.0 #10ms単位の加速度(deg/10ms**2)
sp_now = 0.0 #10ms単位の現在速度(deg/10ms)
cw_flag = False #CW方向回転フラグ
if deg >= deg_new: #目標位置>=現在位置
cw_flag = False #CCW方向回転
else:
cw_flag = True #CW方向回転
all_length = abs(deg - deg_new) #全移動角度
half_length = all_length / 2 #全移動角度の半分
length_now = 0.0 #現在移動角度
acc_length = speed * speed / acc * 2 #加速角度移動距離
if acc_length >= half_length:
acc_length = half_length
dec_length = all_length - acc_length #減速角度移動距離
acc_no = 0 #加速回数
dec_no = 0 #減速回数
#-----加速処理実施------------------
while True:
if length_now >= dec_length: #減速位置到達
break
elif length_now >= acc_length: #加速終了点到達
break
sp_now = sp_now + acc_per10ms #速度インクリメント
if sp_now >= sp_per10ms: #予定速度到達
sp_now = sp_per10ms #速度←予定速度
length_now = length_now + sp_now #移動距離インクリメント
if cw_flag == False: #cw方向回転？
deg_new = deg_new + sp_now #移動角度計算
if deg_new >= deg:
deg_new = deg
else:
deg_new = deg_new - sp_now #移動角度計算
if deg_new <= deg:
deg_new = deg
sv_mov(deg_new) #サーボモータ回転
time.sleep_ms(10) #10msec待ち
#-----定速処理実施-----------
while True:
if length_now >= dec_length: #減速位置到達
break
length_now = length_now + sp_now #移動距離インクリメント
if cw_flag == False: #cw方向回転？
deg_new = deg_new + sp_now #移動角度計算
if deg_new >= deg:
deg_new = deg
else:
deg_new = deg_new - sp_now #移動角度計算
if deg_new <= deg:
deg_new = deg
sv_mov(deg_new) #サーボモータ回転
time.sleep_ms(10) #10msec待ち
#-----減速処理実施-------------
first_flag = True
while True:
if length_now >= all_length:
break #目標位置到達
sp_now = sp_now - acc_per10ms #速度デクリメント
if sp_now <= 0:
sp_now = 0.1 #最低速度セット
length_now = length_now + sp_now #移動距離インクリメント
if cw_flag == False: #cw方向回転？
deg_new = deg_new + sp_now #移動角度計算
if deg_new >= deg:
deg_new = deg
else:
deg_new = deg_new - sp_now #移動角度計算
if deg_new <= deg:
deg_new = deg
sv_mov(deg_new) #サーボモータ回転
time.sleep_ms(10) #10msec待ち
#-------------メインプログラム---------------------
sv_mov(-90.0) #サーボモータ初期位置移動
deg_new = -90.0
time.sleep(1) #１秒待ち
while True:
while True: #スイッチ入力待ち
if sw1.value() == 0:
break
time.sleep_ms(10) #10msec待ち
#90degに回転
servoMov(90, 300, 5.0)
time.sleep(1) #1秒待ち
#-90degに回転
servoMov(-90, 300, 5.0)
time.sleep(1) #1秒待ち
#0degに回転
servoMov(0, 300, 5.0)
time.sleep(1) #1秒待ち
#-45degに回転
servoMov(-45, 300, 5.0)
time.sleep(1) #1秒待ち
#-90degに回転
servoMov(-90, 300, 5.0)
time.sleep(1) #1秒待ち
#90degに回転
servoMov(90, 300, 5.0)
time.sleep(1) #1秒待ち
#-90degに回転
servoMov(-90, 300, 5.0)
time.sleep(1) #1秒待ち【プログラム説明】
１３行目：
deg_new = 0.0
は、サーボモータの現在の角度を表す変数の初期設定です。deg_nowの値を10ms毎に変更することによって、サーボモータの回転を行っています。
２７行目：
def servoMov(deg, speed, acc):
は、サーボモータを設定角度まで加減速して回転させる関数です。関数の引数degは角度(deg)でspeedは回転速度(deg/s),accは回転加速度(deg/s2)です。
３０行目：
sp_per10ms = speed / 100.0
は、10ms毎のサーボモータの角度変更量の設定でservoMov()関数のspeed引数から計算されます。
３１行目：
acc_per10ms = acc / 100.0
は、加減速時の10ms毎の角度変更量の増減分の設定でservoMov()関数のacc変数から計算されます。
３２行目：
sp_now = 0.0
は、10msec毎の角度変更量の現在値です。ここでは初期設定をしています。
◎加速処理実施時
５９行目：
sp_now = sp_now + acc_per10ms
は、加速時に10ms毎に速度(角度変更量/10ms)をインクリメントする計算です。
６６行目、７０行目
deg_new = deg_new + sp_now
または、
deg_new = deg_new – sp_now
は、10ms毎に、現在のサーボモータ軸の角度に10ms毎の角度変更量を足し算、または引き算して、次の回転角度を計算しています。
CW方向回転の場合は、角度変更量を引き算して、CCW方向回転の場合は足し算しています。
加速時なので、sp_nowの値は５９行目のsp_now = sp_now + acc_per10msの計算により徐々に増した値となっています。
７５行目：
time.sleep_ms(10)
は、10ms毎にサーボモータの回転角度の変更処理を行うため、10msのsleep処理を行っています。
◎定速処理実施時
８５行目、８９行目：
deg_new = deg_new + sp_now
または、
deg_new = deg_new – sp_now
は、10ms毎に、現在のサーボモータ軸の角度に10ms毎の角度変更量を足し算、または引き算して、次の回転角度を計算しています。
CW方向回転の場合は、角度変更量を引き算して、CCW方向回転の場合は足し算しています。
定速時なので、sp_nowの値は常に同じ値となっています。
９４行目：
time.sleep_ms(10)
は、10ms毎にサーボモータの回転角度の変更処理を行うため、10msのsleep処理を行っています
◎減速処理実施時
１０２行目：
sp_now = sp_now – acc_per10ms
は、減速時に10ms毎に速度(角度変更量/10ms)をデクリメントする計算です。
１０９行目、１１３行目
deg_new = deg_new + sp_now
または、
deg_new = deg_new – sp_now
は、10ms毎に、現在のサーボモータ軸の角度に10ms毎の角度変更量を足し算、または引き算して、次の回転角度を計算しています。
CW方向回転の場合は、角度変更量を引き算して、CCW方向回転の場合は足し算しています。
減速時なので、sp_nowの値は１０２行目のsp_now = sp_now – acc_per10msの計算により徐々に減った値となっています。
１１８行目：
time.sleep_ms(10)
は、10ms毎にサーボモータの回転角度の変更処理を行うため、10msのsleep処理を行っています。
下記動画は上記プログラムを実行してサーボモータを動作させる様子です。