ESP32とPCA9685を使ってMG996Rを動かす方法

はじめに

備忘録
ESP32とPCA9685を使ってMG996Rを制御する方法
PCA9685は16チャンネルPWMドライバーIC．
最大16個のサーボモーターを動かすことが可能．
I2Cで制御

必要なもの

• ESP-WROOM-32
• MG996R
• PCA9685
• ジャンパー線
• ブレッドボード
• 外部給電モジュール
• 電源アダプター

開発環境(IDFと言語)

• Thonny v4.1.6
• MicroPython v1.25.0

配線について

ESP32	PCA9685
GPIO21	SDA
GPIO22	SCL
外部給電モジュールの+	VCC
GND	GND

サンプルプログラム

import time
from machine import Pin, I2C
import pca9685

try:
    i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
except:
    i2c = I2C(scl=Pin(22), sda=Pin(21))

pca = pca9685.PCA9685(i2c)

SERVO_FREQUENCY = 50

PULSE_WIDTH_MIN_US = 500
PULSE_WIDTH_MAX_US = 2350 #default　2400

pca.freq(SERVO_FREQUENCY)

#サーボモーターで使用しているPCA9685のPIN番号を入力
ALLOWED_PINS = [0, 1, 2, 12, 13, 14]


# --- 関数 ---
def angle_to_duty(angle):
    if angle < 0:
        angle = 0
        print("警告: 角度を0度に制限しました")
    elif angle > 180:
        angle = 180
        print("警告: 角度を180度に制限しました")
    
    # 角度をパルス幅に変換
    pulse_us = PULSE_WIDTH_MIN_US + (angle / 180.0) * (PULSE_WIDTH_MAX_US - PULSE_WIDTH_MIN_US)
    
    # パルス幅をduty値に変換
    cycle_time_us = 1000000.0 / SERVO_FREQUENCY
    duty = int((pulse_us / cycle_time_us) * 4096)
    
    if duty < 0:
        duty = 0
    elif duty > 4095:
        duty = 4095
    
    return duty

def move_servo(pin, angle):
    if pin not in ALLOWED_PINS:
        print("エラー: 許可されていないピン番号です")
        return False
    
    if angle < 0 or angle > 180:
        print("エラー: 角度は0-180度で指定してください")
        return False
        
    print("ピン {} のサーボを {}度に移動します".format(pin, angle))
    duty = angle_to_duty(angle)
    pca.duty(pin, duty)
    
    return True

def get_user_input(prompt):
    try:
        return input(prompt)
    except KeyboardInterrupt:
        raise
    except:
        print("入力エラーが発生しました")
        return ""

def main():
    print("=== MG996R サーボ制御プログラム ===")
    print("MicroPython版")
    print("終了: Ctrl+C")
    print("許可ピン:", ALLOWED_PINS)
    print("")
    
    while True:
        try:
            print("-" * 30)
            pin_str = get_user_input("動かすサーボのピン番号: ")
            try:
                pin = int(pin_str)
            except:
                print("エラー: 数値を入力してください")
                continue
            
            if pin not in ALLOWED_PINS:
                print("エラー: 使われてないピン番号です")
                print("使用ピン番号:", ALLOWED_PINS)
                continue
            
            angle_str = get_user_input("角度 (0-180度): ")
            try:
                angle = int(angle_str)
            except:
                print("エラー: 数値を入力してください")
                continue
            
            move_servo(pin, angle)
                
        except KeyboardInterrupt:
            print("\nプログラムを終了します")
            for i in range(16):
                pca.duty(i, 0)
            break
        except Exception as e:
            print("エラー発生:", str(e))
            continue

if __name__ == "__main__":
    main()

pca9685.pyは以下から持ってくる
引用元: https://github.com/adafruit/micropython-adafruit-pca9685/blob/master/pca9685.py

出力結果と動作結果

サーボモーターを0° ~ 180°に移動させたとき

ガムテープが巻いてあるところが右から左に移動している．

Thonny の結果
起動画面

PIN15に繋がっているサーボモーター(MG996R)を180°移動させるときの入力例

まとめ

各サーボモーターの個体差があるのでそれに合わせてコードを書き換えたりしてください．
以下が変更箇所
PULSE_WIDTH_MIN_US = 500
PULSE_WIDTH_MAX_US = 2350