Zenn
🐱

Raspberry Pi PicoでMG92Bを動かす

2024/11/05に公開
Raspberry Pi Pico
tech

まえがき

差動二輪ロボットになんとなくアームをつけたくなって、Raspberry Pi PicoでPWMサーボを動作させました。

参考URL：理系的な戯れ
(参考、というかほぼ丸パクリです。すみません）

開発環境

C/C++ SDK、PCはThinkpad X260にUbuntu 22.04LTS、Picoは普通のRaspberry Pi Pico。

使用サーボ

TowerPro MG92B(別の用途で買ったけど結局使わずじまいの死蔵品）

動作を考える

サーボ

TowerProのSG90とかMG92BとかそのあたりはPWMのDuty比で動作する仕組みです（いわゆるPWMサーボと呼ばれるやつ）。MG92Bのデータシートには波形の周期やDuty比や動作量が書いてないのでSG90のデータシートを使いました。したがって、

  1. PWM周期は50[Hz] (20[ms])
  2. Dutyサイクル0.5[ms]で-90[度]、1.45[ms]で0[度]、2.4[ms]のパルスで+90[度]
    とします。

PicoのPWM

Raspberry Pi Picoの動作周波数（いわゆるシステムクロック）は125M[Hz]、つまり1秒間に125000000回クロックが入るということ。一方、サーボへのPWM周期は50[Hz]で良いので、2500000回クロックが入ったら1、次に2500000回クロックが入ったら2、…という感じで数え上げれば1秒間に50回を正確な周期で数えることができます。

ただ、PicoのC/C++ SDKにあるpwm_set_wrap()という関数はカウンタが16ビットしかなく最大65535までしか数えられず、2500000は数え上げられません。

そこで、数え上げる方法を2段階にすることでこの問題を解決します。最初125M[Hz]のシステムクロックを割って（分周する、というらしい）、割ったシステムクロックを使って数え上げることで、欲しかった周期が得られるようになります。

2段階方式の最初にやるシステムクロックを割る関数はpwm_set_clkdiv()。この関数に与える割る数の型はfloat。今回は2500000を25000x100と考え、pwm_set_clkdiv()に100、pwm_set_wrap()で25000を数え上げるようにしました。

プログラムの動作

起動直後は-90[度]にして、それから+90[度]と-90[度]を2秒間隔で往復するようにします。

-90[度]と+90[度]のときのPWMのパルス数を計算します。

50[Hz] (20[ms])で数え上げるパルス数は25000回なので、-90[度] (0.5[ms])のときは25000x0.5/20=625回パルスを数え上げれば良く、また、+90[度] (2.4[ms])のときは25000x2.4/20=3000回パルスを数え上げればよいはずです。

これをもとにソースコードを書きました。

ソースコード

#include "pico/stdlib.h"
#include "hardware/pwm.h"

#define SERVO1 14

int main() {
  gpio_set_function(SERVO1, GPIO_FUNC_PWM);

  uint slice_num = pwm_gpio_to_slice_num(SERVO1);

  pwm_set_wrap(slice_num, 24999);
  pwm_set_clkdiv(slice_num, 100.0);

  pwm_set_chan_level(slice_num, PWM_CHAN_A, 3000);

  pwm_set_enabled(slice_num, true);

  sleep_ms(2000);
  pwm_set_chan_level(slice_num, PWM_CHAN_A, 625);
  sleep_ms(2000);


  while (true) {
    pwm_set_chan_level(slice_num, PWM_CHAN_A, 3000);
    sleep_ms(2000);
    pwm_set_chan_level(slice_num, PWM_CHAN_A, 625);
    sleep_ms(2000);
  }

  return 0;
}

最後に

この記事でPicoのPWM機能にあるスライスというものを記載していませんでした。
申し訳ないのですが、以下URLを参照ください。非常にわかりやすく良記事です。

Raspberry Pi PicoとMOSFET ③ PWMのAPI

Picoを使ってPWMは最高16個出すことができるみたいなので、車輪2つアームの関節3つの合計5つはらくらく動かすことができそうです。

Discussion