M5StackGrayでRCサーボの動作テスト
2021.5.26 Coskx Lab
1 はじめに
M5Stack GrayにRCサーボ TowerProMicro Servo SG92Rを接続し動作確認しますします。
RCサーボは指示した角度に追従して軸が回転する位置決めサーボモータです。ただし,約半回転しかできません。
RCサーボはラジコンカーのステアリングなどに用いられています。
指定角度はPWM信号の正パルスのパルス長で与え,回転角度はパルス長対応します。
PWM周期:20msec
パルス長:0.5ms~2.4ms(角度 -90°~90°に対応)
ただし対応する角度には個体差があるようです。
2 使用環境
- Windows 10 64-bit
- Arduino IDE 1.8.13
- M5Stack Gray ESP32-D0WDQ6-V3 (revision 3)
MPU6886 + BMM150 16MB 40MHz
- TowerProMicro Servo SG92R
3 接続
○制御信号にはM5StackのGPIO 26ピンを使用します。
○RCサーボ駆動用電源にはDC5Vを使用しています。
SG92R仕様には4.8Vの推奨値しか載っておらずどれだけ許容範囲があるのかわかりませんでしたが,動作しています。
(定電圧電源でテストしてみたところ駆動用電源低い方は2.5Vくらいまでは動作していました。トルクが低下しているかもしれません。)
○制御用パルス信号は,M5Stack(ESP32)のデジタル出力3.3Vを与えています。
SG92R仕様では,制御用デジタル信号電圧4.8Vとなっています。
しかし,とりあえず動いています。
抵抗分割でテストしてみたところ低い方はデジタル信号電圧3.0Vくらいまでは動作していました。デジタル信号電圧3.3Vではノイズマージンは小さいと考えられます。
SG92R仕様によればデジタル信号電圧4.8V(制御信号入力部の構成は不明,分解したところ,FZMOS-2-2 1727Hが入力ICであることがわかりましたが,FZMOS-2-2 1727Hの仕様は不明)なので,テストではOKですが,本番では,レベルシフタ(補足3参照)を使って3.3V信号を5V信号に変換すべきでしょう。
(同じ型番の製品であっても生産ロットによって,内部部品の構成が異なることがあり,そのため,仕様には細かなことは書いてないのだと思います。)
名称 | 機能 | 配線色 |
駆動用電源 | 4.8V | 赤 |
GND | GND | 茶 |
制御信号 | 駆動パルスを与える | 黄(橙) |
実際の様子 配線が処理されていないので写真の見た目は汚いです。
参考 M5Stackの裏側に書いてあるピンの名称
注意 GPIO 26を割り当ててあるので,他のピンを利用することはできません。
参考 しきい値(Threshould Level)
デジタル電子機器の基本的な入力はデジタル信号で,H信号とL信号です。電源電圧くらいの信号と0Vくらいの信号をそれぞれH信号とL信号としています。
しかし実際の機器の入力はデジタルICがになっていて,デジタルICによってH信号とL信号の境目が異なります。
しかも同じメーカの同じ番号のICでも個々に見ると境目が異なります。
そこで,ICメーカは,必ずHと受け取る最低の電圧と,必ずLと受け取る最大の電圧をデータシートのしきい値で示し,動作を保証しています。
5V駆動のTTL ICでは,0.8V以下をL,2.0V以上をHとして受け付けています。(今はあまり見られません)
5V駆動のCMOS ICでは,1.5V以下をL,3.5V以上をHとして受け付けています。(1.0V以下をL,2.5V以上をHとしているものもあります)
3.3V駆動のCMOS ICでは,1V以下をL,2.3V以上をHとして受け付けています。(0.8V以下をL,1.7V以上をHとしているものもあります)
実測するとCMOS ICへの入力の境目は電源電圧の1/2程度になっていることが多いです。そのため,自己責任で3.3VをHとして与えている例(メーカ保証外)が多いです。
SG92Rのデジタル信号入力部はCMOSの特性(HL境界は電源電圧の1/2程度)のようです。
4 テストプログラム
あらかじめ定めた駆動のためのパルス長(複数)を使用します。ボタンAでパルス長が増加し,ボタンBでパルス長が減少します。
設定値は次の通りです。
PWM生成に使用するビット数: 16bit
PWM周期:20ms
使用するPWMモジュール(PWMチャンネル)8
(利用可能性を調べてある2,3,4,5,8,9,10,11,12,13,14,15の中から1つを選んでいます)
PWM出力に使用するGPIO PIN番号:23
(利用可能性を調べてある2,5,16,17,23,19の中から1つを選んでいます)
RC サーボ SG92Rの動作テスト用プログラム本体
//RCServo_SG92RTest01.ino//
//RC Servo SG92R をテストします
//PWM周期:20mS
//制御パルス:0.5ms~2.4ms
#include <M5Stack.h>
const uint8_t nBits_forPWM = 16; // PWMに使用するビット数 n=1~16[bit]
const double PWM_Period = 20.0; // PWM周期[ms]
const double PWM_Frequency = 1000. / PWM_Period; //PWM周波数[Hz]
const uint8_t PWM_CH = 8; // PWMチャンネル 使用可能
const uint8_t PWM_PIN = 26; // PWM出力に使用するGPIO PIN番号 使用可能
const double PulseLengths[] = {0.5, 0.88, 1.26, 1.64, 2.02, 2.4}; //パルス長[ms]
//Duty = 65536*PulseLength/PWM_Period
const int nValues = sizeof(PulseLengths)/sizeof(double);
int ValueIndex = 0;
int duty;
void setup() {
M5.begin();
M5.Power.begin();
M5.Lcd.setTextSize(2);
M5.Lcd.print("RC Servo Test");
pinMode(PWM_PIN, OUTPUT);
// チャンネルと周波数の分解能を設定
ledcSetup(PWM_CH, PWM_Frequency, nBits_forPWM);
// RCサーボのピンとチャンネルの設定
ledcAttachPin(PWM_PIN, PWM_CH);
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("PlsLen[ms] = %.2f\n",PulseLengths[ValueIndex]);
duty = 65536*PulseLengths[ValueIndex]/PWM_Period;
M5.Lcd.printf("DutyRatio = %5d/65536\n",duty);
M5.Lcd.printf("PWM_Frequency = %f\n",PWM_Frequency);
//RCサーボを駆動するパルス長を与える
ledcWrite(PWM_CH, duty);
}
void loop() {
M5.update();
if (M5.BtnA.wasPressed()) {
ValueIndex++;
if (nValues<=ValueIndex) ValueIndex = nValues-1;
M5.Speaker.tone(880, 200); //Peett
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("PlsLen[ms] = %.2f\n",PulseLengths[ValueIndex]);
duty = 65536*PulseLengths[ValueIndex]/PWM_Period;
M5.Lcd.printf("DutyRatio = %5d/65536\n",duty);
//RCサーボを駆動するパルス長を与える
ledcWrite(PWM_CH, duty);
}
if (M5.BtnB.wasPressed()) {
ValueIndex--;
if (ValueIndex<0) ValueIndex = 0;
M5.Speaker.tone(440, 200); //Peett
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("PlsLen[ms] = %.2f\n",PulseLengths[ValueIndex]);
duty = 65536*PulseLengths[ValueIndex]/PWM_Period;
M5.Lcd.printf("DutyRatio = %5d/65536\n",duty);
//RCサーボを駆動するパルス長を与える
ledcWrite(PWM_CH, duty);
}
delay(10);//[msec]
}
5 駆動の様子
指示しているパルス長は0.5ms(500μs)です。デジタル信号電圧3.3Vのパルスになっています。
指示しているパルス長は0.88ms(880μs)です。
指示しているパルス長は1.26ms(1260μs)です。
指示しているパルス長は2.40msです。20msecの周期がわかります。
6 まとめ
M5Stack GrayでRCサーボの駆動ができました。
ただし,駆動信号の電圧に関しては注意が必要です。
参考 SG92R 仕様
・駆動電源電圧4.8V
・制御用デジタル信号電圧4.8V
・PWM周期:20mS
・制御パルス:0.5ms~2.4ms
・制御角:±約90°(180°)
・配線:茶=GND、赤=電源[+]、橙=制御信号 [JRタイプ]
・トルク:2.5kgf・cm
・動作速度:0.1秒/60度
・動作電圧:4.8V
・温度範囲:0℃~55℃
・外形寸法:23x12.2x27mm
・重量:9g
補足1 M5Stack Gray 出力ピン割り当てとPWMチャンネル選定テストの結果
1)M5Stack Grayで用途が制限されているGPIOピン
・回路図から読み取った,すでに使用されているGPIOピン
GPIO 1 3 ............. USB UART0 (RXD0,TXD0)で使用
GPIO 4 23 18 19 ...... SD SPIと共有
GPIO 33 27 23 18 14 .. LCDで使用
GPIO 21 22 ........... I2C,電源管理,モーションセンサ,磁気センサで使われている
GPIO 25 .............. Audioで使用
・ESP32仕様
GPIO 34~39は読み取り専用
2)M5Stack Grayで使えることを確認したGPIOピン
GPIO 2, GPIO 5 他とは共用していないので自由に使える
GPIO 16, GPIO 17 UART2 (TXD2・RXD2)を使わなければ OK
GPIO 26 DAを使わなければ OK
GPIO 19 SDを使わなければ OK
GPIO 35, GPIO 36 ADを使わなければ,入力のみOK
3)M5Stack Grayで使ってはいけない PWMチャンネル
0 音源として使われている (動作させるとビープ音がなる)
1 用途は不明だが,干渉され周期もデューティ比も指定とは異なる動作をする
6 LCDの輝度調整に使われている (動作させるとLCDの文字の明るさが変化する)
使用bit数が12以上のとき,この現象が現れる
7 LCDの輝度調整に使われている (動作させるとLCDの文字の明るさが変化する)
4)M5Stack Grayで使用確認できたPWMチャンネル
2,3,4,5,8,9,10,11,12,13,14,15
補足2 RCサーボ操作をクラス化
よく使うプログラムの部品はクラス化して,再利用可能にします。
ここでは,超音波センサ操作部を取り出して,Arduinoのしきたりに合わせて,C++のプログラム部品を作り,使用するときに呼び出すようにします。
ライブラリ化するときは,より一般性を持たせるように作ります。
RCサーボ操作クラスを使うとメインプログラムがすっきりして見通しが良くなります。
通常,スケッチRCServo_SG92ROOPTest01.inoは作業用フォルダRCServo_SG92ROOPTest01内に作られます。
同じフォルダ内にRCServoDriver.h,RCServoDriver.cppを作ります。
RC サーボ SG92Rの動作テストメインプログラム RCServo_SG92ROOPTest01.ino
//RCServo_SG92ROOPTest01.ino//
//RC Servo SG92R をテストします
//RCS PWM周期:20mS
//RCS 制御パルス:0.5ms~2.4ms
#include <M5Stack.h>
#include "RCServoDriver.h"
const uint8_t nBits_forRCS = 16; // RCSに使用するビット数 n=1~16[bit]
const double RCS_Period = 20.0; // RCS周期[ms]
const double RCS_Frequency = 1000. / RCS_Period; //RCS周波数[Hz]
const double minlength = 0.5; //[msec] 制御パルス長の最小値
const double maxlength =2.4; //[msec] 制御パルス長の最大値
const uint8_t RCS_CH = 8; // RCS PWMチャンネル
const uint8_t RCS_PIN = 26; // RCS出力に使用するGPIO PIN番号
const double PulseLengths[] = {0.5, 0.88, 1.26, 1.64, 2.02, 2.4}; //パルス長[ms]
//Duty = 65536*PulseLength/PWM_Period
const int nValues = sizeof(PulseLengths)/sizeof(double);
int ValueIndex = 0;
double PulseLength;
RCServoDriver mRCServoDriver;
void setup() {
M5.begin();
M5.Power.begin();
M5.Speaker.tone(880, 100); //Peett
M5.Lcd.setTextSize(2);
M5.Lcd.print("RC Servo Test");
mRCServoDriver.setPins(RCS_PIN, RCS_CH);
mRCServoDriver.setPWM(nBits_forRCS, RCS_Frequency);
mRCServoDriver.setMinMax(minlength, maxlength);
//RCサーボを駆動するパルス長を与える
int duty = mRCServoDriver.driveRCServo(PulseLengths[ValueIndex]);
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("PlsLen[ms] = %.2f\n",PulseLengths[ValueIndex]);
M5.Lcd.printf("DutyRatio = %5d/%d\n",duty,1<<nBits_forRCS);
}
void loop() {
M5.update();
if (M5.BtnA.wasPressed()) {
ValueIndex++;
if (nValues<=ValueIndex) ValueIndex = nValues-1;
M5.Speaker.tone(880, 200); //Peett
//RCサーボを駆動するパルス長を与える
int duty = mRCServoDriver.driveRCServo(PulseLengths[ValueIndex]);
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("PlsLen[ms] = %.2f\n",PulseLengths[ValueIndex]);
M5.Lcd.printf("DutyRatio = %5d/%d\n",duty,1<<nBits_forRCS);
}
if (M5.BtnB.wasPressed()) {
ValueIndex--;
if (ValueIndex<0) ValueIndex = 0;
M5.Speaker.tone(440, 200); //Peett
//RCサーボを駆動するパルス長を与える
int duty = mRCServoDriver.driveRCServo(PulseLengths[ValueIndex]);
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("PlsLen[ms] = %.2f\n",PulseLengths[ValueIndex]);
M5.Lcd.printf("DutyRatio = %5d/%d\n",duty,1<<nBits_forRCS);
}
delay(10);//[msec]
}
RCサーボ操作クラスヘッダファイル RCServoDriver.h
//RCServoDriver.h//
#ifndef RCServoDriver_h
#define RCServoDriver_h
class RCServoDriver {
public:
void setPins(int RCServoPin, int RCServoChannel);
void setPWM(int nBits_forRCS, double RCS_Frequency);
//RCS_Frequency [Hz]
void setMinMax(double minlength, double maxlength);
//minlength, maxlength [msec]
int driveRCServo(double PulseLength);
// PulseLength [msec]
private:
int pin;
int channel;
double period; //[msec]
int pwmMaxValue;
double minpulse, maxpulse; //[msec]
};
#endif
RCサーボ操作クラス本体ファイル RCServoDriver.cpp
//RCServoDriver.cpp//
#include <Arduino.h>
#include "RCServoDriver.h"
void RCServoDriver::setPins(int RCServoPin, int RCServoChannel) {
pin = RCServoPin;
channel = RCServoChannel;
pinMode(pin, OUTPUT); //出力したいピンを出力用途に設定
}
void RCServoDriver::setPWM(int nBits_forRCS, double RCS_Frequency) {
//RCS_Frequency [Hz]
pwmMaxValue = 1<<nBits_forRCS;
period = 1000. / RCS_Frequency; //[msec]
// 指定PWMチャンネルにPWM周波数と使用ビット数を設定
ledcSetup(channel, RCS_Frequency, nBits_forRCS);
// 出力したいピンにPWMチャンネル出力をつなげる設定
ledcAttachPin(pin, channel);
}
void RCServoDriver::setMinMax(double minlength, double maxlength) {
minpulse = minlength;
maxpulse = maxlength;
}
int RCServoDriver::driveRCServo(double PulseLength) {
// PulseLength [msec]
if (PulseLength < minpulse) PulseLength = minpulse;
if (maxpulse < PulseLength) PulseLength = maxpulse;
int duty = pwmMaxValue * PulseLength / period;
//RCサーボを駆動するパルス長を与える
ledcWrite(channel, duty);
return duty; //デバッグ用 dutyratio = duty / pwmMaxValue
}
補足3 レベルシフタ
レバルシフタは3.3Vの信号を5Vの信号に変換するなど,信号レベルの異なる回路をつなぐ役割をします。
レベルシフタは次のような回路で作成できます。
(使用しているNPN型トランジスタは2SC1815)
レベルシフタの動作をオシロスコープで観察しているところです。
緑が前段回路の3.3V出力(レベルシフタの3.3V入力)で,黄が後段回路の5V入力(レベルシフタの5V出力)です。
HとLの時間的関係は変化せず,Hも時の電圧だけが変化していることがわかります。