Raspberry Pi Pico＋Arduinoでサーボをたくさん動かしたい

Raspberry Pi Pico＋Arduinoでサーボを26個まで動かせるプログラムをつくりました。

■この記事の内容を動画でも解説しています

EarlePhilhower版のServoライブラリについて

EarlePhilhower版のPicoサポートをインストールした場合はServoライブラリが使えます。

このライブラリではArduino UNO等と同じコードが利用できるのですが、

servo.attach(2);

のようにしてデフォルトのパルス幅の場合は、Arduino UNO等と比べてサーボの動く範囲(0°から180°の動く角度)が狭いようです。

パルス幅の範囲はArduino UNO等では544～2400がデフォルトですが、
Pico (EarlePhilhower版)ではソースを見たところデフォルトの範囲が1000～2000と狭くなっています。
安全性を考慮して狭めにしてあるとソースのコメントに書かれていました。
以下のようにパルス幅のMinとMaxを指定すると、Arduino UNO等と同じ範囲(180°ぐらい)で動くようになりました。

servo.attach(2, 544, 2400);

なお、EarlePhilhower版のサーボライブラリは最大8つのサーボを制御できるようです。
このサーボライブラリはPIO(Programmable I/O)を使用していて、PIOが8つまでのため、他にPIOを使用していると制御できるサーボの数が減ります。

今回、18個のサーボを制御したくて、どうするか考えてみました。

サーボの制御にanalogWrite()が使えるか？

PWMを使って16個まで使えるanalogWrite()がサーボ制御に使えないかと思い調べてみました。
Picoは8つのPWMスライスがあり、各スライスにA/Bの2つのPWMチャンネルがあります。最大8×2=16チャンネルのPWM出力が可能となっています。
サーボを動かすときは50Hz(20ms)の周波数になりますが、EarlePhilhower版(3.2.0で確認)のソース(wiring_analog.cpp)を確認したところではanalogWriteFreq()で周波数を変更できるものの100Hz以上に制限されているようです。
(なお、公式版(4.0.2で確認)ではそもそも周波数を設定できず、500Hz固定のようです)
※最近のサーボは50Hz以上の周波数(333Hzなど)でも対応しているようです。

※Raspberry Pi Pico 2ではPWM機能が24チャンネル使用できると勘違いしていて、実際に購入して試したところ、16チャンネルまでしか使えませんでした…
あらためてPico2のデータシートを確認したところ、
どうやら、24チャンネル使えるのは80ピンのRP2350Bのチップで、Raspberry Pi Pico 2とRaspberry Pi Pico 2 Wに搭載されている60ピンのRP2350Aのチップでは 16チャンネルのようです。

Pico SDKを直接使えば16個まではPWMで制御でき、周波数も自由に変えられます。（周波数の変更はスライス数の8個までですが、すべてサーボのコントロールに使用するのであれば16個）
しかし、今回は18個使いたいのです！

ソフトウェアでサーボ制御する

ライブラリを使わず、PWMやPIO等のハードウェアの機能も使わず、ソフトウェアでサーボをコントロールするプログラムを作成してみました。
PicoのGPIO最大数の26個までサーボをコントロールできます。
ただし、ハードウェアのPWMに比べれば若干は波形(周波数やデューティー比)が乱れるため、サーボがプルプルしてしまうことが考えられます。

サーボのコントロールは他の処理に時間がかかるとスムーズに動かないので、２つ目のCPUコアを使用しています。
EarlePhilhower版ではsetup1(), loop1()を使用するだけで簡単にマルチコアのプログラムが書けます。

↓サーボ26個が動いているところ

Raspberry Pi Pico＋Arduino開発環境でサーボをたくさん動かしてみました

EarlePhilhower版Arduinoのサーボライブラリは8個までしか動かせないので、2コアを活かしたソフトウェアPWMで組みました

サーボ：DS3218 ２０個＋ SG90 ６個＝２６個
バッテリー：単3ニッケル水素(eneloop) x 4 pic.twitter.com/8we5cI1LCC

— PONTA@電子工作⚡️ロボット制作⚡️プログラミング (@Elec_Robot) May 23, 2023

ソースは以下になります。（サーボ10個を動かす場合のサンプル）

// Raspberry Pi Pico (EarlePhilhower版) 用
// ライブラリやPWMを使わずにサーボをコントロールする
// サーボはGPIO数の26個まで使用できます
// ※マルチコアを使用するためEarlePhilhower版が必要

