MicroPythonでRCサーボを使用するためのドライバーです。

ESP32, RP2040, STM32での動作確認をしています。

STM32で使用する場合には、PWMライブラリとしてstm32pwmが必要です。

STM32用のMicroPython用PWMライブラリ

RCServoドライバーのソースプログラム

"""
RCServo: RC Servo driver
for MicroPython
Version 1.0

Copyright 2023 K.Kakizaki
"""

import sys

if sys.platform.startswith('stm32'):
    from stm32pwm import PWM
else:
    from machine import PWM, Pin

class RCServo:

    def __init__(self, pin, pos=0, min=500, max=2400, fullangle=90, freq=50):
        self._pin = pin
        self._pos = pos
        self._min = min
        self._max = max
        self._fullangle = fullangle
        self._freq = freq
        if sys.platform.startswith('stm32'):
            self._pwm = PWM(pin, freq=self._freq)
        else:
            self._pwm = PWM(Pin(pin), freq=self._freq)
        self.setPosition(pos)

    def setFreq(self, freq=50): # サーボの制御信号の周波数
        self._freq = freq
        self._pwm.freq(freq)
        
    def getFreq(self):
        return self._freq
        
    def setMin(self, us=500): # 最小パルス幅 us
        self._min = us
        
    def getMin(self):
        return self._min
        
    def setMax(self, us=2400): # 最大パルス幅 us
        self._max = us
        
    def getMax(self):
        return self._max

    def setFullAngle(self, deg=90): # 最大回転角度
        self._fullangle = deg

    def getFullAngle(self):
        return self._fullangle

    def angle(self, deg): # 角度を指定した回転指示
        self.setAngle(deg)

    def setAngle(self, deg): # 角度を指定した回転指示
        if deg > self._fullangle:
            deg = self._fullangle
        if deg <-self._fullangle:
            deg = -self._fullangle
        self._angle = deg
        pos = deg / self._fullangle * 100
        self.setPosition(pos)

    def getAngle(self):
        return self._angle

    def pos(self, pos): # -100 から100 までの最大回転角に対する割合を指定した回転指示
        self.setPosition(pos)

    def setPosition(self, pos):
        if pos > 100:
            pos = 100
        if pos <-100:
            pos = -100
        self._pos = pos
        term = self._max - self._min
        pos1 = (self._pos + 100) / 200
        plus_us = self._min + term * pos1
        duty = plus_us / 20_000 * 65535
        self._pwm.duty_u16(int(duty))
        
    def getPosition(self):
        return self._pos

    def setPulse(self, us): # キャリブレーションテスト用のパルス指定
        if us > self._max:
            us = self._max
        if us <self._min:
            us = self._min
        duty = us / 20_000 * 65535
        self._pwm.duty_u16(int(duty))

    def stop(self): # 制御信号出力の一時停止
        self._pwm.duty_u16(0)

RCServoドライバーの使用法

RCServoドライバは、マイクロファンのMicroPythonファームウェアにはあらかじめ組み込まれているので、ファイルシステムにドライバファイルを登録する必要はありません。

RCサーボドライバのメソッドと機能を示します。

メソッド機能
setFreq(freq=50)PWMの周波数を変更したい場合に使用します。規定値は50Hz,周期は20msです。
getFreq()現在のPWMの周波数を返します。
setMin(us=500)PWMパルスの最小幅をμ秒で指定します。規定値は500μ秒です。
getMin()現在のPWMの最小パルス幅を返します。
setMax(us=2400)PWMパルスの最大幅をμ秒で指定します。規定値は2400μ秒です。
getMax()現在のPWMの最大パルス幅を返します。
setFullAngle(deg=90)angle(), setAngle()で指定できる最大の回転角を指定します。
getFullAngle()現在指定できる最大回転角を返します。
angle(deg)setAngle(deg)と同じです。
setAngle(deg)規定値もしくはsetFullAngle()で指定された角度を正負の最大角として、回転角を指定します。
getAngle()現在指定されている回転角を返します。
pos(pos)setPosition(pos)と同じです。
setPosition(pos)規定値もしくはsetMin(), setMax()で指定されたサーボの最小、最大回転角を100として回転位置を指定します。
getPosition()現在指定されている回転位置を返します。
setPulse(us)PWMで出力するパルスの長さをマイクロ秒で指定します。
stop()PWM信号の出力を停止します。（サーボが脱力します）

RCサーボオブジェクトの作成

RCServoをインポートし、RCServo()コンストラクタを使用してRCサーボオブジェクトを作成します。いくつかのオプションを指定できますが、基本的には、RCサーボを接続する端子名を指定します。

RCサーボの初期設定

何もしないという手もありますが、正確な動作を期待するのであれば、まず、キャリブレーション作業などで調査した、PWMパルス幅の最小値と最大値をsetMin(), setMax()で指定します。

回転角（度数）を指定してRCサーボを操作したいのであれば、setMin()とsetMax()で指定したパルス幅で得られる回転位置（最大回転角）の角度の差の1/2をsetFullAngle()で指定します。setFullAngle()で指定した角度をdegとすると、setAngle()またはangle()に-degからdegの範囲の角度を与えてRCサーボの回転を制御することができます。

setFullAngle()を使用して最大回転角を指定するかしないかにかかわらず、setPosition()またはpos()に-100から100の値を指定して、RCサーボの回転を制御することができます。

サーボの操作

角度を指定してRCサーボを操作したいのであれば、setAngle()またはangle()を使用して回転角を指定します。

回転角の最大を100とする回転量を指定してRCサーボを操作したいのであれば、setPosition()またはpos()を使用して回転量を指定します。

RCServoドライバーの使用例

ESP32-C3M-TRYでのRCServoの使用例を以下に示します。

from machine import Pin
from rcservo import RCServo
import time
import sys

sv0=RCServo('SRV1')

sw1=Pin('SW1', Pin.IN)
sw2=Pin('SW2', Pin.IN)
sw3=Pin('SW3', Pin.IN)

while True:
    if (sw1.value() == 0):
        sv0.pos(80) # 安全のため最大の回転角にはしていない：最大100
    elif (sw2.value() == 0):
        sv0.pos(-80)
    elif (sw3.value() == 0):
        sv0.stop() # 信号出力を停止、サーボは脱力する
        sys.exit() # プログラムを終了
    else:
        sv0.pos(0) # スイッチを押さない場合には中央に復帰
    time.sleep_ms(100)

関連製品

ESP32-ROBO

ESP32-ROBO

RP2040-ROBO

RP2040-ROBO

ESP32-C3M-ROBO