Zenn
🦾

ラズパイ-ラジコン＋αを自作する - その３（腕の制御）

2021/09/20に公開
Python
Raspberry Pi
電子工作
tech

🏁デモ

📚記事一覧

📍本記事の範囲

サーボモータを制御し、腕を制御できるようにします。
サーボモータ管理について説明します。

🔧パーツ一覧

部品名 個数 内容 備考
RaspberryPiZeroWH 1 ラズベリーパイゼロ本体 秋月電子
カムロボット 1 カムロボット本体 Amazon
SG90 2 SG90本体 秋月電子

※全パーツは、その１－コンセプトを参照のこと

接続図

ピンの接続

ラズパイ側

Pin サーボモータ 備考
GND GND サーボモータ1/2
5V 5V サーボモータ1/2
BCM17 サーボモータ1-PWM -
BCM27 サーボモータ2-PWM -

💻環境

開発環境

  • ラズベリーパイ
    • Linux rpi 5.10.17-v7l+ #1403 SMP Mon Feb 22 11:33:35 GMT 2021 armv7l GNU/Linux
  • Python
    • Python 3.7.3 (default, Jan 22 2021, 20:04:44)

ラズベリーパイの設定

特になし

モジュールのインストール

apt

pigpioライブラリは、Raspberry PiのGPIOを制御するためのライブラリです。
以下のコマンドは、はじめてインストールする場合のみ必要です。

$ sudo apt install pigpio
$ sudo service pigpiod start
$ sudo systemctl enable pigpiod.service

pip

Pythonに関するモジュールをインストールします。

$ python3 -m venv env
$ source env/bin/activate
(env) $ pip install pigpio
(env) $ pip install gpiozero
(env) $ pip install icecream

📝手順

最終的に、すべてのモジュールが連携するため、キューを使用しています。

コード

servo.py
from queue import Queue
import threading
import time
from gpiozero import AngularServo
from gpiozero.pins.pigpio import PiGPIOFactory
from icecream import ic

# SG90のピン設定
SERVO_LEFT_PIN = 17  # SG90-1
SERVO_RIGHT_PIN = 27  # SG90-2

MIN_DEGREE = -90       # 000 : -90degree
MAX_DEGREE = 90       # 180 : +90degree

LEFT_OFFSET = 0
RIGHT_OFFSET = 0

servo_pis = {
    "left": SERVO_LEFT_PIN,
    "right": SERVO_RIGHT_PIN,
}


class ServoThread(threading.Thread):
    """
    サーボ管理
    例:
    queue経由で、{"type":"servo", "name": "right", "action": "raise/down/swing"}
    """
    def __init__(self):
        threading.Thread.__init__(self)
        self.stop_event = threading.Event()
        self.setDaemon(True)

        self._rcv_que = Queue()
        self._servo = {}
        factory = PiGPIOFactory()
        for key, pin in servo_pis.items():
            self._servo[key] = AngularServo(pin, min_angle=MIN_DEGREE, max_angle=MAX_DEGREE, pin_factory=factory)
        return

    def stop(self):
        self.stop_event.set()
        return


    def run(self):
        while True:
            item = self.rcv_que.get()
            ic("[servo_th]", "run : get : ", item)
            
            if "servo" not in item["type"]:
                ic("[servo_th]", "error!")
                continue
            self._recvice(item)
        return

    @property
    def rcv_que(self):
        return self._rcv_que
    
    def _recvice(self, item):
        name = item["name"]
        action = item["action"]
        if action in ["raise", "down"]:
            deg = 90 if action == "raise" else -90
            self._servo[name].angle = self._cal_degree(name, deg)
        elif action in ["swing"]:
            # for deg in ([45, 90] * 5):
            for deg in ([-45, 90] * 5):
                self._servo[name].angle = self._cal_degree(name, deg)
                time.sleep(0.30)
        else:
            ic("[ServoThread] - _recvice - error!!!")
        return

    def _cal_degree(self, name, degree):
        if "left" in name:
            degree *= (-1)
            offset = LEFT_OFFSET
        else:
            offset = RIGHT_OFFSET
        return degree + offset

def main():
    import time

    servo_th = ServoThread()
    servo_th.start()
    q = servo_th.rcv_que

    q.put({"type": "servo", "name": "left", "action": "raise"})
    time.sleep(2)
    q.put({"type": "servo", "name": "left", "action": "down"})
    time.sleep(2)
    q.put({"type": "servo", "name": "right", "action": "raise"})
    time.sleep(2)
    q.put({"type": "servo", "name": "right", "action": "down"})
    time.sleep(2)
    q.put({"type": "servo", "name": "left", "action": "swing"})
    time.sleep(5)
    q.put({"type": "servo", "name": "right", "action": "swing"})
    time.sleep(5)

    servo_th.stop()
   
    return

if __name__ == "__main__":
    main()

実行手順

(env) $ python servo.py

以下の順番でカムロボットが動作します。

  • 左腕をあげる
  • 左腕をさげる
  • 右腕でをあげる
  • 右腕でをさげる
  • 左腕を振る
  • 右腕を振る

🔎ポイント

サーボモータの動作

サーボモータへは、PWM制御で角度を指定しています。
サーボモータの制御は、gpiozero - AngularServoモジュールでラップされ、
角度を指定するだけで制御可能です。

パラメータ - min_pulse_width, max_pulse_width, frame_width

本パラメータを指定しないと、狭い角度（-45度～+45度）で動作してしまいます。
SG90のデータシートを見ると、下記となっています。

AngularServoのデフォルト値とは異なるため、以下を指定しています。

パラメータ名 パラメータ
min_pulse_width 0.5msec
max_pulse_width 2.4msec
frame_width 20msec(50Hz)

さいごに

次は、LED/OLED/ブザーの動作について記載したいと思います。

上記以外のラズパイの活用方法を

https://zenn.dev/kotaproj/books/raspberrypi-tips

としてまとめ中です。

GitHubで編集を提案

Discussion