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を使用していると制御できるサーボの数が減ります。

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

サーボの制御に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でも行けるのではないかと思いますので、少しコードを変更すれば対応できそうですが、いったん別の方法を検討してみます。

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

ライブラリを使わず、PWMやPIO等のハードウェアの機能も使わず、ソフトウェアでサーボをコントロールするプログラムを作成してみました。
PicoのGPIO最大数の26個までサーボをコントロールできます。

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

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

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

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

// サーボを接続したGPピン番号の配列 (26個まで対応)
int8_t SERVO_PIN[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
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 = 0; dir *= -1;}
  if (deg > 180) {deg = 180; dir *= -1;}
  servo_write(0, deg);
  servo_write(1, deg);
  servo_write(2, deg);
  servo_write(3, deg);
  servo_write(4, deg);
  servo_write(5, deg);
  servo_write(6, deg);
  servo_write(7, deg);
  servo_write(8, deg);
  servo_write(9, 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);

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

↓Raspberry Pi Pico

■Raspberry Pi Picoの関連記事
Raspberry Pi Pico+Arduinoでサーボをたくさん動かしたい
会話ができる「ぴよロボ」作りました! (Raspberry Pi + Pico + ChatGPT)
Raspberry Pi Pico W でPCとBluetooth(シリアル)接続する
Raspberry Pi Pico/Pico WをArduino開発環境で使うためのメモ
超音波距離センサー + Raspberry Pi Picoで潜水艦ソナー風
コップの水がこぼれない台 MPU6050 + Raspberry Pi Pico(Arduino)
MPU6050 + Raspberry Pi Pico(Arduino) -> PCで3Dのキューブを回転表示

本格派対局将棋 ぴよ将棋
本格派対局将棋アプリ ぴよ将棋
[Android] [iOS]

かわいい「ひよこ」と対局する将棋アプリ。かわいいけどAIは本格派!
対局後の検討機能や棋譜管理機能も充実!棋譜解析機能も搭載!

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です