// サーボを接続したGPピン番号の配列 (26個まで対応)
uint8_t SERVO_PIN[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
#define SRV_NO   (sizeof(SERVO_PIN) / sizeof(uint8_t))   // サーボの数

int SERVO_MIN = 544;    // 0°のパルス幅(μs)
int SERVO_MAX = 2400;   // 180°のパルス幅(μs)
int SERVO_FREQ = 20;    // サーボの周期(ms)

int16_t servo_deg[28];   // サーボ角度(0-180)

int8_t servo_count;     // サーボ数
int8_t servo_flag[28];  // パルス状態(0/1)
uint32_t servo_timer[28];
uint32_t servo_freq_timer;  // 20msのタイマー
int8_t servo_freq_flag;     // 1: 20msの周期中

void servo_setup() {
  servo_count = sizeof(SERVO_PIN) / sizeof(uint8_t);
  for (int i = 0; i < servo_count; i++) {
    pinMode(SERVO_PIN[i], OUTPUT);
    digitalWrite(SERVO_PIN[i], LOW);
    servo_deg[i] = 90;
    servo_flag[i] = 0;
    servo_timer[i] = 0;
  }
  servo_freq_timer = 0;
  servo_freq_flag = 0;
}

// 定期的に呼び出す(最低10μsec毎)
void servo_tick() {
  // 20ms周期
  uint32_t currentTime = micros();
  if (servo_freq_flag == 0 && currentTime > servo_freq_timer) {
    servo_freq_flag = 1;
    servo_freq_timer = currentTime + SERVO_FREQ * 1000; // 次の20ms後

    // パルス幅の時間を計算
    for (int i = 0; i < servo_count; i++) {
      servo_timer[i] = currentTime + SERVO_MIN + (SERVO_MAX - SERVO_MIN) * servo_deg[i] / 180;
    }

    // 全てのサーボをパルスHIGH
    for (int i = 0; i < servo_count; i++) {
      digitalWrite(SERVO_PIN[i], HIGH);
      servo_flag[i] = 1;
    }
  }

  //  パルス幅の時間が来たサーボをLOWにする
  if (servo_freq_flag == 1) {
    uint32_t currentTime = micros();
    for (int i = 0; i < servo_count; i++) {
      if (servo_flag[i] == 1 && currentTime > servo_timer[i]) {
        digitalWrite(SERVO_PIN[i], LOW);
        servo_flag[i] = 0;
      }
    }

    // すべてLOWになったか調べる
    int8_t complete_flag = 1;
    for (int i = 0; i < servo_count; i++) {
      if (servo_flag[i] == 1) {
        complete_flag = 0;
        break;
      }
    }
    if (complete_flag == 1) {
       // すべてLOWになった
       servo_freq_flag = 0;
    }
  }
}

void servo_write(int idx, int deg) {
  if (deg < 0) deg = 0;
  if (deg > 180) deg = 180;
  servo_deg[idx] = deg;
}

// -------------------------------
// CORE0
// -------------------------------
int dir = 1;
int deg = 90;

void setup() {
  Serial.begin(115200);
  delay(1000);  // 1秒待つ
}

void loop() {
  Serial.println(deg);

  deg += dir;
  if (deg <= 0 || deg >= 180) dir *= -1;
  for (int i = 0; i < SRV_NO; i++) {
    servo_write(i, deg);
  }
  delay(20);
}

// -------------------------------
// CORE1
// servo_tick()を10μs以下の周期で呼び出す必要があるため、CORE1を利用する
// -------------------------------

void setup1() {
  servo_setup();
}

void loop1() {
  servo_tick();
}

プログラムの使い方
↓の配列にサーボを接続したピンを記載します。この例では要素数が10ですが、例えばサーボを4つ接続する場合は要素数は4つにします。

int8_t SERVO_PIN[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};

↓の関数を呼び出せばサーボの角度を変更できます。一つ目の引数は上で定義したSERVO_PIN[]のインデックスです。二つ目の引数が角度(0～180)です。

servo_write(0, deg);

PWM機能とPIOを使用したServoライブラリを併用する

CPUでサーボを制御する場合はPWMのタイミングが微妙にずれてしまうため、サーボがプルプルしてしまいます。
動いている時はあまりわかりませんが、停止しているとはっきりわかります。

Raspberry Pi Picoが持つ16チャンネルのPWM機能と、PIOを使用したServoライブラリを組み合わせて、サーボを24個まで制御できるようにしてみました。
PWM機能はPico SDKを直接使用しているため、Pico/Pico W/Pico 2/ Pico 2 W専用です。

↓サーボ16個が動いているところ

✨六脚ロボのプログラムから、サーボ制御箇所を抜き出しました

Raspberry Pi PicoのPWM機能（16個）とServoライブラリ(8個)で、サーボを24個まで動かせます‼

※動画はサーボ16個です（手元に16個しかなかった） pic.twitter.com/MqjdTxNAtH

— PONTA@電子工作⚡️ロボット制作⚡️プログラミング (@Elec_Robot) March 15, 2025

ソースは以下になります。（サーボ10個を動かす場合のサンプル）

// Raspberry Pi Pico (EarlePhilhower版※) 専用
// PWM(16個まで)とServoライブラリ(8個まで)を使ってサーボをコントロールする
// サーボは最大24個まで使用できるはず(試してない)
// 16個まではPWMを使用し、それ以上はPIOを使うServoライブラリを使用する
// ※PIOを使用するServoライブラリを使用するためEarlePhilhower版が必要

#include <hardware/pwm.h>
#include <hardware/clocks.h>
#include <Servo.h>

// サーボを接続したGPピン番号の配列 (24個まで対応)
//int8_t SERVO_PIN[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 26};
uint8_t SERVO_PIN[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
#define SRV_NO   (sizeof(SERVO_PIN) / sizeof(uint8_t))   // サーボの数

#define SERVO_MIN 544    // 0°のパルス幅(μs)
#define SERVO_MAX 2400   // 180°のパルス幅(μs)
//#define SERVO_MIN 500    // 0°のパルス幅(μs)   DS3218サーボ用
//#define SERVO_MAX 2500   // 180°のパルス幅(μs) DS3218サーボ用

#define SERVO_FREQ 20000  // サーボの周期(μs) 50Hz 通常は20ms=50Hz
//#define SERVO_FREQ 3000     // サーボの周期(μs) 333Hz 通常は20ms=50Hzだが、DS3218サーボは3ms=333hzに対応している ※周波数を増やすと反応が速くなる
#define SERVO_WRAP 65000    // PWM解像度(65535まで)

// 17個以上はServo（PIO使用）を使用する ※8個まで
Servo servo[SRV_NO - 16];
uint8_t pwm_Slice_flag[8][2]; // PWM機能の使用済フラグ -> 使用済はServoを使用
int8_t servo_index[SRV_NO];   // Servoライブラリのindex (-1はPWM)

void servo_setup() {
  for (int slice = 0; slice < 8; slice++) {
    pwm_Slice_flag[slice][0] = 0;
    pwm_Slice_flag[slice][1] = 0;
  }
  for (int i = 0; i < SRV_NO; i++) {
    servo_index[i] = -1;
  }
  int8_t servo_num = 0; // Servoライブラリの使用数

  // CPUクロックを取得
  uint hz_clock = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_PLL_SYS_CLKSRC_PRIMARY) * 1000;
  Serial.print("hz_clock:"); Serial.println(hz_clock);

  for (int i = 0; i < SRV_NO; i++) {
    uint8_t pin = SERVO_PIN[i];
    uint slice_num = pwm_gpio_to_slice_num(pin);
    int channel = pin & 1;
    Serial.print("pin:"); Serial.print(pin);
    Serial.print(" slice_num:"); Serial.print(slice_num);
    Serial.print(" ch:"); Serial.print(channel);
    if (pwm_Slice_flag[slice_num][channel] == 0) {
      // PWM
      pwm_Slice_flag[slice_num][channel] = 1;
      gpio_set_function(pin, GPIO_FUNC_PWM);

      // PWM周波数 = クロック周波数 / ((wrap+1) * clkdiv)
      // clkdiv = クロック周波数 / ((wrap+1) * PWM周波数)
      // ※PWM周波数はスライス毎に設定可能
      float clkdiv = (float)((double)hz_clock / ((double)SERVO_WRAP * 1000000.0 / (double)SERVO_FREQ));
      Serial.print(" clkdiv:");  Serial.println(clkdiv);
      pwm_set_clkdiv(slice_num, clkdiv);
      pwm_set_wrap(slice_num, SERVO_WRAP - 1);
      pwm_set_chan_level(slice_num, channel, 0);
      pwm_set_enabled(slice_num, true);
    }
    else {
      // Servo(PIO)
      Serial.print(" servo_num:"); Serial.println(servo_num);
      servo_index[i] = servo_num;
      servo[servo_num].attach(pin, SERVO_MIN, SERVO_MAX);
      servo_num++;
    }
  }
}

void servo_write(int idx, int16_t deg) {
  uint8_t pin = SERVO_PIN[idx];
  if (deg < 0) deg = 0;
  if (deg > 180) deg = 180;

  if (servo_index[idx] == -1) {
    // PWM
    uint slice_num = pwm_gpio_to_slice_num(pin);
    int channel = pin & 1;

    uint16_t duty;
    if (deg == 255) {
      duty = 0;
    }
    else {
      duty = (uint16_t)((SERVO_MIN + (SERVO_MAX - SERVO_MIN) * deg / 180) * SERVO_WRAP / SERVO_FREQ);
    }
    
    pwm_set_chan_level(slice_num, channel, duty);
  }
  else {
    // Servo
    if (deg != 255) {
      servo[servo_index[idx]].write(deg);
    }
  }
}

void setup() {
  Serial.begin(115200);
  delay(1000);  // 1秒待つ
  servo_setup();
}

int16_t dir = 1;
int16_t deg = 90;

void loop() {
  Serial.println(deg);

  deg += dir;
  if (deg <= 0 || deg >= 180) dir *= -1;
  for (int i = 0; i < SRV_NO; i++) {
    servo_write(i, deg);
  }
  delay(20);
}

↓使用したサーボ。SG-90の互換サーボを使用しました。